5088 lines
188 KiB
C#
5088 lines
188 KiB
C#
using AgroBase.Models.Operadores;
|
||
using AgroBase.Services;
|
||
using AgroBase.Services.Operadores;
|
||
using Newtonsoft.Json;
|
||
using Newtonsoft.Json.Linq;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Linq;
|
||
using static AgroBase.Models.CorredorTrajetoriaModel;
|
||
using static AgroBase.Models.Enums;
|
||
|
||
namespace AgroBase.Models
|
||
{
|
||
public class TrajetoriaMapaOperacaoModel
|
||
{
|
||
public TrajetoriaMapaOperacaoModel(List<List<GPSModel>> RuasMapa)
|
||
{
|
||
RuasPlantacao = new List<List<GPSModel>>(RuasMapa);
|
||
AutonomiaCorredor = new AutonomiaCorredorModel();
|
||
}
|
||
|
||
#region PARAMETROS
|
||
|
||
public double AnguloAberturaCurva { get; set; } = 25; // Angulo usado para deslocar o ponto de curva
|
||
public double DistanciaProjecaoRua { get; set; } = (VariaveisEquipamento.DistanciaEntreEixos / 100.0 / 2.0) + 3.0; // Distancia para projetar o primeiro ponto para fora do corredor
|
||
public static double DistanciaEntrePontos { get; set; } = 0.8; // Distancia entre os pontos dentro do corredor
|
||
public static double DistanciaEntrePontosCurva { get; set; } = 0.25; // Distancia entre os pontos durante a curva entre corredores
|
||
private double DistanciaManobraEntreRuas { get; set; } = 3.0; // Distancia máxima para gerar a curva de conexão entre os corredores
|
||
private bool EspacamentoPrimeirosPontosProjecao { get; set; } = false; // Projetar os primeiros pontos com distancia menor entre eles
|
||
public static double LarguraCorredorPadrao { get; set; } = 1.5; // Largura de um corredor padrão
|
||
private int JanelaAtualizacaoDePontos { get; set; } = 20; // Define o tamanho da janela de pontos a atualizar
|
||
public bool VerificacaoInicialMeioRuaConcluida { get; set; } = false; // Realiza a verificacao inicial para saber se o robo deve comecar a operacao do meio da rua
|
||
private int JanelaBuscaAvancoPontos { get; set; } = 14;
|
||
private double MargemExtraAvancoM { get; set; } = 0.35;
|
||
private double DistanciaConclusaoTrajetoriaM { get; set; } = 3.0;
|
||
private double DistanciaRestanteMaximaParaTrocarCorredorM { get; set; } = 1.0;
|
||
private int PontosRestantesMaximosParaTrocarCorredor { get; set; } = 0;
|
||
private int MaxTentativasRecuperacaoTrajetoria { get; set; } = 3;
|
||
private double ToleranciaLateralRecuperacaoM { get; set; } = 1.5;
|
||
private double ToleranciaLongitudinalAntesPontoM { get; set; } = 0.10;
|
||
private int MaxPontosRecuperadosPorCiclo { get; set; } = 8;
|
||
|
||
#endregion
|
||
|
||
public DateTime UltimaAtualizacaoDados { get; set; } = DateTime.MinValue;
|
||
public bool RetornandoBase { get; set; }
|
||
public AutonomiaCorredorModel AutonomiaCorredor { get; set; }
|
||
|
||
[JsonIgnore]
|
||
private GPSModel GPSPosicaoAtual => Variaveis.OperacaoEmAndamento.Sensoriamento?.Gps?.Clone();
|
||
|
||
[JsonProperty]
|
||
public double TempoEntreLeituras { get; private set; }
|
||
private void AtualizarTempoEntreLeituras()
|
||
{
|
||
DateTime leitura_gps = GPSPosicaoAtual?.Momento ?? DateTime.MinValue;
|
||
double t_min = (1.0 / GPSService.TaxaAmostragemHz);
|
||
double dt = (leitura_gps - UltimaAtualizacaoDados).TotalSeconds;
|
||
TempoEntreLeituras = Math.Min(t_min, dt);
|
||
UltimaAtualizacaoDados = leitura_gps;
|
||
}
|
||
|
||
public List<List<GPSModel>> RuasPlantacao { get; set; }
|
||
public List<List<GPSModel>> Corredores { get; set; }
|
||
public GPSModel PrimeiroPontoRuaMapa
|
||
{
|
||
get
|
||
{
|
||
// Verifica se o índice é válido e se a rua não está vazia
|
||
if (CorredorAtual != null && CorredorAtual.Idx < RuasPlantacao.Count && RuasPlantacao[CorredorAtual.Idx].Count > 0)
|
||
{
|
||
return RuasPlantacao[CorredorAtual.Idx][0]; // Acessa diretamente o primeiro ponto da rua
|
||
}
|
||
else if (CorredorAtual == null && RuasPlantacao.Count > 0)
|
||
{
|
||
return RuasPlantacao[0][0];
|
||
}
|
||
|
||
return new GPSModel(); // Pode ser null se preferir
|
||
}
|
||
}
|
||
public GPSModel UltimoPontoRuaMapa
|
||
{
|
||
get
|
||
{
|
||
// Verifica se o índice é válido e se a rua não está vazia
|
||
if (CorredorAtual != null && CorredorAtual.Idx < RuasPlantacao.Count && RuasPlantacao[CorredorAtual.Idx].Count > 0)
|
||
{
|
||
return RuasPlantacao[CorredorAtual.Idx][RuasPlantacao[CorredorAtual.Idx].Count - 1]; // Acessa diretamente o ultimo ponto da rua
|
||
}
|
||
else if (CorredorAtual == null && RuasPlantacao.Count > 0)
|
||
{
|
||
return RuasPlantacao[0][RuasPlantacao[0].Count - 1];
|
||
}
|
||
|
||
return new GPSModel(); // Pode ser null se preferir
|
||
}
|
||
}
|
||
public int ExtremoMaisProximoMapa
|
||
{
|
||
get
|
||
{
|
||
double distPP = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, PrimeiroPontoRuaMapa);
|
||
double distUP = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, UltimoPontoRuaMapa);
|
||
|
||
// 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
|
||
// 1 para mais próximo do último ponto da rua (SAINDO)
|
||
return (distPP < distUP) ? 0 : 1;
|
||
}
|
||
}
|
||
|
||
public List<PontoTrajetoriaModel> _TrajetoriaFixa { get; set; }
|
||
public bool _TrajetoriaFixaDefinida
|
||
{
|
||
get
|
||
{
|
||
return _TrajetoriaFixa != null && _TrajetoriaFixa.Count > 0;
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public List<PontoTrajetoriaModel> _TrajetoriaJanela { get; private set; }
|
||
private void AtualizarTrajetoriaJanela()
|
||
{
|
||
_TrajetoriaJanela = new List<PontoTrajetoriaModel>();
|
||
|
||
if (!_TrajetoriaFixaDefinida) return;
|
||
|
||
for (int i = idxInicialJanela; i <= idxFinalJanela; i++)
|
||
{
|
||
_TrajetoriaJanela.Add(_TrajetoriaFixa[i]);
|
||
}
|
||
}
|
||
public List<CorredorTrajetoriaModel> _Corredores { get; set; }
|
||
public List<GPSModel> TrajetoriaFixa => _TrajetoriaFixa?.ConvertAll(x => x.Posicao);
|
||
public List<PontoTrajetoriaModel> _TrajetoriaDinamica { get; private set; }
|
||
public List<GPSModel> TrajetoriaDinamica => _TrajetoriaDinamica.ConvertAll(x => x.Posicao);
|
||
public void AtualizarTrajetoriaDinamica()
|
||
{
|
||
// Cria a lista com a última leitura do robô
|
||
var trajetoria = new List<PontoTrajetoriaModel>()
|
||
{
|
||
new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
|
||
{
|
||
Posicao = GPSPosicaoAtual,
|
||
LarguraCorredor = 1.0,
|
||
idxCorredor = CorredorAtual?.Idx ?? 0,
|
||
Visitado = true
|
||
}
|
||
};
|
||
|
||
// Se a trajetória fixa estiver vazia, retorna apenas a leitura atual
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
_TrajetoriaDinamica = trajetoria;
|
||
return;
|
||
}
|
||
|
||
// Adiciona os pontos não visitados, já ordenados por idxPonto
|
||
foreach (var ponto in _TrajetoriaFixa)
|
||
{
|
||
if (!ponto.Visitado)
|
||
{
|
||
trajetoria.Add(ponto);
|
||
}
|
||
}
|
||
|
||
_TrajetoriaDinamica = trajetoria;
|
||
}
|
||
|
||
[JsonProperty]
|
||
public static double DistanciaMaximaEntreLeituras { get; private set; }
|
||
public void AtualizarDistanciaMaximaEntreLeituras()
|
||
{
|
||
double velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP);
|
||
double distanciaMaxima = (velocidadeCarroMs / GPSService.TaxaAmostragemHz);
|
||
DistanciaMaximaEntreLeituras = distanciaMaxima;
|
||
}
|
||
|
||
private int idxInicialJanela = 0;
|
||
private int idxFinalJanela = 0;
|
||
private void AtualizarIndicesJanela(int qtdPontos = -1)
|
||
{
|
||
if (!_TrajetoriaFixaDefinida) return;
|
||
|
||
if (qtdPontos == -1) qtdPontos = JanelaAtualizacaoDePontos;
|
||
|
||
idxInicialJanela = Math.Max(0, (PontoAtual?.idxPonto ?? 0) - qtdPontos / 2);
|
||
idxFinalJanela = Math.Min(_TrajetoriaFixa.Count - 1, (PontoAtual?.idxPonto ?? 0) + qtdPontos / 2);
|
||
}
|
||
|
||
[JsonProperty]
|
||
public CorredorTrajetoriaModel CorredorAtual { get; private set; }
|
||
private bool _CorredorAtualDefinido
|
||
{
|
||
get
|
||
{
|
||
return (CorredorAtual?.Pontos?.Count ?? 0) > 0;
|
||
}
|
||
}
|
||
private void AtualizarCorredorAtual()
|
||
{
|
||
if (_Corredores == null || _Corredores.Count == 0)
|
||
{
|
||
CorredorAtual = null;
|
||
return;
|
||
}
|
||
|
||
int idxNovoCorredor = PontoAtual?.idxCorredor ?? 0;
|
||
|
||
if (idxNovoCorredor < 0 || idxNovoCorredor >= _Corredores.Count)
|
||
{
|
||
return;
|
||
}
|
||
|
||
bool mudouCorredor = (CorredorAtual?.Idx ?? -1) != idxNovoCorredor;
|
||
|
||
if (mudouCorredor)
|
||
{
|
||
/*
|
||
* Fecha os dados acumulados do corredor anterior
|
||
* antes de tentar assumir o próximo.
|
||
*/
|
||
CorredorAtual?.AtualizarDados();
|
||
}
|
||
|
||
/*
|
||
* Sempre chama.
|
||
*
|
||
* Antes da entrada:
|
||
* recalcula a autonomia em tempo real.
|
||
*
|
||
* Depois da confirmação:
|
||
* AutonomiaCorredorModel devolve o snapshot congelado
|
||
* rapidamente, sem recalcular.
|
||
*/
|
||
AtualizarDadosAutonomiaCorredor(idxCorredor: idxNovoCorredor);
|
||
|
||
/*
|
||
* Não assume o novo corredor se a autonomia
|
||
* ainda não autorizou a entrada.
|
||
*/
|
||
if (!AutonomiaCorredor.Liberado)
|
||
return;
|
||
|
||
CorredorAtual = _Corredores[idxNovoCorredor];
|
||
|
||
CorredorAtual.AtualizarDados();
|
||
}
|
||
public DirecaoCarroRua DirecaoCaminho
|
||
{
|
||
get
|
||
{
|
||
return PontoAtual?.Direcao ?? Enums.DirecaoCarroRua.Parado;
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public double AnguloMedioCorredor { get; private set; }
|
||
private void AtualizarAnguloMedioCorredor()
|
||
{
|
||
AnguloMedioCorredor = GPSUtils.CalcularOrientacao(PontoAtual.Posicao, ProximoPonto.Posicao);
|
||
}
|
||
[JsonProperty]
|
||
public double AnguloCaminho { get; private set; }
|
||
private void AtualizarAnguloCaminho()
|
||
{
|
||
PontoTrajetoriaModel pontoComparar = PontoAtual.Aproximando ? PontoAtual : ProximoPonto != null ? ProximoPonto : PontoAtual;
|
||
AnguloCaminho = GPSUtils.CalcularOrientacao(GPSPosicaoAtual, pontoComparar.Posicao);
|
||
}
|
||
[JsonProperty]
|
||
public double DistanciaEsquerda { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaDireita { get; private set; }
|
||
private void AtualizarDistanciasLaterais()
|
||
{
|
||
DistanciaEsquerda = CalcularDistanciaLateral(true, GPSPosicaoAtual, CorredorAtual?.idxRuaEsquerda ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
|
||
DistanciaDireita = CalcularDistanciaLateral(false, GPSPosicaoAtual, CorredorAtual?.idxRuaDireita ?? 0, PontoAtual?.Direcao ?? DirecaoCarroRua.Ida);
|
||
}
|
||
|
||
public double CalcularDistanciaLateral(bool ladoEsquerdo, GPSModel posicaoAtual, int idxRua, DirecaoCarroRua direcaoAtual)
|
||
{
|
||
if (posicaoAtual == null) return 0;
|
||
|
||
// Verifica se o corredor atual existe
|
||
if (!_CorredorAtualDefinido) return 0;
|
||
|
||
List<GPSModel> rua = new List<GPSModel>(RuasPlantacao[idxRua]);
|
||
|
||
double d0 = double.MaxValue;
|
||
int idx = -1;
|
||
for (int i = 0; i < rua.Count; i++)
|
||
{
|
||
double dist = GPSUtils.DistanciaEntrePontos(posicaoAtual, rua[i]);
|
||
if (dist < d0)
|
||
{
|
||
d0 = dist;
|
||
idx = i;
|
||
}
|
||
else
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
|
||
GPSModel p0 = rua[idx - (idx > 0 ? 1 : 0)];
|
||
GPSModel p1 = rua[idx + (idx < (rua.Count - 1) ? 1 : 0)];
|
||
int idxPa = rua.IndexOf(p0);
|
||
int idxPb = rua.IndexOf(p1);
|
||
double anguloAprojetar = GPSUtils.CalcularOrientacao(rua[idxPb], rua[idxPa]);
|
||
double anguloBprojetar = GPSUtils.CalcularOrientacao(rua[idxPa], rua[idxPb]);
|
||
|
||
GPSModel pontoA = GPSUtils.GerarPontoDeslocado(rua[idxPa], anguloAprojetar, DistanciaManobraEntreRuas);
|
||
GPSModel pontoB = GPSUtils.GerarPontoDeslocado(rua[idxPb], anguloBprojetar, DistanciaManobraEntreRuas);
|
||
|
||
List<GPSModel> novaRua = new List<GPSModel>();
|
||
novaRua.Add(pontoA);
|
||
for (int i = idxPa; i <= idxPb; i++)
|
||
{
|
||
novaRua.Add(rua[i]);
|
||
}
|
||
novaRua.Add(pontoB);
|
||
|
||
rua = novaRua;
|
||
|
||
int qtdPontosRua = Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua));
|
||
rua = GPSUtils.InterpolarRota(rua, qtdPontosRua);
|
||
|
||
// Calcula a menor distância entre o robô e a rua
|
||
double distancia = GPSUtils.CalcularMenorDistanciaAteTrecho(posicaoAtual, rua);
|
||
|
||
// Calcula o vetor entre os pontos da rua e o vetor entre o ponto A e o GPS
|
||
double crossProduct =
|
||
(pontoB.Longitude - pontoA.Longitude) * (posicaoAtual.Latitude - pontoA.Latitude) -
|
||
(pontoB.Latitude - pontoA.Latitude) * (posicaoAtual.Longitude - pontoA.Longitude);
|
||
|
||
// Ajusta a distância com base no sentido de deslocamento do robô
|
||
if ((direcaoAtual == DirecaoCarroRua.Volta && ladoEsquerdo) || (direcaoAtual == DirecaoCarroRua.Ida && !ladoEsquerdo))
|
||
{
|
||
distancia *= crossProduct > 0 ? 1 : -1;
|
||
}
|
||
else
|
||
{
|
||
distancia *= crossProduct > 0 ? -1 : 1;
|
||
}
|
||
|
||
// Ajusta a largura do robô no cálculo final
|
||
double larguraAjuste = ladoEsquerdo ? VariaveisEquipamento.LarguraEsquerda : VariaveisEquipamento.LarguraDireita;
|
||
distancia -= (larguraAjuste / 100.0);
|
||
|
||
return distancia;
|
||
}
|
||
[JsonProperty]
|
||
public PontoTrajetoriaModel PontoAtual { get; private set; }
|
||
private void DefinirPontoAtual()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
PontoAtual = null;
|
||
return;
|
||
}
|
||
|
||
int idxUltimoVisitado = -1;
|
||
|
||
for (int i = _TrajetoriaFixa.Count - 1; i >= 0; i--)
|
||
{
|
||
if (_TrajetoriaFixa[i].Visitado)
|
||
{
|
||
idxUltimoVisitado = i;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (idxUltimoVisitado >= 0)
|
||
{
|
||
PontoAtual = _TrajetoriaFixa[idxUltimoVisitado];
|
||
return;
|
||
}
|
||
|
||
PontoAtual = _TrajetoriaFixa[0];
|
||
PontoAtual.Visitado = true;
|
||
}
|
||
[JsonProperty]
|
||
public PontoTrajetoriaModel ProximoPonto { get; private set; }
|
||
private void DefinirProximoPonto()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
ProximoPonto = null;
|
||
return;
|
||
}
|
||
|
||
if (PontoAtual == null)
|
||
{
|
||
DefinirPontoAtual();
|
||
}
|
||
|
||
if (PontoAtual == null)
|
||
{
|
||
ProximoPonto = null;
|
||
return;
|
||
}
|
||
|
||
int idxAtual = Math.Max(0, PontoAtual.idxPonto);
|
||
|
||
for (int i = idxAtual + 1; i < _TrajetoriaFixa.Count; i++)
|
||
{
|
||
if (!_TrajetoriaFixa[i].Visitado)
|
||
{
|
||
ProximoPonto = _TrajetoriaFixa[i];
|
||
return;
|
||
}
|
||
}
|
||
|
||
ProximoPonto = PontoAtual;
|
||
}
|
||
[JsonProperty]
|
||
public PontoTrajetoriaModel PontoMaisProximo { get; private set; }
|
||
private void DefinirPontoMaisProximo()
|
||
{
|
||
// ✅ Evita processamento desnecessário se a lista for nula ou vazia
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
PontoMaisProximo = null;
|
||
return;
|
||
}
|
||
|
||
double menorDistancia = double.MaxValue;
|
||
PontoTrajetoriaModel melhorPonto = null;
|
||
|
||
// ✅ Busca apenas na janela otimizada
|
||
for (int i = idxInicialJanela; i <= idxFinalJanela && i < _TrajetoriaFixa.Count; i++)
|
||
{
|
||
if (Math.Abs(_TrajetoriaFixa[i].DistanciaTrajeto) < menorDistancia)
|
||
{
|
||
melhorPonto = _TrajetoriaFixa[i];
|
||
menorDistancia = Math.Abs(melhorPonto.DistanciaTrajeto);
|
||
}
|
||
}
|
||
|
||
// ✅ Só atualiza se encontrou um ponto
|
||
if (melhorPonto != null)
|
||
{
|
||
PontoMaisProximo = melhorPonto;
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public bool NaMargemDoCorredor { get; private set; }
|
||
private void AtualizarNaMargemDoCorredor()
|
||
{
|
||
NaMargemDoCorredor = false;
|
||
foreach (var ponto in _TrajetoriaJanela)
|
||
{
|
||
if (ponto.NaMargem && ponto.PontoBorda)
|
||
{
|
||
NaMargemDoCorredor = true; // Paramos a busca assim que encontramos um ponto válido
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public bool ManobrandoEntreRuas { get; private set; }
|
||
private void AtualizarManobrandoEntreRuas()
|
||
{
|
||
//bool proximoAentrada = (ProximoPonto.PontoBorda || ProximoPonto.PontoLigacao) && ProximoPonto.DistanciaAtual < DistanciaManobraEntreRuas;
|
||
//ManobrandoEntreRuas = (proximoAentrada && StatusAtual == StatusCarroMapa.Direcionando) || NaMargemDoCorredor;
|
||
|
||
ManobrandoEntreRuas = StatusAtual == StatusCarroMapa.Manobrando;
|
||
}
|
||
[JsonProperty]
|
||
public StatusCarroMapa StatusAtual { get; private set; }
|
||
private void AtualizarStatusAtual()
|
||
{
|
||
StatusAtual = StatusCarroMapa.Parado;
|
||
if (ProximoPonto != null)
|
||
{
|
||
if (CorredorAtual != null)
|
||
{
|
||
if ((RetornandoBase && CorredorAtual.Dentro) || (!RetornandoBase && Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual == StatusOperacao.EmAndamento))
|
||
{
|
||
PontoAtual.AtualizarPropriedades();
|
||
if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas)
|
||
{
|
||
StatusAtual = StatusCarroMapa.Direcionando;
|
||
}
|
||
else if (!CorredorAtual.Dentro && (PontoAtual.PontoBorda || PontoAtual.PontoLigacao || PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores || PontoAtual.Tipo == TipoPontoRua.Desvio || ProximoPonto.Tipo == TipoPontoRua.LigacaoEntrada))
|
||
{
|
||
StatusAtual = StatusCarroMapa.Manobrando;
|
||
}
|
||
else if (!CorredorAtual.Dentro && PontoAtual.Tipo == TipoPontoRua.Rua && ProximoPonto.Tipo == TipoPontoRua.Rua && ProximoPonto.OrientacaoAtual > 7.0)
|
||
{
|
||
StatusAtual = StatusCarroMapa.Manobrando;
|
||
}
|
||
else if (CorredorAtual.Dentro && !NaMargemDoCorredor)
|
||
{
|
||
StatusAtual = StatusCarroMapa.CaminhandoRua;
|
||
}
|
||
else if (_TrajetoriaJanela.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaEntrada))
|
||
{
|
||
StatusAtual = StatusCarroMapa.EntrandoRua;
|
||
}
|
||
else if (_TrajetoriaJanela.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaSaida))
|
||
{
|
||
StatusAtual = StatusCarroMapa.SaindoRua;
|
||
}
|
||
else if (!CorredorAtual.Dentro)
|
||
{
|
||
StatusAtual = StatusCarroMapa.Direcionando;
|
||
}
|
||
}
|
||
else if (RetornandoBase)
|
||
{
|
||
StatusAtual = StatusCarroMapa.RetornandoBase;
|
||
}
|
||
}
|
||
else if (RetornandoBase)
|
||
{
|
||
StatusAtual = StatusCarroMapa.RetornandoBase;
|
||
}
|
||
}
|
||
}
|
||
[JsonProperty]
|
||
public double DistanciaTotal { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaPercorrida { get; private set; }
|
||
public void AtualizarDistanciaPercorrida(double distancia)
|
||
{
|
||
DistanciaPercorrida += distancia;
|
||
CorredorAtual?.AtualizarDistancias(distancia);
|
||
}
|
||
[JsonProperty]
|
||
public double DistanciaRestante { get; private set; }
|
||
private void AtualizarDistanciaRestante()
|
||
{
|
||
var _trajetoria = (_TrajetoriaDinamica ?? new List<PontoTrajetoriaModel>()).Select(x => x.Posicao).ToList();
|
||
DistanciaRestante = GPSUtils.DistanciaDoTrecho(_trajetoria);
|
||
}
|
||
[JsonProperty]
|
||
public double PercentualTrajetoria { get; private set; }
|
||
public void AtualizarPercentualTrajetoria()
|
||
{
|
||
PercentualTrajetoria = DistanciaTotal > 0 ? (DistanciaPercorrida / DistanciaTotal) * 100.0 : 0;
|
||
}
|
||
[JsonProperty]
|
||
public string TempoEstimadoOperacao { get; private set; }
|
||
[JsonProperty]
|
||
public string TempoEstimadoRestante { get; private set; }
|
||
public void AtualizarTempoEstimado(double? velocidadeSemErvasMs = null, double? velocidadeComErvasMs = null)
|
||
{
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
|
||
if (!velocidadeSemErvasMs.HasValue)
|
||
{
|
||
velocidadeSemErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(op.Parametros.Controle.MovVelocidadeSErvasPercent) * 0.8;
|
||
}
|
||
if (!velocidadeComErvasMs.HasValue)
|
||
{
|
||
velocidadeComErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(op.Parametros.Controle.MovVelocidadeCErvasPercent);
|
||
}
|
||
|
||
// Supondo que metade do percurso tem ervas e a outra metade não tem
|
||
double distanciaComErvas = DistanciaTotal * op.Sensoriamento.Atuador.PercentualErvasTerreno;
|
||
double distanciaSemErvas = DistanciaTotal * (100 - op.Sensoriamento.Atuador.PercentualErvasTerreno);
|
||
|
||
// Tempo em segundos para cada parte do percurso
|
||
double tempoSemErvas = velocidadeSemErvasMs.Value > 0 ? distanciaSemErvas / velocidadeSemErvasMs.Value : 0;
|
||
double tempoComErvas = velocidadeComErvasMs.Value > 0 ? distanciaComErvas / velocidadeComErvasMs.Value : 0;
|
||
|
||
// Tempo total em segundos
|
||
double tempoTotalSegundos = tempoSemErvas + tempoComErvas;
|
||
|
||
// Conversão para horas, minutos e segundos
|
||
TimeSpan tempoTotal = TimeSpan.FromSeconds(tempoTotalSegundos);
|
||
DateTime total = new DateTime(tempoTotal.Ticks);
|
||
|
||
TempoEstimadoOperacao = total.ToString("HH:mm:ss");
|
||
|
||
// Distâncias (em metros)
|
||
double distanciaRestante = DistanciaRestante;
|
||
|
||
// Calcular a distância restante sem ervas e com ervas
|
||
double distanciaSemErvasRestante = distanciaRestante * (100 - op.Sensoriamento.Atuador.PercentualErvasTerreno);
|
||
double distanciaComErvasRestante = distanciaRestante * op.Sensoriamento.Atuador.PercentualErvasTerreno;
|
||
|
||
// Calcular o tempo restante para as áreas sem ervas e com ervas
|
||
double tempoRestanteSemErvas = distanciaSemErvasRestante / velocidadeSemErvasMs.Value;
|
||
double tempoRestanteComErvas = distanciaComErvasRestante / velocidadeComErvasMs.Value;
|
||
|
||
// Tempo total restante em segundos
|
||
double tempoTotalRestanteSegundos = tempoRestanteSemErvas + tempoRestanteComErvas;
|
||
|
||
// Conversão para horas, minutos e segundos
|
||
TimeSpan tempoTotalRestante = TimeSpan.FromSeconds(tempoTotalRestanteSegundos);
|
||
DateTime totalRestante = new DateTime(tempoTotalRestante.Ticks);
|
||
|
||
TempoEstimadoRestante = totalRestante.ToString("HH:mm:ss");
|
||
}
|
||
[JsonProperty]
|
||
public double DistanciaErroMapaPlantacao { get; private set; } = -1;
|
||
public GPSModel PrimeiroPontoDentroMapa = null;
|
||
public GPSModel PrimeiroPontoDentroCana = null;
|
||
public void AtualizarDistanciaErroMapaPlantacao()
|
||
{
|
||
if (DistanciaErroMapaPlantacao > -1) return;
|
||
if (CorredorAtual == null) return;
|
||
if (CorredorAtual.Idx > 0) return;
|
||
if (!CorredorAtual.Dentro) return;
|
||
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
|
||
if (PrimeiroPontoDentroMapa == null) PrimeiroPontoDentroMapa = GPSPosicaoAtual;
|
||
|
||
var OpVisual = op.Sensoriamento?.OperadorVisual ?? new VisualWorkerModel();
|
||
StatusModulo OpVisualStatus = op.Sensoriamento.OperadorSaude.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado;
|
||
|
||
if (OpVisualStatus != StatusModulo.Operante) return;
|
||
|
||
var DadosSegmentacao = OpVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel();
|
||
bool CanaNoRadar = new List<StatusCarroMapa>() { StatusCarroMapa.EntrandoRua, StatusCarroMapa.CaminhandoRua, StatusCarroMapa.SaindoRua }.Contains(DadosSegmentacao.status_corredor);
|
||
if (!CanaNoRadar) return;
|
||
|
||
PrimeiroPontoDentroCana = GPSPosicaoAtual;
|
||
|
||
DistanciaErroMapaPlantacao = GPSUtils.DistanciaEntrePontos(PrimeiroPontoDentroMapa, PrimeiroPontoDentroCana);
|
||
}
|
||
|
||
[JsonProperty]
|
||
public double ErroOrientacaoAngular { get; private set; }
|
||
[JsonProperty]
|
||
public double ErroOrientacaoAngularCombinado { get; private set; }
|
||
[JsonProperty]
|
||
public double ErroOrientacaoAngularCaminho { get; private set; }
|
||
[JsonProperty]
|
||
public double ErroOrientacaoAngularProximoPonto { get; private set; }
|
||
[JsonProperty]
|
||
public double ErroLateralAngular { get; private set; }
|
||
[JsonProperty]
|
||
public bool TrajeotiraConcluida { get; private set; }
|
||
private void AtualizarTrajetoriaConcluida()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
{
|
||
TrajeotiraConcluida = false;
|
||
return;
|
||
}
|
||
|
||
TrajeotiraConcluida = _TrajetoriaFixa.All(x => x.Visitado);
|
||
}
|
||
|
||
private bool TentarConcluirTrajetoriaPorFimFisico()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
return false;
|
||
|
||
var ultimo = _TrajetoriaFixa.LastOrDefault();
|
||
if (ultimo == null)
|
||
return false;
|
||
|
||
ultimo.AtualizarPropriedades();
|
||
|
||
int idxUltimoVisitado = ObterIdxUltimoVisitado();
|
||
bool pertoDoFimPorIndice = idxUltimoVisitado >= _TrajetoriaFixa.Count - 3;
|
||
|
||
bool pertoDoUltimoPonto =
|
||
ultimo.DistanciaAtual <= DistanciaConclusaoTrajetoriaM;
|
||
|
||
bool passouDoUltimoPonto =
|
||
!ultimo.Aproximando &&
|
||
ultimo.DistanciaAnterior <= DistanciaConclusaoTrajetoriaM &&
|
||
ultimo.DistanciaAtual <= DistanciaConclusaoTrajetoriaM + 2.0;
|
||
|
||
if (pertoDoFimPorIndice && (pertoDoUltimoPonto || passouDoUltimoPonto))
|
||
{
|
||
MarcarVisitadoAte(_TrajetoriaFixa.Count - 1);
|
||
|
||
Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(
|
||
T_Code.Trj,
|
||
StatusModulo.Operante,
|
||
100,
|
||
$"[TRJ] Trajetória concluída por fim físico. dist_ultimo={ultimo.DistanciaAtual:F2}m"
|
||
);
|
||
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private void AtualizarErroCombinado()
|
||
{
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
|
||
var _Sensoriamento = op.Sensoriamento;
|
||
|
||
//double anguloCamera = _Sensoriamento.OperadorVisual.Leitura.obj?.segmentacao?.angulo_rua ?? 0;
|
||
double anguloCorredor = AnguloMedioCorredor;
|
||
double anguloProximoPonto = AnguloCaminho;
|
||
double anguloCarro = _Sensoriamento.Gps?.AnguloCarroDefinido ?? 0;
|
||
|
||
// Erro 1: Direção para onde o robô deve ir (centro do corredor)
|
||
double erroIrParaCentro = GPSUtils.CalcularDiferencaAngulo(anguloCarro, anguloProximoPonto);
|
||
|
||
// Erro 2: Quanto o robô está desalinhado da orientação da rua
|
||
double desalinhamentoComRua = GPSUtils.CalcularDiferencaAngulo(anguloCarro, anguloCorredor);
|
||
|
||
// Erro 3 (secundário): Diferença entre o ângulo do corredor e o ponto (curva futura)
|
||
double curvaDaRua = GPSUtils.CalcularDiferencaAngulo(anguloProximoPonto, anguloCorredor);
|
||
|
||
// Peso do desalinhamento atual (prioriza alinhar o robô ao corredor)
|
||
double pesoAlinhamento = Math.Min(Math.Abs(desalinhamentoComRua) / op.Parametros.Controle.DirAnguloMaximo, 1.0); // Normaliza até 30°
|
||
|
||
// Peso do centro (prioriza ir pro ponto central do corredor)
|
||
double pesoCentro = 1.0 - pesoAlinhamento;
|
||
|
||
// Peso extra para curvas mais fechadas
|
||
double boostCurva = 1.0 + (Math.Abs(curvaDaRua) / 45.0); // aumenta peso em curvas
|
||
|
||
// Calcule o erro lateral baseado nas distâncias entre as ruas do mapa atraves do GPS
|
||
double erroLateral = !(CorredorAtual?.Dentro ?? false) ? 0 : (DistanciaEsquerda - DistanciaDireita);
|
||
double distanciaProximoPonto = ProximoPonto.DistanciaAtual;
|
||
double erroLateralGraus = Math.Atan2(erroLateral, distanciaProximoPonto) * (180.0 / Math.PI);
|
||
|
||
// Calcule o erro do ultrassom
|
||
double erroUltrassom = UltrasonicA05Helper.CalcularErroUltrassons();
|
||
|
||
// Combina os erros com pesos dinâmicos
|
||
double erroCombinado = (erroIrParaCentro * pesoCentro) + (desalinhamentoComRua * pesoAlinhamento);
|
||
|
||
|
||
ErroOrientacaoAngularProximoPonto = erroIrParaCentro;
|
||
ErroOrientacaoAngularCaminho = desalinhamentoComRua;
|
||
ErroOrientacaoAngular = curvaDaRua;
|
||
ErroOrientacaoAngularCombinado = erroCombinado * boostCurva;
|
||
ErroLateralAngular = erroLateralGraus;
|
||
}
|
||
|
||
private void MarcarPontosIntermediariosNaoVisitados()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida || PontoAtual == null)
|
||
return;
|
||
|
||
int idxFinal = Math.Max(0, Math.Min(PontoAtual.idxPonto, _TrajetoriaFixa.Count - 1));
|
||
|
||
for (int i = 0; i <= idxFinal; i++)
|
||
{
|
||
if (!_TrajetoriaFixa[i].Visitado)
|
||
_TrajetoriaFixa[i].Visitado = true;
|
||
}
|
||
}
|
||
|
||
private void AtualizarPropriedadesPontos(int idxInicial, int idxFinal)
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
return;
|
||
|
||
idxInicial = Math.Max(0, idxInicial);
|
||
idxFinal = Math.Min(_TrajetoriaFixa.Count - 1, idxFinal);
|
||
|
||
for (int i = idxInicial; i <= idxFinal; i++)
|
||
{
|
||
_TrajetoriaFixa[i].AtualizarPropriedades();
|
||
}
|
||
}
|
||
|
||
private void MarcarVisitadoAte(int idx)
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
return;
|
||
|
||
idx = Math.Max(0, Math.Min(idx, _TrajetoriaFixa.Count - 1));
|
||
|
||
for (int i = 0; i <= idx; i++)
|
||
{
|
||
_TrajetoriaFixa[i].Visitado = true;
|
||
}
|
||
}
|
||
|
||
private int ObterIdxUltimoVisitado()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
return -1;
|
||
|
||
for (int i = _TrajetoriaFixa.Count - 1; i >= 0; i--)
|
||
{
|
||
if (_TrajetoriaFixa[i].Visitado)
|
||
return i;
|
||
}
|
||
|
||
return -1;
|
||
}
|
||
|
||
private void ConfirmarEntradaAutonomiaCorredor()
|
||
{
|
||
if (RetornandoBase)
|
||
return;
|
||
|
||
if (CorredorAtual == null)
|
||
return;
|
||
|
||
if (AutonomiaCorredor == null)
|
||
return;
|
||
|
||
if (!AutonomiaCorredor.Iniciado)
|
||
return;
|
||
|
||
if (!AutonomiaCorredor.Liberado)
|
||
return;
|
||
|
||
/*
|
||
* Proteção importante:
|
||
*
|
||
* PontoAtual e CorredorAtual precisam representar
|
||
* o mesmo corredor. Isso evita confirmar o corredor
|
||
* antigo durante uma transição bloqueada.
|
||
*/
|
||
if (PontoAtual == null || PontoAtual.idxCorredor != CorredorAtual.Idx)
|
||
{
|
||
return;
|
||
}
|
||
|
||
/*
|
||
* EntrandoRua:
|
||
* confirmação normal na borda de entrada.
|
||
*
|
||
* CaminhandoRua:
|
||
* fallback para início no meio da rua,
|
||
* recuperação de trajetória ou salto de estado.
|
||
*/
|
||
bool entradaFisicaConfirmada =
|
||
StatusAtual == StatusCarroMapa.EntrandoRua ||
|
||
StatusAtual == StatusCarroMapa.CaminhandoRua;
|
||
|
||
if (!entradaFisicaConfirmada)
|
||
return;
|
||
|
||
int idxCorredor = CorredorAtual.Idx;
|
||
|
||
if (AutonomiaCorredor.CorredorEstaConfirmado(idxCorredor))
|
||
{
|
||
return;
|
||
}
|
||
|
||
bool confirmou = AutonomiaCorredor.ConfirmarEntradaCorredor(idxCorredor);
|
||
|
||
if (!confirmou)
|
||
return;
|
||
|
||
Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(
|
||
T_Code.Trj,
|
||
StatusModulo.Operante,
|
||
100,
|
||
"[AUTONOMIA] Entrada no corredor confirmada. " +
|
||
$"corredor={idxCorredor + 1}, " +
|
||
$"status={StatusAtual}, " +
|
||
$"bat_segura={AutonomiaCorredor.DistanciaSeguraBateria_m:F1}m, " +
|
||
$"herb_seguro={AutonomiaCorredor.DistanciaSeguraHerbicida_m:F1}m, " +
|
||
$"bat_folga={AutonomiaCorredor.FolgaBateria_m:F1}m, " +
|
||
$"herb_folga={AutonomiaCorredor.FolgaHerbicida_m:F1}m"
|
||
);
|
||
}
|
||
|
||
|
||
private void AtualizarDadosTrajetoria()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
return;
|
||
|
||
AtualizarTempoEntreLeituras();
|
||
AtualizarDistanciaMaximaEntreLeituras();
|
||
|
||
for (int tentativa = 0; tentativa < MaxTentativasRecuperacaoTrajetoria; tentativa++)
|
||
{
|
||
DefinirPontoAtual();
|
||
|
||
if (PontoAtual == null)
|
||
return;
|
||
|
||
AtualizarCorredorAtual();
|
||
DefinirProximoPonto();
|
||
|
||
AtualizarIndicesJanela();
|
||
AtualizarTrajetoriaJanela();
|
||
AtualizarPropriedadesPontos(idxInicialJanela, idxFinalJanela);
|
||
|
||
VerificaFimCorredorSegmentacao();
|
||
|
||
if (VerificaInicioOperacaoMeioRua(true))
|
||
continue;
|
||
|
||
MarcarPontosIntermediariosNaoVisitados();
|
||
|
||
//bool recuperouAvanco = RecuperarAvancoPorPontoFuturo();
|
||
bool recuperouAvanco = RecuperarPontosDeixadosParaTras();
|
||
|
||
DefinirPontoAtual();
|
||
AtualizarCorredorAtual();
|
||
DefinirProximoPonto();
|
||
|
||
bool recuperouTransicao = RecuperarTransicaoEntreCorredores();
|
||
|
||
DefinirPontoAtual();
|
||
AtualizarCorredorAtual();
|
||
DefinirProximoPonto();
|
||
|
||
/*
|
||
* Uma troca de corredor encerra as recuperações deste ciclo.
|
||
*
|
||
* Sem isso, o próximo giro do for pode executar
|
||
* RecuperarAvancoPorPontoFuturo() já no corredor novo
|
||
* e confirmar mais pontos imediatamente.
|
||
*/
|
||
if (recuperouTransicao)
|
||
{
|
||
break;
|
||
}
|
||
|
||
if (!recuperouAvanco)
|
||
{
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (PontoAtual == null)
|
||
return;
|
||
|
||
DefinirProximoPonto();
|
||
|
||
if (ProximoPonto == null)
|
||
return;
|
||
|
||
AtualizarIndicesJanela();
|
||
AtualizarTrajetoriaJanela();
|
||
AtualizarPropriedadesPontos(idxInicialJanela, idxFinalJanela);
|
||
|
||
MarcarPontosIntermediariosNaoVisitados();
|
||
|
||
TentarConcluirTrajetoriaPorFimFisico();
|
||
|
||
DefinirPontoAtual();
|
||
AtualizarCorredorAtual();
|
||
DefinirProximoPonto();
|
||
|
||
if (ProximoPonto == null)
|
||
return;
|
||
|
||
DefinirPontoMaisProximo();
|
||
AtualizarAnguloMedioCorredor();
|
||
AtualizarAnguloCaminho();
|
||
AtualizarDistanciasLaterais();
|
||
AtualizarNaMargemDoCorredor();
|
||
AtualizarStatusAtual();
|
||
ConfirmarEntradaAutonomiaCorredor();
|
||
AtualizarManobrandoEntreRuas();
|
||
AtualizarPercentualTrajetoria();
|
||
AtualizarTempoEstimado();
|
||
|
||
AtualizarErroCombinado();
|
||
|
||
AtualizarTrajetoriaDinamica();
|
||
AtualizarDistanciaRestante();
|
||
AtualizarTrajetoriaConcluida();
|
||
|
||
AtualizarDistanciaErroMapaPlantacao();
|
||
}
|
||
|
||
|
||
private int VerificaEquipamentoDentroCorredor(GPSModel posicaoAtual)
|
||
{
|
||
Dictionary<int, (bool, double)> melhoresPontosCorredores = new Dictionary<int, (bool, double)>();
|
||
foreach (var corredor in Corredores)
|
||
{
|
||
(GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(posicaoAtual, corredor);
|
||
if (pontoMaisProximo == null)
|
||
{
|
||
return -1;
|
||
}
|
||
|
||
double distanciaMargem = LarguraCorredorPadrao * 0.8;
|
||
bool naMargem = distanciaAtual < distanciaMargem;
|
||
|
||
melhoresPontosCorredores.Add(melhoresPontosCorredores.Count, (naMargem, distanciaAtual));
|
||
}
|
||
return melhoresPontosCorredores.Any(x => x.Value.Item1) ? melhoresPontosCorredores.Where(x => x.Value.Item1).OrderBy(x => x.Value.Item2).FirstOrDefault().Key : -1;
|
||
}
|
||
|
||
private (bool, int) VerificaEquipamentoDentroCorredorMontado(GPSModel posicaoAtual)
|
||
{
|
||
if (Corredores == null)
|
||
return (false, -1);
|
||
|
||
List<PontoTrajetoriaModel> melhoresPontosCorredores = new List<PontoTrajetoriaModel>();
|
||
foreach (var corredor in Corredores)
|
||
{
|
||
(GPSModel pontoMaisProximo, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(posicaoAtual, corredor);
|
||
if (pontoMaisProximo == null) continue;
|
||
|
||
var melhorPonto = _Corredores[Corredores.IndexOf(corredor)].Pontos.FirstOrDefault(x => x.Posicao.Latitude == pontoMaisProximo.Latitude && x.Posicao.Longitude == pontoMaisProximo.Longitude);
|
||
melhorPonto.AtualizarPropriedades();
|
||
melhoresPontosCorredores.Add(melhorPonto);
|
||
}
|
||
var ponto = melhoresPontosCorredores.Where(x => x.NaMargem).OrderBy(x => x.DistanciaAtual).FirstOrDefault();
|
||
bool pontoNaMargem = ponto != null;
|
||
int idxPonto = !pontoNaMargem ? -1 : ponto.idxPonto;
|
||
return (pontoNaMargem, idxPonto);
|
||
}
|
||
|
||
private bool VerificaInicioOperacaoMeioRua(bool considerarStatus)
|
||
{
|
||
if (_TrajetoriaFixaDefinida && !VerificacaoInicialMeioRuaConcluida && ((considerarStatus && new List<StatusOperacao>() { StatusOperacao.Aguardando, StatusOperacao.EmAndamento }.Contains(Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual)) || !considerarStatus))
|
||
{
|
||
VerificacaoInicialMeioRuaConcluida = true;
|
||
(bool noCoredor, var idxPontoMaisProximo) = VerificaEquipamentoDentroCorredorMontado(_TrajetoriaFixa[0].Posicao);
|
||
if (noCoredor)
|
||
{
|
||
for (int i = 0; i <= idxPontoMaisProximo; i++)
|
||
{
|
||
_TrajetoriaFixa[i].Visitado = true;
|
||
}
|
||
|
||
CorredorAtual.AtualizarDados();
|
||
|
||
return true;
|
||
}
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
|
||
private void VerificaFimCorredorSegmentacao()
|
||
{
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
// Limiar de "margem" para considerar proximidade do fim
|
||
//double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 10.0;
|
||
double limiarDistanciaMargem = op.Parametros?.Controle?.AnteciparManobraCorredorM ?? 0;
|
||
if (limiarDistanciaMargem <= 0) return;
|
||
|
||
if (CorredorAtual == null) return;
|
||
if (CorredorAtual.DadosVisuais.JaAntecipado) return;
|
||
|
||
bool modoSimulador = false && op.Simulando;
|
||
|
||
double distanciaCorredor = CorredorAtual.DistanciaTotal;
|
||
double distanciaRestanteCorredor = CorredorAtual.DistanciaRestante;
|
||
double distanciaUltimoPontoCorredor = GPSUtils.DistanciaEntrePontos(GPSPosicaoAtual, CorredorAtual.Pontos.LastOrDefault().Posicao);
|
||
|
||
var opVisual = op.Sensoriamento?.OperadorVisual ?? new VisualWorkerModel();
|
||
StatusModulo opVisualStatus = op.Sensoriamento.OperadorSaude.ModulosSaude.FirstOrDefault(x => x.modulo == T_Code.Snr)?.status ?? StatusModulo.Desconectado;
|
||
var dadosSegmentacao = opVisual.Analises?.segmentacao ?? new VisualWorkerMessageSegmentacaoSemanticaModel();
|
||
|
||
// Se não tem visual operante, não está no modoSimulador ou o corredor eh curto demais, não faz nada
|
||
if (!(opVisualStatus == StatusModulo.Operante || modoSimulador || distanciaCorredor <= limiarDistanciaMargem))
|
||
return;
|
||
|
||
// Garante estrutura de dados visuais do corredor
|
||
var dv = CorredorAtual.DadosVisuais ?? (CorredorAtual.DadosVisuais = new DadosVisuaisCorredor());
|
||
|
||
StatusCarroMapa statusAtual = dadosSegmentacao.status_corredor;
|
||
StatusCarroMapa statusAnterior = dadosSegmentacao.status_corredor_anterior;
|
||
var agora = DateTime.Now;
|
||
|
||
// 1) Atualiza tempo e distância por status (usando o status anterior)
|
||
if (dv.UltimoUpdate > DateTime.MinValue)
|
||
{
|
||
double dt = (agora - dv.UltimoUpdate).TotalSeconds;
|
||
if (dt < 0) dt = 0;
|
||
|
||
// Aproximação da distância percorrida nesse intervalo pelo mapa
|
||
double deltaDist = 0;
|
||
if (dv.DistanciaRestanteAnterior >= 0)
|
||
{
|
||
deltaDist = dv.DistanciaRestanteAnterior - distanciaRestanteCorredor;
|
||
if (deltaDist < 0) deltaDist = 0; // proteção contra ruído
|
||
}
|
||
|
||
bool emMovimento = statusAnterior != StatusCarroMapa.Parado;
|
||
|
||
if (emMovimento)
|
||
{
|
||
switch (statusAnterior)
|
||
{
|
||
case StatusCarroMapa.CaminhandoRua:
|
||
case StatusCarroMapa.EntrandoRua:
|
||
case StatusCarroMapa.SaindoRua:
|
||
dv.TempoDentroRuaTotal += dt;
|
||
dv.DistanciaDentroRuaTotal += deltaDist;
|
||
dv.TempoForaRuaContinuo = 0;
|
||
dv.DistanciaForaRuaContinuo = 0;
|
||
break;
|
||
|
||
case StatusCarroMapa.Direcionando:
|
||
dv.TempoForaRuaTotal += dt;
|
||
dv.DistanciaForaRuaTotal += deltaDist;
|
||
dv.TempoForaRuaContinuo += dt;
|
||
dv.DistanciaForaRuaContinuo += deltaDist;
|
||
break;
|
||
|
||
default:
|
||
// Outros estados quebram a continuidade "fora do corredor"
|
||
dv.TempoForaRuaContinuo = 0;
|
||
dv.DistanciaForaRuaContinuo = 0;
|
||
break;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Parado: não acumula movimento, mas quebra janela contínua
|
||
dv.TempoForaRuaContinuo = 0;
|
||
dv.DistanciaForaRuaContinuo = 0;
|
||
}
|
||
}
|
||
|
||
dv.UltimoUpdate = agora;
|
||
dv.DistanciaRestanteAnterior = distanciaRestanteCorredor;
|
||
|
||
// 2) Confirmar se este corredor realmente existiu (teve cana)
|
||
if (!dv.CorredorConfirmado)
|
||
{
|
||
const double MIN_DIST_CORREDOR_CONFIRMADO = 5.0; // metros dentro da rua
|
||
const double MIN_TEMPO_CORREDOR_CONFIRMADO = 10.0; // segundos dentro da rua
|
||
if (dv.DistanciaDentroRuaTotal >= MIN_DIST_CORREDOR_CONFIRMADO || dv.TempoDentroRuaTotal >= MIN_TEMPO_CORREDOR_CONFIRMADO)
|
||
{
|
||
dv.CorredorConfirmado = true;
|
||
}
|
||
}
|
||
|
||
// 3) Critérios para considerar que chegou ao fim do corredor antes do mapa acabar
|
||
|
||
double tsAgora = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() / 1000.0;
|
||
bool dadoFresco = (tsAgora - dadosSegmentacao.timestamp) < 2.0;
|
||
|
||
bool pertoDoFim = distanciaRestanteCorredor < limiarDistanciaMargem && distanciaUltimoPontoCorredor < limiarDistanciaMargem;
|
||
|
||
bool foraDoCorredorAgora = (statusAtual == StatusCarroMapa.Direcionando);
|
||
|
||
// Limiar de "quanto tempo/distância já estou fora do corredor" para evitar buracos pontuais
|
||
const double MIN_TEMPO_FORA_CORREDOR = 1.0; // segundos
|
||
const double MIN_DIST_FORA_CORREDOR = 1.0; // metros
|
||
|
||
bool foraContinuoSuficiente = dv.TempoForaRuaContinuo >= MIN_TEMPO_FORA_CORREDOR || dv.DistanciaForaRuaContinuo >= MIN_DIST_FORA_CORREDOR;
|
||
|
||
bool podeUsarSegmentacao = modoSimulador || (dadoFresco && dv.CorredorConfirmado);
|
||
|
||
CorredorAtual.DadosVisuais.JaAntecipado = podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente;
|
||
|
||
//Console.WriteLine($"podeUsarSegmentacao: {podeUsarSegmentacao}, pertoDoFim: {pertoDoFim}, foraDoCorredorAgora: {foraDoCorredorAgora}, foraContinuoSuficiente: {foraContinuoSuficiente}");
|
||
|
||
if (CorredorAtual.DadosVisuais.JaAntecipado)
|
||
{
|
||
List<PontoTrajetoriaModel> pontosRemover = CorredorAtual.Pontos.Where(x => !x.Visitado).ToList();
|
||
pontosRemover.ForEach(x => x.Visitado = true);
|
||
|
||
if (!CorredorAtual.Ultimo)
|
||
{
|
||
foreach (var ponto in pontosRemover)
|
||
{
|
||
CorredorAtual.Pontos.Remove(ponto);
|
||
_TrajetoriaFixa.Remove(ponto);
|
||
}
|
||
|
||
var pls = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 1];
|
||
if (CorredorAtual.Pontos.Count >= 2)
|
||
{
|
||
var pbs = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 2];
|
||
pls.AlterarTipo(TipoPontoRua.LigacaoSaida);
|
||
pbs.AlterarTipo(TipoPontoRua.BordaSaida);
|
||
_TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pls))?.AlterarTipo(pls.Tipo);
|
||
_TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pbs))?.AlterarTipo(pbs.Tipo);
|
||
}
|
||
|
||
// acha o ponto mais próximo do corredor seguinte
|
||
var pc = _Corredores[CorredorAtual.Idx + 1];
|
||
(GPSModel pp, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(GPSPosicaoAtual, pc.Pontos.Select(x => x.Posicao).ToList());
|
||
var _pp = pc.Pontos.FirstOrDefault(x => Math.Abs(x.Posicao.Latitude - pp.Latitude) < 1e-8 && Math.Abs(x.Posicao.Longitude - pp.Longitude) < 1e-8);
|
||
|
||
double anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda ? -AnguloAberturaCurva : AnguloAberturaCurva;
|
||
double anguloDesvio = PontoAtual.Orientacao + anguloControle;
|
||
double fd = FuncoesMatematicas.Map(op.Controle.PercentualVelocidadeSP, 10, 100, 0.7, 2.5);
|
||
|
||
var pontoDesvio = GPSUtils.GerarPontoDeslocado(pls.Posicao, anguloDesvio, DistanciaProjecaoRua * fd);
|
||
|
||
|
||
double _FatorLarguraCorredor = CorredorAtual.Largura / LarguraCorredorPadrao;
|
||
var proximoPontoProximoCorredor = pc.Pontos[_pp.idxPontoCorredor].Posicao;
|
||
List<GPSModel> CurvaConexao = CriarCurvaEntrePontos(pontoDesvio, proximoPontoProximoCorredor, DistanciaProjecaoRua * 1.5, pls.Orientacao, 6, false);
|
||
CurvaConexao.Insert(0, pontoDesvio);
|
||
|
||
|
||
IncluirDesvioNaTrajetoria(CurvaConexao, pls.idxPonto, pls.LarguraCorredor * 0.8);
|
||
|
||
|
||
|
||
// Garante que os primeiros pontos do corredor atual estão como visitados
|
||
if (_pp != null)
|
||
{
|
||
int idxProx = _pp.idxPontoCorredor;
|
||
|
||
// garante que não vai remover tudo (pra sobrar pelo menos 2 pontos)
|
||
int qtdRemoverProx = Math.Max(0, Math.Min(idxProx, pc.Pontos.Count - 2));
|
||
|
||
var pontosRemoverProx = pc.Pontos.Take(qtdRemoverProx).ToList();
|
||
|
||
foreach (var p in pontosRemoverProx)
|
||
{
|
||
pc.Pontos.Remove(p);
|
||
_TrajetoriaFixa.Remove(p);
|
||
}
|
||
|
||
if (pc.Pontos.Count >= 2)
|
||
{
|
||
var ple = pc.Pontos[0];
|
||
var pbe = pc.Pontos[1];
|
||
ple.AlterarTipo(TipoPontoRua.LigacaoEntrada);
|
||
pbe.AlterarTipo(TipoPontoRua.BordaEntrada);
|
||
_TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, ple))?.AlterarTipo(ple.Tipo);
|
||
_TrajetoriaFixa.FirstOrDefault(x => ReferenceEquals(x, pbe))?.AlterarTipo(pbe.Tipo);
|
||
}
|
||
}
|
||
|
||
ReindexarTrajetoriaFixa();
|
||
|
||
op.Sensoriamento.AtualizarDados();
|
||
HealthWorkerService.AtualizarDadosOperacao(true);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
public void AtualizarDadosAutonomiaCorredor(int? idxCorredor = null, bool? bat_liberada = null, bool? herb_liberado = null)
|
||
{
|
||
if (idxCorredor == null) idxCorredor = CorredorAtual?.Idx ?? 0;
|
||
if (bat_liberada == null) bat_liberada = false;
|
||
if (herb_liberado == null) herb_liberado = false;
|
||
AutonomiaCorredor.AtualizarDados((int)idxCorredor, (bool)bat_liberada, (bool)herb_liberado);
|
||
}
|
||
|
||
|
||
public void LoopAtualizaDados()
|
||
{
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
|
||
if (!(op.Parametros?.ControleAutomatico ?? false))
|
||
return;
|
||
|
||
//AtualizarPropriedadesPontosTrajetoria();
|
||
|
||
AtualizarDadosTrajetoria();
|
||
|
||
//op.AtualizaInformacoesControleOperacao();
|
||
|
||
//RedisService.Publish(CmdKey.ManagerWorkerRx, JsonConvert.SerializeObject(new { cmd = ManagerWorkerCommandType.AtualizarDadosControle }));
|
||
|
||
AtualizarDadosControleRedis();
|
||
}
|
||
|
||
private void AtualizarDadosControleRedis()
|
||
{
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
|
||
if (op == null ||
|
||
!op.Sensoriamento.Operacao.OperacaoIniciada)
|
||
{
|
||
return;
|
||
}
|
||
|
||
const int minTicksSemResposta = 500;
|
||
const int maxTicksSemResposta = 2000;
|
||
|
||
ManagerWorkerMessageResponseComandoModel novoComando = null;
|
||
|
||
try
|
||
{
|
||
string controleStr = RedisService.Get(CtxKey.DadosControle);
|
||
|
||
if (!string.IsNullOrWhiteSpace(controleStr))
|
||
{
|
||
var dictParams = JObject.Parse(controleStr);
|
||
novoComando = ManagerWorkerControlParser.LerComandoControle(dictParams);
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Variaveis.MostrarLog("[ManagerWorker] Erro ao ler comando: " + ex.Message);
|
||
}
|
||
|
||
DateTime agora = DateTime.Now;
|
||
var controle = op.Controle;
|
||
|
||
if (controle == null)
|
||
return;
|
||
|
||
bool recebeuComando = novoComando != null;
|
||
bool heartbeatMudou = recebeuComando && novoComando.heartbeat != controle.heartbeat;
|
||
bool sessaoMudou = recebeuComando && !string.IsNullOrWhiteSpace(novoComando.sessao_comando) && !string.Equals(controle.UltimaSessaoComando, novoComando.sessao_comando, StringComparison.Ordinal);
|
||
bool movimentoMudou = recebeuComando && (Math.Abs(controle.PercentualVelocidadeSP - novoComando.percentual_velocidade) >= 0.001 || controle.EmFreio != novoComando.em_freio);
|
||
bool direcionalMudou = recebeuComando && (Math.Abs(controle.Angulo - novoComando.angulo) >= 0.001 || controle.TipoMovimento != novoComando.tipo_movimento);
|
||
|
||
bool seqMovimentoMudou =
|
||
recebeuComando &&
|
||
novoComando.seq_movimento.HasValue &&
|
||
(
|
||
sessaoMudou ||
|
||
!controle
|
||
.UltimaSeqMovimentoAplicada
|
||
.HasValue ||
|
||
controle
|
||
.UltimaSeqMovimentoAplicada
|
||
.Value !=
|
||
novoComando.seq_movimento.Value
|
||
);
|
||
|
||
bool seqDirecionalMudou =
|
||
recebeuComando &&
|
||
novoComando.seq_direcional.HasValue &&
|
||
(
|
||
sessaoMudou ||
|
||
!controle
|
||
.UltimaSeqDirecionalAplicada
|
||
.HasValue ||
|
||
controle
|
||
.UltimaSeqDirecionalAplicada
|
||
.Value !=
|
||
novoComando.seq_direcional.Value
|
||
);
|
||
|
||
bool managerDestravou = recebeuComando && controle.ticks_sem_resposta != DateTime.MinValue && controle.ticks_sem_resposta.AddMilliseconds(minTicksSemResposta) < agora && heartbeatMudou;
|
||
|
||
bool possuiComandoNovo =
|
||
recebeuComando &&
|
||
(
|
||
sessaoMudou ||
|
||
seqMovimentoMudou ||
|
||
seqDirecionalMudou ||
|
||
movimentoMudou ||
|
||
direcionalMudou ||
|
||
managerDestravou
|
||
);
|
||
|
||
if (possuiComandoNovo)
|
||
{
|
||
ManagerWorkerControlParser.AplicarComandoControle(novoComando, forcarAplicacao: managerDestravou || sessaoMudou);
|
||
}
|
||
|
||
/*
|
||
* Watchdog:
|
||
*
|
||
* Sem mensagem ou heartbeat parado:
|
||
* começa/continua contagem.
|
||
*
|
||
* Heartbeat mudou:
|
||
* limpa contagem.
|
||
*/
|
||
if (!recebeuComando || !heartbeatMudou)
|
||
{
|
||
if (controle.ticks_sem_resposta == DateTime.MinValue)
|
||
{
|
||
controle.ticks_sem_resposta = agora;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
controle.ticks_sem_resposta = DateTime.MinValue;
|
||
}
|
||
|
||
if (recebeuComando)
|
||
{
|
||
controle.heartbeat = novoComando.heartbeat;
|
||
controle.Motivos = (novoComando.motivos ?? Array.Empty<string>()).ToList();
|
||
}
|
||
|
||
if (controle.ticks_sem_resposta == DateTime.MinValue)
|
||
{
|
||
return;
|
||
}
|
||
|
||
bool venceuTimeoutMaximo = controle.ticks_sem_resposta.AddMilliseconds(maxTicksSemResposta) < agora;
|
||
bool venceuTimeoutMinimo = controle.ticks_sem_resposta.AddMilliseconds(minTicksSemResposta) < agora;
|
||
|
||
if (venceuTimeoutMaximo)
|
||
{
|
||
if (op.Controle?.RPM_SP > 0)
|
||
{
|
||
op.Sensoriamento?.InserirLog(
|
||
T_Code.Npc,
|
||
StatusModulo.Falha,
|
||
0,
|
||
"Muito tempo sem receber novo comando " +
|
||
"do Manager Worker! Parando movimento!"
|
||
);
|
||
|
||
Variaveis.MostrarLog(
|
||
"Muito tempo sem receber novo comando " +
|
||
"do Manager Worker! Parando movimento!"
|
||
);
|
||
|
||
RedisService.AtualizarCampos(
|
||
CtxKey.DadosControle,
|
||
("velocidade_sp", 0)
|
||
);
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
if (venceuTimeoutMinimo)
|
||
{
|
||
double velocidadeSegura = op.Parametros.Controle.MovVelocidadeCErvasPercent;
|
||
if (op.Controle?.RPM_SP > velocidadeSegura)
|
||
{
|
||
op.Sensoriamento?.InserirLog(
|
||
T_Code.Npc,
|
||
StatusModulo.Alerta,
|
||
80,
|
||
"Muito tempo sem receber novo comando " +
|
||
"do Manager Worker! Reduzindo velocidade!"
|
||
);
|
||
|
||
Variaveis.MostrarLog(
|
||
"Muito tempo sem receber novo comando " +
|
||
"do Manager Worker! Reduzindo velocidade!"
|
||
);
|
||
|
||
RedisService.AtualizarCampos(
|
||
CtxKey.DadosControle,
|
||
(
|
||
"velocidade_sp",
|
||
velocidadeSegura
|
||
)
|
||
);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
private bool RecuperarAvancoPorPontoFuturo()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida)
|
||
return false;
|
||
|
||
int idxAtual = PontoAtual?.idxPonto ?? ObterIdxUltimoVisitado();
|
||
if (idxAtual < 0)
|
||
idxAtual = 0;
|
||
|
||
int idxFim = Math.Min(_TrajetoriaFixa.Count - 1, idxAtual + JanelaBuscaAvancoPontos);
|
||
|
||
int idxCorredorAtual = CorredorAtual?.Idx ?? PontoAtual?.idxCorredor ?? 0;
|
||
|
||
int idxPrimeiroNaoVisitado = _TrajetoriaFixa.FindIndex(Math.Max(0, idxAtual + 1), x => !x.Visitado);
|
||
|
||
int idxBarreiraEstrutural = -1;
|
||
|
||
if (idxPrimeiroNaoVisitado >= 0)
|
||
{
|
||
for (int i = idxPrimeiroNaoVisitado;
|
||
i <= idxFim;
|
||
i++)
|
||
{
|
||
var ponto = _TrajetoriaFixa[i];
|
||
|
||
if (ponto.idxCorredor != idxCorredorAtual)
|
||
break;
|
||
|
||
/*
|
||
* Não pode escolher um ponto depois de uma borda ou ligação
|
||
* que ainda não foi confirmada.
|
||
*/
|
||
if (idxBarreiraEstrutural >= 0 && i > idxBarreiraEstrutural)
|
||
break;
|
||
|
||
bool pontoEstrutural =
|
||
ponto.PontoBorda ||
|
||
ponto.PontoLigacao ||
|
||
ponto.Tipo == TipoPontoRua.BordaSaida ||
|
||
ponto.Tipo == TipoPontoRua.LigacaoSaida ||
|
||
ponto.Tipo == TipoPontoRua.BordaEntrada ||
|
||
ponto.Tipo == TipoPontoRua.LigacaoEntrada;
|
||
|
||
if (pontoEstrutural)
|
||
{
|
||
idxBarreiraEstrutural = i;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
int idxMelhor = -1;
|
||
double melhorDist = double.MaxValue;
|
||
|
||
AtualizarPropriedadesPontos(idxAtual + 1, idxFim);
|
||
|
||
for (int i = idxAtual + 1; i <= idxFim; i++)
|
||
{
|
||
var ponto = _TrajetoriaFixa[i];
|
||
|
||
// Recuperação local não pode trocar corredor.
|
||
// A troca de corredor fica exclusivamente em RecuperarTransicaoEntreCorredores().
|
||
if (ponto.idxCorredor != idxCorredorAtual)
|
||
continue;
|
||
|
||
double margemBase = ponto.LarguraCorredor * 0.8;
|
||
double margem = Math.Max(
|
||
margemBase + MargemExtraAvancoM,
|
||
DistanciaMaximaEntreLeituras + margemBase
|
||
);
|
||
|
||
if (ponto.DistanciaAtual <= margem && ponto.DistanciaAtual < melhorDist)
|
||
{
|
||
idxMelhor = i;
|
||
melhorDist = ponto.DistanciaAtual;
|
||
}
|
||
}
|
||
|
||
if (idxMelhor > idxAtual)
|
||
{
|
||
MarcarVisitadoAte(idxMelhor);
|
||
DefinirPontoAtual();
|
||
DefinirProximoPonto();
|
||
|
||
Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(
|
||
T_Code.Trj,
|
||
StatusModulo.Operante,
|
||
100,
|
||
$"[TRJ] Avanço recuperado por ponto futuro no mesmo corredor. idx={idxMelhor}, dist={melhorDist:F2}m"
|
||
);
|
||
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private bool RecuperarTransicaoEntreCorredores()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida ||
|
||
CorredorAtual == null ||
|
||
CorredorAtual.Ultimo ||
|
||
_Corredores == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
int idxCorredorAtual = CorredorAtual.Idx;
|
||
int idxProximoCorredor = idxCorredorAtual + 1;
|
||
|
||
if (idxProximoCorredor < 0 ||
|
||
idxProximoCorredor >= _Corredores.Count)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var pontosCorredorAtual = CorredorAtual.Pontos?
|
||
.OrderBy(x => x.idxPontoCorredor)
|
||
.ToList();
|
||
|
||
if (pontosCorredorAtual == null ||
|
||
pontosCorredorAtual.Count == 0)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
/*
|
||
* Atualiza somente a região final do corredor atual.
|
||
* Isso evita decidir com DistanciaAtual/Aproximando antigos.
|
||
*/
|
||
foreach (var ponto in pontosCorredorAtual
|
||
.Skip(Math.Max(0, pontosCorredorAtual.Count - 8)))
|
||
{
|
||
ponto.AtualizarPropriedades();
|
||
}
|
||
|
||
CorredorAtual.AtualizarDados();
|
||
|
||
var ultimoPontoCorredorAtual =
|
||
pontosCorredorAtual[pontosCorredorAtual.Count - 1];
|
||
|
||
int pontosRestantesCorredorAtual =
|
||
pontosCorredorAtual.Count(x => !x.Visitado);
|
||
|
||
/*
|
||
* Regra soberana:
|
||
*
|
||
* Com PontosRestantesMaximosParaTrocarCorredor = 0,
|
||
* nenhum ponto do corredor atual pode ficar pendente.
|
||
*
|
||
* BordaSaida não conclui o corredor se LigacaoSaida
|
||
* ainda não foi visitada.
|
||
*/
|
||
bool corredorAtualConcluido =
|
||
pontosRestantesCorredorAtual <=
|
||
PontosRestantesMaximosParaTrocarCorredor &&
|
||
ultimoPontoCorredorAtual.Visitado;
|
||
|
||
if (!corredorAtualConcluido)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var proximoCorredor =
|
||
_Corredores[idxProximoCorredor];
|
||
|
||
var primeiroPontoProximoCorredor =
|
||
proximoCorredor.Pontos?
|
||
.OrderBy(x => x.idxPontoCorredor)
|
||
.FirstOrDefault();
|
||
|
||
if (primeiroPontoProximoCorredor == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
/*
|
||
* Só observa o PRIMEIRO ponto do próximo corredor.
|
||
*
|
||
* Não procura o mais próximo entre vários pontos,
|
||
* porque isso permitia pular:
|
||
*
|
||
* LigacaoEntrada -> BordaEntrada -> ponto interno.
|
||
*/
|
||
primeiroPontoProximoCorredor.AtualizarPropriedades();
|
||
|
||
double distanciaEntrada =
|
||
primeiroPontoProximoCorredor.DistanciaAtual;
|
||
|
||
bool entradaProximoCorredorAlcancada =
|
||
distanciaEntrada <=
|
||
DistanciaRestanteMaximaParaTrocarCorredorM;
|
||
|
||
if (!entradaProximoCorredorAlcancada)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
/*
|
||
* Marca somente o primeiro ponto do novo corredor.
|
||
*
|
||
* Nunca utilizar MarcarVisitadoAte(idxMelhor) aqui,
|
||
* pois ele pode confirmar pontos internos não percorridos.
|
||
*/
|
||
primeiroPontoProximoCorredor.Visitado = true;
|
||
|
||
DefinirPontoAtual();
|
||
AtualizarCorredorAtual();
|
||
DefinirProximoPonto();
|
||
|
||
Variaveis
|
||
.OperacaoEmAndamento
|
||
.Sensoriamento?
|
||
.InserirLog(
|
||
T_Code.Trj,
|
||
StatusModulo.Operante,
|
||
100,
|
||
"[TRJ] Transição confirmada de forma estrita. " +
|
||
$"corredor={idxCorredorAtual}->{idxProximoCorredor}, " +
|
||
$"primeiro_idx={primeiroPontoProximoCorredor.idxPonto}, " +
|
||
$"dist={distanciaEntrada:F2}m, " +
|
||
$"restantes_anterior={pontosRestantesCorredorAtual}"
|
||
);
|
||
|
||
return true;
|
||
}
|
||
|
||
private bool RecuperarPontosDeixadosParaTras()
|
||
{
|
||
if (!_TrajetoriaFixaDefinida ||
|
||
GPSPosicaoAtual == null ||
|
||
PontoAtual == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
int idxAtual = Math.Max(
|
||
0,
|
||
Math.Min(
|
||
PontoAtual.idxPonto,
|
||
_TrajetoriaFixa.Count - 1
|
||
)
|
||
);
|
||
|
||
int idxCorredorAtual =
|
||
CorredorAtual?.Idx ??
|
||
PontoAtual.idxCorredor;
|
||
|
||
int idxPrimeiroNaoVisitado =
|
||
_TrajetoriaFixa.FindIndex(
|
||
idxAtual + 1,
|
||
x => !x.Visitado
|
||
);
|
||
|
||
if (idxPrimeiroNaoVisitado < 0)
|
||
return false;
|
||
|
||
bool recuperou = false;
|
||
int quantidadeRecuperada = 0;
|
||
|
||
int idxLimite = Math.Min(
|
||
_TrajetoriaFixa.Count - 1,
|
||
idxPrimeiroNaoVisitado +
|
||
MaxPontosRecuperadosPorCiclo - 1
|
||
);
|
||
|
||
for (int i = idxPrimeiroNaoVisitado;
|
||
i <= idxLimite;
|
||
i++)
|
||
{
|
||
var ponto = _TrajetoriaFixa[i];
|
||
|
||
/*
|
||
* Recuperação local nunca atravessa corredor.
|
||
*/
|
||
if (ponto.idxCorredor != idxCorredorAtual)
|
||
break;
|
||
|
||
/*
|
||
* Borda e ligação são confirmadas pelos fluxos
|
||
* específicos de entrada, saída e transição.
|
||
*/
|
||
if (EhPontoEstrutural(ponto))
|
||
break;
|
||
|
||
if (i + 1 >= _TrajetoriaFixa.Count)
|
||
break;
|
||
|
||
var proximo = _TrajetoriaFixa[i + 1];
|
||
|
||
if (proximo.idxCorredor != idxCorredorAtual)
|
||
break;
|
||
|
||
ponto.AtualizarPropriedades();
|
||
proximo.AtualizarPropriedades();
|
||
|
||
var progresso =
|
||
CalcularProgressoNoSegmento(
|
||
ponto.Posicao,
|
||
proximo.Posicao,
|
||
GPSPosicaoAtual
|
||
);
|
||
|
||
/*
|
||
* avançoLongitudinal:
|
||
*
|
||
* < 0 -> rover ainda está antes do ponto
|
||
* = 0 -> rover está na perpendicular do ponto
|
||
* > 0 -> ponto ficou atrás do rover
|
||
*/
|
||
bool pontoFicouParaTras =
|
||
progresso.avancoLongitudinalM >=
|
||
-ToleranciaLongitudinalAntesPontoM &&
|
||
progresso.distanciaLateralM <=
|
||
ObterToleranciaLateralRecuperacao(ponto);
|
||
|
||
if (!pontoFicouParaTras)
|
||
{
|
||
/*
|
||
* Regra fundamental:
|
||
* se este ponto ainda não ficou para trás,
|
||
* nenhum ponto posterior pode ser recuperado.
|
||
*/
|
||
break;
|
||
}
|
||
|
||
ponto.Visitado = true;
|
||
|
||
recuperou = true;
|
||
quantidadeRecuperada++;
|
||
}
|
||
|
||
if (!recuperou)
|
||
return false;
|
||
|
||
DefinirPontoAtual();
|
||
AtualizarCorredorAtual();
|
||
DefinirProximoPonto();
|
||
|
||
Variaveis
|
||
.OperacaoEmAndamento
|
||
.Sensoriamento?
|
||
.InserirLog(
|
||
T_Code.Trj,
|
||
StatusModulo.Operante,
|
||
100,
|
||
"[TRJ] Pontos deixados para trás recuperados. " +
|
||
$"qtd={quantidadeRecuperada}, " +
|
||
$"novo_atual={PontoAtual?.idxPonto}, " +
|
||
$"proximo={ProximoPonto?.idxPonto}"
|
||
);
|
||
|
||
return true;
|
||
}
|
||
private static bool EhPontoEstrutural(PontoTrajetoriaModel ponto)
|
||
{
|
||
if (ponto == null)
|
||
return true;
|
||
|
||
return
|
||
ponto.PontoBorda ||
|
||
ponto.PontoLigacao ||
|
||
ponto.Tipo == TipoPontoRua.BordaEntrada ||
|
||
ponto.Tipo == TipoPontoRua.BordaSaida ||
|
||
ponto.Tipo == TipoPontoRua.LigacaoEntrada ||
|
||
ponto.Tipo == TipoPontoRua.LigacaoSaida;
|
||
}
|
||
private double ObterToleranciaLateralRecuperacao(PontoTrajetoriaModel ponto)
|
||
{
|
||
double margemPonto =
|
||
Math.Max(
|
||
0.30,
|
||
ponto?.DistanciaMargem ?? 0.70
|
||
);
|
||
|
||
/*
|
||
* Não deixa uma largura muito grande do corredor
|
||
* transformar a recuperação em teletransporte lateral.
|
||
*/
|
||
return Math.Min(
|
||
ToleranciaLateralRecuperacaoM,
|
||
margemPonto + MargemExtraAvancoM
|
||
);
|
||
}
|
||
private static (double avancoLongitudinalM, double distanciaLateralM, double parametroSegmento) CalcularProgressoNoSegmento(GPSModel inicio, GPSModel fim, GPSModel robo)
|
||
{
|
||
if (inicio == null || fim == null || robo == null)
|
||
{
|
||
return (
|
||
double.NegativeInfinity,
|
||
double.PositiveInfinity,
|
||
0.0
|
||
);
|
||
}
|
||
|
||
double latRefRad =
|
||
inicio.Latitude *
|
||
Math.PI / 180.0;
|
||
|
||
const double metrosPorGrauLat =
|
||
110540.0;
|
||
|
||
double metrosPorGrauLon =
|
||
111320.0 *
|
||
Math.Cos(latRefRad);
|
||
|
||
double vx =
|
||
(fim.Longitude - inicio.Longitude) *
|
||
metrosPorGrauLon;
|
||
|
||
double vy =
|
||
(fim.Latitude - inicio.Latitude) *
|
||
metrosPorGrauLat;
|
||
|
||
double wx =
|
||
(robo.Longitude - inicio.Longitude) *
|
||
metrosPorGrauLon;
|
||
|
||
double wy =
|
||
(robo.Latitude - inicio.Latitude) *
|
||
metrosPorGrauLat;
|
||
|
||
double comprimentoQuadrado =
|
||
vx * vx + vy * vy;
|
||
|
||
if (comprimentoQuadrado < 1e-8)
|
||
{
|
||
return (
|
||
double.NegativeInfinity,
|
||
GPSUtils.DistanciaEntrePontos(
|
||
inicio,
|
||
robo
|
||
),
|
||
0.0
|
||
);
|
||
}
|
||
|
||
double comprimento =
|
||
Math.Sqrt(comprimentoQuadrado);
|
||
|
||
double parametro =
|
||
(wx * vx + wy * vy) /
|
||
comprimentoQuadrado;
|
||
|
||
double avancoLongitudinal =
|
||
(wx * vx + wy * vy) /
|
||
comprimento;
|
||
|
||
double produtoVetorial =
|
||
vx * wy - vy * wx;
|
||
|
||
double distanciaLateral =
|
||
Math.Abs(produtoVetorial) /
|
||
comprimento;
|
||
|
||
return (
|
||
avancoLongitudinal,
|
||
distanciaLateral,
|
||
parametro
|
||
);
|
||
}
|
||
|
||
|
||
private (List<List<GPSModel>>, List<double>) ProjetarCorredores()
|
||
{
|
||
var corredores = new List<List<GPSModel>>();
|
||
var larguras = new List<double>();
|
||
|
||
// Calcula o deslocamento lateral do GPS
|
||
double larguraTotal = VariaveisEquipamento.LarguraEsquerda + VariaveisEquipamento.LarguraDireita; // Largura total do robô
|
||
double deslocamentoLateralGPSpercentual = ((Math.Abs(VariaveisEquipamento.LarguraEsquerda - VariaveisEquipamento.LarguraDireita) / larguraTotal) / 2.0);
|
||
double deslocamentoLateralGPSmetros = (LarguraCorredorPadrao * deslocamentoLateralGPSpercentual);
|
||
|
||
if (Variaveis.OperacaoEmAndamento.Mapa.TipoMapa == TipoMapaOperacao.Corredores)
|
||
{
|
||
var ruasProjetadas = ProjetarRuasApartirDosCorredores(RuasPlantacao);
|
||
|
||
// Atualiza RuasPlantacao com as ruas projetadas
|
||
RuasPlantacao = ruasProjetadas;
|
||
}
|
||
|
||
for (int i = 0; i < RuasPlantacao.Count - 1; i++)
|
||
{
|
||
var rua1 = RuasPlantacao[i];
|
||
var rua2 = RuasPlantacao[i + 1];
|
||
|
||
// Garantir que as ruas tenham o mesmo número de pontos
|
||
if (rua1.Count != rua2.Count)
|
||
{
|
||
if (rua1.Count > rua2.Count)
|
||
{
|
||
rua2 = GPSUtils.InterpolarRota(rua2, rua1.Count);
|
||
}
|
||
else
|
||
{
|
||
rua1 = GPSUtils.InterpolarRota(rua1, rua2.Count);
|
||
}
|
||
}
|
||
|
||
var corredor = new List<GPSModel>();
|
||
|
||
double primeiraDistancia = 0;
|
||
|
||
// Calcular pontos médios entre as duas ruas e interpolar a cada 25 cm
|
||
for (int j = 0; j < rua1.Count - 1; j++)
|
||
{
|
||
var ponto1Rua1 = rua1[j];
|
||
var ponto2Rua1 = rua1[j + 1];
|
||
var ponto1Rua2 = rua2[j];
|
||
var ponto2Rua2 = rua2[j + 1];
|
||
|
||
//var pontoMedioInicial = GPSUtils.PontoMedio(ponto1Rua1, ponto1Rua2);
|
||
//var pontoMedioFinal = GPSUtils.PontoMedio(ponto2Rua1, ponto2Rua2);
|
||
|
||
var pontoMedioInicial = GPSUtils.PontoMedioComOffset(ponto1Rua1, ponto1Rua2, deslocamentoLateralGPSmetros);
|
||
var pontoMedioFinal = GPSUtils.PontoMedioComOffset(ponto2Rua1, ponto2Rua2, deslocamentoLateralGPSmetros);
|
||
|
||
var distancia = GPSUtils.DistanciaEntrePontos(pontoMedioInicial, pontoMedioFinal);
|
||
if (EspacamentoPrimeirosPontosProjecao && j == 0)
|
||
{
|
||
primeiraDistancia = distancia;
|
||
}
|
||
double distInterpolar = (EspacamentoPrimeirosPontosProjecao && j == 0) ? DistanciaEntrePontosCurva : DistanciaEntrePontos;
|
||
|
||
int numInterpolacoes = (int)(distancia / distInterpolar);
|
||
|
||
for (int k = 0; k <= numInterpolacoes; k++)
|
||
{
|
||
double t = numInterpolacoes == 0 ? 0 : k / (double)numInterpolacoes;
|
||
var pontoInterpolado = GPSUtils.InterpolarPonto(pontoMedioInicial, pontoMedioFinal, t);
|
||
corredor.Add(pontoInterpolado);
|
||
}
|
||
}
|
||
|
||
double distanciaCorredor = GPSUtils.DistanciaDoTrecho(corredor);
|
||
double distanciaRua1 = GPSUtils.DistanciaDoTrecho(rua1);
|
||
double distanciaRua2 = GPSUtils.DistanciaDoTrecho(rua2);
|
||
|
||
if ((distanciaCorredor + DistanciaEntrePontos) < distanciaRua1)
|
||
{
|
||
double dif = Math.Abs(distanciaRua1 - distanciaCorredor);
|
||
double anguloCorredor = GPSUtils.CalcularOrientacao(corredor[corredor.Count - 1], corredor[corredor.Count - 2]);
|
||
GPSModel novoPontoCorredor = GPSUtils.ProjetarPontoDeslocado(corredor[0], dif, anguloCorredor);
|
||
corredor.Insert(0, novoPontoCorredor);
|
||
}
|
||
else if ((distanciaCorredor + DistanciaEntrePontos) < distanciaRua2)
|
||
{
|
||
double dif = Math.Abs(distanciaRua2 - distanciaCorredor);
|
||
double anguloCorredor = GPSUtils.CalcularOrientacao(corredor[corredor.Count - 1], corredor[corredor.Count - 2]);
|
||
GPSModel novoPontoCorredor = GPSUtils.ProjetarPontoDeslocado(corredor[0], dif, anguloCorredor);
|
||
corredor.Insert(0, novoPontoCorredor);
|
||
}
|
||
|
||
// Remover pontos que estão a menos de 1m de distância
|
||
corredor = RemoverPontosMuitoProximos(corredor, DistanciaEntrePontos, primeiraDistancia, DistanciaEntrePontosCurva);
|
||
|
||
// Calcular largura média entre as duas ruas
|
||
double larguraCorredor = CalcularLarguraMediaDoCorredor(rua1, rua2);
|
||
|
||
corredores.Add(corredor);
|
||
larguras.Add(larguraCorredor);
|
||
}
|
||
|
||
|
||
return (corredores, larguras);
|
||
}
|
||
|
||
private List<List<GPSModel>> ProjetarRuasApartirDosCorredores(List<List<GPSModel>> corredores)
|
||
{
|
||
var ruasProjetadas = new List<List<GPSModel>>();
|
||
double larguraCorredor = LarguraCorredorPadrao;
|
||
|
||
if (corredores.Count > 1)
|
||
{
|
||
larguraCorredor = CalcularLarguraMediaDoCorredor(corredores[0], corredores[1]);
|
||
}
|
||
|
||
for (int i = 0; i < corredores.Count; i++)
|
||
{
|
||
var corredor = corredores[i];
|
||
double offset = larguraCorredor / 2.0;
|
||
|
||
// Calcular ruas apenas para o primeiro corredor
|
||
if (i == 0)
|
||
{
|
||
var ruaEsquerda = new List<GPSModel>();
|
||
|
||
for (int j = 0; j < corredor.Count - 1; j++)
|
||
{
|
||
var pontoAtual = corredor[j];
|
||
var pontoProximo = corredor[j + 1];
|
||
|
||
double angulo = GPSUtils.CalcularOrientacao(pontoAtual, pontoProximo);
|
||
var pontoEsquerda = GPSUtils.ProjetarPontoDeslocado(pontoAtual, offset, angulo + 90);
|
||
ruaEsquerda.Add(pontoEsquerda);
|
||
}
|
||
|
||
// Adiciona último ponto da rua esquerda
|
||
var ultimo = corredor[corredor.Count - 1];
|
||
var anterior = corredor[corredor.Count - 2];
|
||
double anguloFinal = GPSUtils.CalcularOrientacao(anterior, ultimo);
|
||
ruaEsquerda.Add(GPSUtils.ProjetarPontoDeslocado(ultimo, offset, anguloFinal + 90));
|
||
|
||
ruasProjetadas.Add(ruaEsquerda);
|
||
}
|
||
|
||
// Sempre criar a rua direita (inclusive para o último corredor)
|
||
var ruaDireita = new List<GPSModel>();
|
||
|
||
for (int j = 0; j < corredor.Count - 1; j++)
|
||
{
|
||
var pontoAtual = corredor[j];
|
||
var pontoProximo = corredor[j + 1];
|
||
|
||
double angulo = GPSUtils.CalcularOrientacao(pontoAtual, pontoProximo);
|
||
var pontoDireita = GPSUtils.ProjetarPontoDeslocado(pontoAtual, offset, angulo - 90);
|
||
ruaDireita.Add(pontoDireita);
|
||
}
|
||
|
||
var ultimoPonto = corredor[corredor.Count - 1];
|
||
var pontoAnterior = corredor[corredor.Count - 2];
|
||
double anguloUltimo = GPSUtils.CalcularOrientacao(pontoAnterior, ultimoPonto);
|
||
ruaDireita.Add(GPSUtils.ProjetarPontoDeslocado(ultimoPonto, offset, anguloUltimo - 90));
|
||
|
||
ruasProjetadas.Add(ruaDireita);
|
||
}
|
||
|
||
return ruasProjetadas;
|
||
}
|
||
|
||
// Método auxiliar para remover pontos muito próximos de forma balanceada
|
||
private List<GPSModel> RemoverPontosMuitoProximos(List<GPSModel> pontos, double distanciaMinima, double apartirDe, double distanciaMenor)
|
||
{
|
||
if (pontos == null || pontos.Count == 0)
|
||
return pontos;
|
||
|
||
pontos = pontos.Where(x => !double.IsNaN(x.Latitude) && !double.IsNaN(x.Longitude)).ToList();
|
||
|
||
var resultado = new List<GPSModel> { pontos[0] }; // Sempre incluir o primeiro ponto
|
||
double distanciaAcumulada = 0.0; // Distância total percorrida
|
||
|
||
for (int i = 1; i < pontos.Count - 1; i++) // Evita verificar o último ponto diretamente
|
||
{
|
||
double distancia = GPSUtils.DistanciaEntrePontos(resultado.Last(), pontos[i]);
|
||
distanciaAcumulada += distancia;
|
||
|
||
// Nos primeiros 2 metros, mantemos um espaçamento mínimo de 25 cm, depois 1 metro
|
||
double distanciaRequerida = distanciaAcumulada <= apartirDe ? distanciaMenor : distanciaMinima;
|
||
|
||
if (distancia >= distanciaRequerida)
|
||
{
|
||
resultado.Add(pontos[i]); // Mantém o ponto se a distância for suficiente
|
||
}
|
||
}
|
||
|
||
// Sempre adiciona o último ponto da lista para garantir fechamento da trajetória
|
||
if (!resultado.Contains(pontos.Last()))
|
||
{
|
||
resultado.Add(pontos.Last());
|
||
}
|
||
|
||
return resultado;
|
||
}
|
||
|
||
public static double CalcularLarguraMediaDoCorredor(List<GPSModel> rua1, List<GPSModel> rua2)
|
||
{
|
||
// 1. Interpolar as duas ruas para garantir pontos uniformemente distribuídos
|
||
List<GPSModel> rua1Interpolada = GPSUtils.InterpolarRota(rua1, Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua1)));
|
||
List<GPSModel> rua2Interpolada = GPSUtils.InterpolarRota(rua2, Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua2)));
|
||
|
||
double somaDistancias = 0;
|
||
int totalPontos = 0;
|
||
|
||
// 2. Para cada ponto da rua1 interpolada, encontrar o mais próximo na rua2 interpolada
|
||
foreach (var ponto1 in rua1Interpolada)
|
||
{
|
||
var pontoMaisProximo = rua2Interpolada.OrderBy(ponto2 => GPSUtils.DistanciaEntrePontos(ponto1, ponto2)).First();
|
||
|
||
double distancia = GPSUtils.DistanciaEntrePontos(ponto1, pontoMaisProximo);
|
||
|
||
somaDistancias += distancia;
|
||
totalPontos++;
|
||
}
|
||
|
||
// 3. Retorna a largura média do corredor
|
||
return totalPontos > 0 ? somaDistancias / totalPontos : 0;
|
||
}
|
||
|
||
public void ProjetarTrajetoriaFixa(GPSModel posicaoRobo = null)
|
||
{
|
||
List<PontoTrajetoriaModel> _trajetoriaFixa = new List<PontoTrajetoriaModel>();
|
||
(List<List<GPSModel>> corredores, List<double> CorredoresLarguras) = ProjetarCorredores();
|
||
Corredores = corredores;
|
||
|
||
double larguraCorredorMenor = LarguraCorredorPadrao * 0.9;
|
||
DirecaoCarroRua direcaoAtual = ExtremoMaisProximoMapa == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
||
|
||
PontoTrajetoriaModel PontoRobo = new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
|
||
{
|
||
idxCorredor = 0,
|
||
idxPonto = 0,
|
||
idxPontoCorredor = 0,
|
||
Posicao = posicaoRobo ?? GPSPosicaoAtual,
|
||
Orientacao = 0,
|
||
Direcao = direcaoAtual,
|
||
Visitado = true,
|
||
LarguraCorredor = 1.0
|
||
};
|
||
|
||
int idxCorredorDentro = VerificaEquipamentoDentroCorredor(PontoRobo.Posicao);
|
||
if (idxCorredorDentro > -1)
|
||
{
|
||
var corredor = Corredores[idxCorredorDentro];
|
||
double orientacaoCorredor = GPSUtils.CalcularOrientacao(corredor[0], corredor[1]);
|
||
double difOri = Math.Abs(orientacaoCorredor - PontoRobo.Posicao?.OrientacaoReal ?? 0);
|
||
bool alinhado = difOri < 180;
|
||
if (idxCorredorDentro % 2 == 0)
|
||
{
|
||
direcaoAtual = alinhado ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
|
||
}
|
||
else
|
||
{
|
||
direcaoAtual = alinhado ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
|
||
}
|
||
PontoRobo.Direcao = direcaoAtual;
|
||
}
|
||
|
||
_trajetoriaFixa.Add(PontoRobo);
|
||
|
||
int idxEsq = 0;
|
||
int idxDir = 0;
|
||
|
||
double anguloAcrescentar = direcaoAtual == DirecaoCarroRua.Ida ? AnguloAberturaCurva : -AnguloAberturaCurva;
|
||
|
||
for (int idx = 0; idx < Corredores.Count(); idx++)
|
||
{
|
||
double _FatorLarguraCorredor = CorredoresLarguras[idx] / LarguraCorredorPadrao;
|
||
bool _FatorLarguraElevado = _FatorLarguraCorredor > 1.2;
|
||
|
||
if (_trajetoriaFixa.Count() > 1 && idxEsq == 0 && idxDir == 0)
|
||
{
|
||
var pontos = _trajetoriaFixa.Where(x => x.Tipo == TipoPontoRua.Rua).ToList();
|
||
if (pontos.Count >= 2)
|
||
{
|
||
var p0 = pontos[0];
|
||
var p1 = pontos[1];
|
||
|
||
(List<GPSModel> ruaEsq, List<GPSModel> ruaDir) = DeterminarRuasLaterais(p0.Posicao, p1.Posicao, RuasPlantacao[0], RuasPlantacao[1]);
|
||
|
||
idxEsq = RuasPlantacao.IndexOf(ruaEsq);
|
||
idxDir = RuasPlantacao.IndexOf(ruaDir);
|
||
}
|
||
else
|
||
{
|
||
// ⚠️ Caso de erro: não há dois pontos de rua, mesmo com mais de 1 ponto na trajetória.
|
||
Console.WriteLine("Não há ao menos dois pontos do tipo Rua.");
|
||
}
|
||
}
|
||
|
||
bool ultimoCorredor = idx == Corredores.Count() - 1;
|
||
List<GPSModel> CorredorAtual = Corredores[idx];
|
||
|
||
//double dP0 = GPSUtils.DistanciaEntrePontos(_trajetoriaFixa.Last().Posicao, CorredorAtual[0]);
|
||
//double dP1 = GPSUtils.DistanciaEntrePontos(_trajetoriaFixa.Last().Posicao, CorredorAtual[CorredorAtual.Count() - 1]);
|
||
//if (dP1 < dP0)
|
||
if (direcaoAtual == DirecaoCarroRua.Volta)
|
||
{
|
||
CorredorAtual.Reverse();
|
||
}
|
||
|
||
double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).First(), CorredorAtual.First());
|
||
GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.First(), DistanciaProjecaoRua, anguloProjetar1);
|
||
|
||
var ultimoPontoTrajetoria = _trajetoriaFixa.LastOrDefault();
|
||
|
||
PontoTrajetoriaModel PontoInicial = new PontoTrajetoriaModel(TipoPontoRua.LigacaoEntrada)
|
||
{
|
||
idxCorredor = idx,
|
||
idxPonto = _trajetoriaFixa.Count(),
|
||
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
||
Posicao = PrimeiroPonto,
|
||
Direcao = direcaoAtual,
|
||
Orientacao = GPSUtils.CalcularOrientacao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto),
|
||
Visitado = false,
|
||
LarguraCorredor = larguraCorredorMenor
|
||
};
|
||
|
||
bool PontoInicialAdicionado = false;
|
||
// Criar curva suave para ligacao dos corredores
|
||
if (idx > 0)
|
||
{
|
||
// Verifica se a distância entre o ultimo ponto da rua anterior e o primeiro ponto da nova rua é curto o bastante para gerar a curva de manobra
|
||
double distancia = GPSUtils.DistanciaEntrePontos(ultimoPontoTrajetoria.Posicao, PontoInicial.Posicao);
|
||
bool distanciaMinima = distancia < DistanciaManobraEntreRuas;
|
||
|
||
if (distanciaMinima && false)
|
||
{
|
||
int pontosAdicionr = Convert.ToInt32(CorredoresLarguras[idx] / (DistanciaEntrePontosCurva * _FatorLarguraCorredor));
|
||
var _penultimoPonto = _trajetoriaFixa[_trajetoriaFixa.Count() - 2].Posicao;
|
||
double anguloProjecao = GPSUtils.CalcularOrientacao(Corredores[idx - 1].First(), Corredores[idx - 1].Last());
|
||
GPSModel _ultimoPonto = GPSUtils.ProjetarPontoDeslocado(Corredores[idx - 1].Last(), DistanciaProjecaoRua, anguloProjecao);
|
||
List<GPSModel> CurvaConexao = CriarCurvaEntrePontos(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, DistanciaProjecaoRua * 2.0, anguloProjecao, pontosAdicionr, false);
|
||
|
||
for (int i = 0; i < CurvaConexao.Count; i++)
|
||
{
|
||
var ponto = CurvaConexao[i];
|
||
bool pontoCorredorAnterior = i < (pontosAdicionr / 2);
|
||
int idxCorredorPontoCurva = pontoCorredorAnterior ? idx - 1 : idx;
|
||
bool pontoLigacao = CurvaConexao.IndexOf(ponto) == CurvaConexao.Count() - 1;
|
||
|
||
bool meiaCurva = false;
|
||
|
||
if ((meiaCurva && pontoCorredorAnterior) || !meiaCurva)
|
||
{
|
||
_trajetoriaFixa.Add(new PontoTrajetoriaModel(pontoLigacao ? TipoPontoRua.LigacaoEntrada : TipoPontoRua.CruvaEntreCorredores)
|
||
{
|
||
idxCorredor = idxCorredorPontoCurva,
|
||
idxPonto = _trajetoriaFixa.Count(),
|
||
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
||
Posicao = ponto,
|
||
Direcao = pontoLigacao ? direcaoAtual : DirecaoCarroRua.Manobra,
|
||
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, ponto),
|
||
Visitado = false,
|
||
LarguraCorredor = larguraCorredorMenor * 0.9
|
||
});
|
||
}
|
||
|
||
}
|
||
PontoInicialAdicionado = true;
|
||
}
|
||
}
|
||
|
||
if (!PontoInicialAdicionado)
|
||
{
|
||
_trajetoriaFixa.Add(PontoInicial);
|
||
}
|
||
|
||
foreach (GPSModel pontoAtual in CorredorAtual)
|
||
{
|
||
int idxPonto = CorredorAtual.IndexOf(pontoAtual);
|
||
bool bordaEntrada = idxPonto == 0;
|
||
bool bordaSaida = idxPonto == CorredorAtual.Count() - 1;
|
||
double distanciaCorredor = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.idxCorredor == idx).Select(x => x.Posicao).ToList());
|
||
|
||
PontoTrajetoriaModel PontoTrajetoria = new PontoTrajetoriaModel(bordaEntrada ? TipoPontoRua.BordaEntrada : bordaSaida ? TipoPontoRua.BordaSaida : TipoPontoRua.Rua)
|
||
{
|
||
idxCorredor = idx,
|
||
idxPonto = _trajetoriaFixa.Count(),
|
||
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
||
Posicao = pontoAtual,
|
||
Direcao = direcaoAtual,
|
||
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, pontoAtual),
|
||
Visitado = false,
|
||
LarguraCorredor = bordaEntrada || bordaSaida ? larguraCorredorMenor : CorredoresLarguras[idx]
|
||
};
|
||
|
||
_trajetoriaFixa.Add(PontoTrajetoria);
|
||
}
|
||
|
||
double anguloProjetar2 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(CorredorAtual.Count() - 2).First(), CorredorAtual.Last());
|
||
// Realiza um acréscimo no angulo à projetar, permitindo que o equipamento faça uma curva mais aberta entre corredores
|
||
if (!ultimoCorredor && !_FatorLarguraElevado)
|
||
{
|
||
anguloProjetar2 += anguloAcrescentar;
|
||
anguloAcrescentar *= -1;
|
||
}
|
||
double percentualAcrescimoPontoAberturaCurva = 0.8;
|
||
GPSModel UltimoPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.Last(), !ultimoCorredor ? (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva) : (DistanciaProjecaoRua * percentualAcrescimoPontoAberturaCurva), anguloProjetar2);
|
||
|
||
PontoTrajetoriaModel PontoFinal = new PontoTrajetoriaModel(TipoPontoRua.LigacaoSaida)
|
||
{
|
||
idxCorredor = idx,
|
||
idxPonto = _trajetoriaFixa.Count(),
|
||
idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
|
||
Posicao = UltimoPonto,
|
||
Direcao = direcaoAtual,
|
||
Orientacao = GPSUtils.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, UltimoPonto),
|
||
Visitado = false,
|
||
LarguraCorredor = ultimoCorredor ? larguraCorredorMenor : (larguraCorredorMenor * 0.8)
|
||
};
|
||
_trajetoriaFixa.Add(PontoFinal);
|
||
|
||
direcaoAtual = direcaoAtual == Enums.DirecaoCarroRua.Ida ? Enums.DirecaoCarroRua.Volta : Enums.DirecaoCarroRua.Ida;
|
||
}
|
||
|
||
_TrajetoriaFixa = new List<PontoTrajetoriaModel>(_trajetoriaFixa);
|
||
|
||
if (_trajetoriaFixa.Count() > 1 && idxEsq == 0 && idxDir == 0)
|
||
{
|
||
(List<GPSModel> ruaEsq, List<GPSModel> ruaDir) = DeterminarRuasLaterais(_trajetoriaFixa[1].Posicao, _trajetoriaFixa[2].Posicao, RuasPlantacao[0], RuasPlantacao[1]);
|
||
idxEsq = RuasPlantacao.IndexOf(ruaEsq);
|
||
idxDir = RuasPlantacao.IndexOf(ruaDir);
|
||
}
|
||
int incEsq = 0;
|
||
int incDir = 0;
|
||
_Corredores = _TrajetoriaFixa
|
||
.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo)
|
||
.GroupBy(x => x.idxCorredor)
|
||
.Select((x, i) =>
|
||
{
|
||
double distanciaTotal = GPSUtils.DistanciaDoTrecho(x.Select(y => y.Posicao).ToList());
|
||
|
||
(incDir, incEsq) = AtualizaIndiceRuaLateralCorredor(i, idxDir, incDir, incEsq);
|
||
|
||
var obj = new CorredorTrajetoriaModel()
|
||
{
|
||
Idx = x.Key,
|
||
Largura = CorredoresLarguras[x.Key],
|
||
DistanciaTotal = distanciaTotal,
|
||
Pontos = x.ToList(),
|
||
QtdPontos = x.Count(),
|
||
idxRuaDireita = idxDir + incDir,
|
||
idxRuaEsquerda = idxEsq + incEsq,
|
||
FatorLarguraCorredor = CorredoresLarguras[x.Key] / LarguraCorredorPadrao
|
||
};
|
||
|
||
return obj;
|
||
})
|
||
.ToList();
|
||
|
||
_Corredores.ForEach(x => x.Ultimo = x.Idx == _Corredores.Count() - 1);
|
||
|
||
DistanciaTotal = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo).Select(x => x.Posicao).ToList());
|
||
DistanciaPercorrida = 0.0;
|
||
|
||
RetornandoBase = false;
|
||
|
||
CorredorAtual = _Corredores.Count > 0 ? _Corredores[0] : null;
|
||
|
||
AutonomiaCorredor.Iniciar();
|
||
|
||
AtualizarDadosTrajetoria();
|
||
|
||
LoopAtualizaDados();
|
||
|
||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||
}
|
||
|
||
private List<GPSModel> CriarCurvaEntrePontos(GPSModel _p1, GPSModel _p2, double distanciaCurva, double anguloCurva, int qtdPontos, bool manterPontos)
|
||
{
|
||
GPSModel P0 = CalcularPontoControle(anguloCurva, _p1, _p2, distanciaCurva);
|
||
List<GPSModel> curva = GerarCurvaConexao(_p1, _p2, P0, qtdPontos);
|
||
if (manterPontos) curva.Insert(0, _p1);
|
||
else curva.RemoveAt(curva.Count - 1);
|
||
return curva;
|
||
}
|
||
|
||
private (int, int) AtualizaIndiceRuaLateralCorredor(int i, int idxDir, int incDir, int incEsq)
|
||
{
|
||
bool par = (i % 2 == 0);
|
||
if (i > 0)
|
||
{
|
||
if (idxDir == 0)
|
||
{
|
||
incDir += !par ? 2 : 0;
|
||
incEsq += !par ? 0 : 2;
|
||
}
|
||
else
|
||
{
|
||
incDir += par ? 2 : 0;
|
||
incEsq += par ? 0 : 2;
|
||
}
|
||
}
|
||
return (incDir, incEsq);
|
||
}
|
||
|
||
public static GPSModel CalcularPontoControle(double anguloCurva, GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
|
||
{
|
||
double angulo = GPSUtils.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
|
||
|
||
// Calcular a diferença de ângulo corretamente no espaço de 360 graus
|
||
double diferencaAngulo = ((angulo - anguloCurva + 540) % 360) - 180;
|
||
|
||
bool ParaDentro = diferencaAngulo > 0; // Agora o cálculo é confiável
|
||
|
||
double lat = (ultimoPontoRuaAtual.Latitude + primeiroPontoProximaRua.Latitude) / 2;
|
||
double lon = (ultimoPontoRuaAtual.Longitude + primeiroPontoProximaRua.Longitude) / 2;
|
||
GPSModel pontoMedio = new GPSModel()
|
||
{
|
||
Latitude = lat,
|
||
Longitude = lon,
|
||
};
|
||
|
||
// Ajuste o ângulo com base na direção da curva
|
||
double ajusteAngulo = ParaDentro ? -90 : 90;
|
||
|
||
// Projetar um ponto a uma certa distância na direção ajustada
|
||
return GPSUtils.ProjetarPontoDeslocado(pontoMedio, distancia, angulo + ajusteAngulo);
|
||
}
|
||
|
||
public static List<GPSModel> GerarCurvaConexao(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, GPSModel pontoControle, int numeroPontos)
|
||
{
|
||
List<GPSModel> pontosCurva = new List<GPSModel>();
|
||
for (int i = 2; i <= numeroPontos; i++)
|
||
{
|
||
double t = i / (double)numeroPontos;
|
||
pontosCurva.Add(CalcularBezierQuadratica(ultimoPontoRuaAtual, pontoControle, primeiroPontoProximaRua, t));
|
||
}
|
||
return pontosCurva;
|
||
}
|
||
|
||
public static GPSModel CalcularBezierQuadratica(GPSModel p0, GPSModel p1, GPSModel p2, double t)
|
||
{
|
||
double x = Math.Pow(1 - t, 2) * p0.Latitude + 2 * (1 - t) * t * p1.Latitude + Math.Pow(t, 2) * p2.Latitude;
|
||
double y = Math.Pow(1 - t, 2) * p0.Longitude + 2 * (1 - t) * t * p1.Longitude + Math.Pow(t, 2) * p2.Longitude;
|
||
return new GPSModel { Latitude = x, Longitude = y };
|
||
}
|
||
|
||
|
||
public void AjustarTrajetoriaParaDesvio(Obstaculo obstaculo)
|
||
{
|
||
double distanciaObstaculo = ((double)obstaculo.DistanciaMedia_mm / 1000);
|
||
double larguraObstaculo = ((double)obstaculo.Largura_mm / 1000);
|
||
double anguloCaminho = AnguloCaminho;
|
||
double anguloDesvioInicial = anguloCaminho + obstaculo.AnguloParaDesvio;
|
||
double anguloDesvioFinal = anguloCaminho - obstaculo.AnguloParaDesvio;
|
||
|
||
GPSModel ponto1 = GPSUtils.GerarPontoDeslocado(GPSPosicaoAtual, anguloDesvioInicial, distanciaObstaculo);
|
||
GPSModel ponto2 = GPSUtils.GerarPontoDeslocado(ponto1, anguloCaminho, larguraObstaculo);
|
||
GPSModel ponto3 = GPSUtils.GerarPontoDeslocado(ponto2, anguloDesvioFinal, distanciaObstaculo);
|
||
List<PontoTrajetoriaModel> lista = new List<PontoTrajetoriaModel>()
|
||
{
|
||
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
||
{
|
||
Posicao = ponto1,
|
||
},
|
||
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
||
{
|
||
Posicao = ponto2,
|
||
},
|
||
new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
||
{
|
||
Posicao = ponto3,
|
||
}
|
||
};
|
||
|
||
var idxRemover = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, ponto3, distanciaObstaculo);
|
||
_TrajetoriaDinamica.RemoveRange(1, idxRemover);
|
||
_TrajetoriaDinamica.InsertRange(1, lista);
|
||
}
|
||
|
||
private int EncontrarIndiceFinalDesvio(List<GPSModel> trajetoria, GPSModel ultimoWaypointDesvio, double distanciaSeguranca)
|
||
{
|
||
// Encontrar o índice do waypoint que está imediatamente após o retorno do desvio
|
||
// Este é um método simplificado. A lógica exata pode variar dependendo da precisão necessária.
|
||
double distanciaAnterior = 0;
|
||
for (int i = 0; i < trajetoria.Count; i++)
|
||
{
|
||
double distancia = GPSUtils.DistanciaEntrePontos(trajetoria[i], ultimoWaypointDesvio);
|
||
bool aumentando = distancia > distanciaAnterior;
|
||
distanciaAnterior = distancia;
|
||
if (distancia < distanciaSeguranca)
|
||
{
|
||
if (aumentando)
|
||
{
|
||
return i;
|
||
}
|
||
}
|
||
}
|
||
return 0; // Se não encontrar, assuma o último waypoint
|
||
}
|
||
|
||
private (List<GPSModel> ruaEsquerda, List<GPSModel> ruaDireita) DeterminarRuasLaterais(GPSModel robo, GPSModel proximoPonto, List<GPSModel> rua1, List<GPSModel> rua2)
|
||
{
|
||
// Determinar qual está à esquerda e qual está à direita
|
||
var r1 = new List<GPSModel>(rua1);
|
||
if (ExtremoMaisProximoMapa == 1)
|
||
{
|
||
r1.Reverse();
|
||
}
|
||
r1 = r1.Take(3).ToList();
|
||
bool rua1EstaEsquerda = EstaEsquerda(robo, proximoPonto, Centroide(r1));
|
||
|
||
return rua1EstaEsquerda ? (rua1, rua2) : (rua2, rua1);
|
||
}
|
||
|
||
// Retorna o ponto central da rua (média das coordenadas)
|
||
private GPSModel Centroide(List<GPSModel> rua)
|
||
{
|
||
double latMedia = rua.Average(p => p.Latitude);
|
||
double lonMedia = rua.Average(p => p.Longitude);
|
||
return new GPSModel { Latitude = latMedia, Longitude = lonMedia };
|
||
}
|
||
|
||
// Determina se um ponto está à esquerda ou à direita do movimento do robô
|
||
private bool EstaEsquerda(GPSModel robo, GPSModel proximoPonto, GPSModel ponto)
|
||
{
|
||
double dxRobo = proximoPonto.Longitude - robo.Longitude;
|
||
double dyRobo = proximoPonto.Latitude - robo.Latitude;
|
||
double dxPonto = ponto.Longitude - robo.Longitude;
|
||
double dyPonto = ponto.Latitude - robo.Latitude;
|
||
|
||
double crossProduct = (dxRobo * dyPonto) - (dyRobo * dxPonto);
|
||
return crossProduct > 0; // Se for positivo, está à esquerda; se for negativo, está à direita.
|
||
}
|
||
|
||
|
||
|
||
// Retorno a base automatico
|
||
public void IniciarRetornoBase(GPSModel baseGps, List<GPSModel> waypointsManual = null)
|
||
{
|
||
bool valido = false;
|
||
List<GPSModel> pontosRetorno = null;
|
||
|
||
// =========================
|
||
// MODO MANUAL (operador)
|
||
// =========================
|
||
if (waypointsManual != null && waypointsManual.Count > 0)
|
||
{
|
||
pontosRetorno = new List<GPSModel>();
|
||
pontosRetorno.AddRange(waypointsManual);
|
||
valido = true;
|
||
}
|
||
else
|
||
{
|
||
// =========================
|
||
// MODO AUTOMÁTICO
|
||
// =========================
|
||
if (baseGps != null)
|
||
(valido, pontosRetorno) = GerarTrajetoriaRetornoAutomatico(baseGps);
|
||
}
|
||
|
||
if (!valido || pontosRetorno == null || pontosRetorno.Count < 1)
|
||
Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Falha, 0, "Trajetória de retorno inválida.");
|
||
else
|
||
{
|
||
AplicarTrajetoriaFixaRetorno(pontosRetorno, "RetornoBase", waypointsManual?.Any() ?? false);
|
||
AtualizarDadosOperacaoParaRetorno();
|
||
}
|
||
}
|
||
|
||
private void AplicarTrajetoriaFixaRetorno(List<GPSModel> pontos, string motivo, bool manual)
|
||
{
|
||
var traj = new List<PontoTrajetoriaModel>();
|
||
|
||
// Ponto atual do robô (padrão do sistema)
|
||
traj.Add(new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
|
||
{
|
||
idxCorredor = 0,
|
||
idxPonto = 0,
|
||
idxPontoCorredor = 0,
|
||
Posicao = GPSPosicaoAtual,
|
||
Visitado = true,
|
||
LarguraCorredor = 1.0
|
||
});
|
||
|
||
var pontos_corrigidos = RemoverPontosMuitoProximos(pontos, DistanciaEntrePontos, 0, DistanciaEntrePontosCurva);
|
||
double distanciaPararAntecipado = 3.5;
|
||
int idx = 1;
|
||
int qtdPontosRemoverFinal = manual ? 0 : (int)(distanciaPararAntecipado / DistanciaEntrePontos);
|
||
foreach (var p in pontos_corrigidos)
|
||
{
|
||
if (idx < (pontos_corrigidos.Count - qtdPontosRemoverFinal) || idx == pontos_corrigidos.Count - 1)
|
||
{
|
||
traj.Add(new PontoTrajetoriaModel(TipoPontoRua.Rua)
|
||
{
|
||
idxCorredor = 0,
|
||
idxPonto = traj.Count,
|
||
idxPontoCorredor = traj.Count,
|
||
Posicao = p,
|
||
Visitado = false,
|
||
LarguraCorredor = 1.5
|
||
});
|
||
}
|
||
idx++;
|
||
}
|
||
|
||
traj[traj.Count - 1].LarguraCorredor = distanciaPararAntecipado;
|
||
|
||
DistanciaTotal = GPSUtils.DistanciaDoTrecho(traj.Select(x => x.Posicao).ToList());
|
||
DistanciaPercorrida = 0.0;
|
||
|
||
RetornandoBase = true;
|
||
|
||
_TrajetoriaFixa = traj;
|
||
|
||
Variaveis.OperacaoEmAndamento.Sensoriamento?.InserirLog(T_Code.Trj, StatusModulo.Operante, 100, $"[RETORNO] Trajetória aplicada ({motivo}) - Pontos: {traj.Count}");
|
||
}
|
||
|
||
private void AtualizarDadosOperacaoParaRetorno()
|
||
{
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
|
||
AutonomiaCorredor.Parar();
|
||
|
||
AtualizarDadosTrajetoria();
|
||
|
||
LoopAtualizaDados();
|
||
|
||
var new_op = OperacaoModel.CarregarParametrosOperacaoPadrao(ModoOperacao.RetornoBase, CarregarModulos: false);
|
||
op.Parametros = new_op.Parametros;
|
||
|
||
if (!op.Sensoriamento.Operacao.OperacaoIniciada)
|
||
{
|
||
op.Sensoriamento.Operacao.OperacaoIniciada = true;
|
||
op.Simulando = !GPSService.Iniciado;
|
||
}
|
||
|
||
RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false));
|
||
}
|
||
|
||
private (bool valido, List<GPSModel> pontos) GerarTrajetoriaRetornoAutomatico(GPSModel baseGps)
|
||
{
|
||
var pontos = new List<GPSModel>();
|
||
|
||
GPSModel posicaoAtual = GPSPosicaoAtual;
|
||
|
||
double distancia = GPSUtils.DistanciaEntrePontos(posicaoAtual, baseGps);
|
||
if (distancia < 5)
|
||
return (false, pontos);
|
||
|
||
VerificaInicioOperacaoMeioRua(false);
|
||
AtualizarDadosTrajetoria();
|
||
|
||
// =========================
|
||
// REGRA 0 – Está dentro de corredor?
|
||
// =========================
|
||
if (CorredorAtual != null && CorredorAtual.Dentro && !CorredorAtual.Concluido)
|
||
{
|
||
var pontosRestantesCorredor = CorredorAtual.Pontos.Where(x => x.idxPonto >= ProximoPonto.idxPonto).Select(x => x.Posicao).ToList();
|
||
if (pontosRestantesCorredor.Count > 0)
|
||
{
|
||
pontos.AddRange(pontosRestantesCorredor);
|
||
posicaoAtual = pontos[pontos.Count - 1];
|
||
}
|
||
}
|
||
|
||
int caso = EscolherCasoRetorno(posicaoAtual, baseGps);
|
||
|
||
List<GPSModel> trecho = new List<GPSModel>();
|
||
switch (caso)
|
||
{
|
||
case 1:
|
||
// =========================
|
||
// CASO 1 – Linha reta até a base
|
||
// (V1 simples para validar tudo)
|
||
// =========================
|
||
trecho.AddRange(GerarTrechoReto(posicaoAtual, baseGps, DistanciaEntrePontos));
|
||
break;
|
||
case 2:
|
||
// =========================
|
||
// CASO 2 – Existe mapa entre a base e o rover, e tambem existe corredores com bordas proximos a ambos
|
||
// (V1 simples para validar tudo)
|
||
// =========================
|
||
trecho.AddRange(GerarTrajetoriaViaCorredor(posicaoAtual, baseGps));
|
||
break;
|
||
case 3:
|
||
// =========================
|
||
// CASO 3 – Existe mapa entre a base e o rover, mas nao existe corredores com bordas proximos a ambos
|
||
// (V1 simples para validar tudo)
|
||
// =========================
|
||
trecho.AddRange(GerarTrajetoriaPerimetral(posicaoAtual, baseGps, margem_m: 2.0));
|
||
break;
|
||
}
|
||
pontos.AddRange(trecho);
|
||
|
||
return (trecho.Any(), pontos);
|
||
}
|
||
|
||
private int EscolherCasoRetorno(GPSModel posRobo, GPSModel posBase)
|
||
{
|
||
// Coleta pontos do mapa (use RuasPlantacao, que é o "mapa real")
|
||
var ptsMapa = new List<GPSModel>();
|
||
foreach (var r in RuasPlantacao)
|
||
ptsMapa.AddRange(r);
|
||
|
||
// Se não tem mapa suficiente, não tem envelope pra cruzar => caso 1
|
||
if (ptsMapa.Count < 3)
|
||
return 1;
|
||
|
||
// 1) Dá pra ir reto sem cruzar envelope?
|
||
bool cruzaEnvelope = LinhaCruzaEnvelopeConvexo(posRobo, posBase, ptsMapa);
|
||
|
||
if (!cruzaEnvelope)
|
||
return 1;
|
||
|
||
// 2) Se cruza, tenta usar corredor
|
||
var cand = EncontrarCorredoresProximos(posRobo, posBase, raioRobo: 15.0, raioBase: 15.0);
|
||
if (cand != null && cand.Count > 0)
|
||
return 2;
|
||
|
||
// 3) Se cruza e não tem corredor, vai perimetral
|
||
return 3;
|
||
}
|
||
|
||
private bool LinhaCruzaEnvelopeConvexo(GPSModel a, GPSModel b, List<GPSModel> ptsMapa)
|
||
{
|
||
var refLat = ptsMapa.Average(p => p.Latitude);
|
||
var refLon = ptsMapa.Average(p => p.Longitude);
|
||
|
||
var aXY = LatLonToXY(a, refLat, refLon);
|
||
var bXY = LatLonToXY(b, refLat, refLon);
|
||
|
||
var hull = ConvexHull(ptsMapa.Select(p => LatLonToXY(p, refLat, refLon)).ToList());
|
||
if (hull.Count < 3) return false;
|
||
|
||
// Se A ou B está dentro do envelope, consideramos que "cruza" (está no mapa)
|
||
if (PontoDentroPoligono(aXY, hull) || PontoDentroPoligono(bXY, hull))
|
||
return true;
|
||
|
||
// Interseção do segmento com arestas do hull
|
||
for (int i = 0; i < hull.Count; i++)
|
||
{
|
||
var p1 = hull[i];
|
||
var p2 = hull[(i + 1) % hull.Count];
|
||
|
||
if (SegmentosIntersectam(aXY, bXY, p1, p2))
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private bool PontoDentroPoligono((double x, double y) p, List<(double x, double y)> poly)
|
||
{
|
||
bool inside = false;
|
||
for (int i = 0, j = poly.Count - 1; i < poly.Count; j = i++)
|
||
{
|
||
var pi = poly[i];
|
||
var pj = poly[j];
|
||
|
||
bool intersect = ((pi.y > p.y) != (pj.y > p.y)) &&
|
||
(p.x < (pj.x - pi.x) * (p.y - pi.y) / (pj.y - pi.y + 1e-12) + pi.x);
|
||
|
||
if (intersect) inside = !inside;
|
||
}
|
||
return inside;
|
||
}
|
||
|
||
private bool SegmentosIntersectam((double x, double y) a, (double x, double y) b, (double x, double y) c, (double x, double y) d)
|
||
{
|
||
double o1 = Orient(a, b, c);
|
||
double o2 = Orient(a, b, d);
|
||
double o3 = Orient(c, d, a);
|
||
double o4 = Orient(c, d, b);
|
||
|
||
if (o1 * o2 < 0 && o3 * o4 < 0) return true;
|
||
|
||
if (Math.Abs(o1) < 1e-12 && OnSegment(a, b, c)) return true;
|
||
if (Math.Abs(o2) < 1e-12 && OnSegment(a, b, d)) return true;
|
||
if (Math.Abs(o3) < 1e-12 && OnSegment(c, d, a)) return true;
|
||
if (Math.Abs(o4) < 1e-12 && OnSegment(c, d, b)) return true;
|
||
|
||
return false;
|
||
}
|
||
|
||
private double Orient((double x, double y) a, (double x, double y) b, (double x, double y) c)
|
||
{
|
||
return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
|
||
}
|
||
|
||
private bool OnSegment((double x, double y) a, (double x, double y) b, (double x, double y) p)
|
||
{
|
||
return p.x >= Math.Min(a.x, b.x) - 1e-9 && p.x <= Math.Max(a.x, b.x) + 1e-9 &&
|
||
p.y >= Math.Min(a.y, b.y) - 1e-9 && p.y <= Math.Max(a.y, b.y) + 1e-9;
|
||
}
|
||
|
||
#region CASO 1
|
||
|
||
private List<GPSModel> GerarTrechoReto(GPSModel a, GPSModel b, double espacamento_m)
|
||
{
|
||
var lista = new List<GPSModel>();
|
||
|
||
double dist = GPSUtils.DistanciaEntrePontos(a, b);
|
||
if (dist < espacamento_m)
|
||
{
|
||
lista.Add(b);
|
||
return lista;
|
||
}
|
||
|
||
int passos = (int)Math.Ceiling(dist / espacamento_m);
|
||
|
||
for (int i = 1; i <= passos; i++)
|
||
{
|
||
double t = (double)i / passos;
|
||
lista.Add(GPSUtils.InterpolarPonto(a, b, t));
|
||
}
|
||
|
||
return lista;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region CASO 2
|
||
|
||
private enum TipoCondicaoRoboRua
|
||
{
|
||
Indefinido,
|
||
EntraPeloInicio,
|
||
EntraPeloFim,
|
||
SaiPeloInicio,
|
||
SaiPeloFim
|
||
}
|
||
|
||
private List<(int idxCorredor, TipoCondicaoRoboRua condicaoCarro)> EncontrarCorredoresProximos(GPSModel posRobo, GPSModel posBase, double raioRobo = 15.0, double raioBase = 15.0)
|
||
{
|
||
var candidatos = new List<(int idxCorredor, TipoCondicaoRoboRua condicaoCarro)>();
|
||
|
||
double angRoboBase = GPSUtils.CalcularOrientacao(posRobo, posBase);
|
||
double limiar = 45.0;
|
||
|
||
for (int i = 0; i < Corredores.Count; i++)
|
||
{
|
||
var corredor = Corredores[i];
|
||
if (corredor.Count < 2) continue;
|
||
|
||
double angCorredor = GPSUtils.CalcularOrientacao(corredor[0], corredor[corredor.Count - 1]);
|
||
double angCorredorI = (angCorredor + 180.0) % 360.0;
|
||
|
||
double difAng = GPSUtils.CalcularDiferencaAngulo(angRoboBase, angCorredor);
|
||
double difAngI = GPSUtils.CalcularDiferencaAngulo(angRoboBase, angCorredorI);
|
||
bool angCorreto = Math.Abs(difAng) <= limiar;
|
||
bool angCorretoI = Math.Abs(difAngI) <= limiar;
|
||
if (!angCorreto && !angCorretoI) continue;
|
||
|
||
var pInicio = corredor.First();
|
||
var pFim = corredor.Last();
|
||
|
||
double dBaseInicio = GPSUtils.DistanciaEntrePontos(posBase, pInicio);
|
||
double dBaseFim = GPSUtils.DistanciaEntrePontos(posBase, pFim);
|
||
|
||
double dRoboInicio = GPSUtils.DistanciaEntrePontos(posRobo, pInicio);
|
||
double dRoboFim = GPSUtils.DistanciaEntrePontos(posRobo, pFim);
|
||
|
||
// Caso 1: robô entra pelo início, base sai pelo fim
|
||
if (dRoboInicio <= raioRobo && dBaseFim <= raioBase)
|
||
candidatos.Add((i, TipoCondicaoRoboRua.EntraPeloInicio));
|
||
|
||
// Caso 2: robô entra pelo fim, base sai pelo início
|
||
else if (dRoboFim <= raioRobo && dBaseInicio <= raioBase)
|
||
candidatos.Add((i, TipoCondicaoRoboRua.EntraPeloFim));
|
||
|
||
// Caso 2: robô sai pelo inicio, base entra pelo início
|
||
else if (dRoboInicio <= raioRobo && dBaseInicio <= raioBase)
|
||
candidatos.Add((i, TipoCondicaoRoboRua.SaiPeloInicio));
|
||
|
||
// Caso 2: robô sai pelo fim, base entra pelo fim
|
||
else if (dRoboFim <= raioRobo && dBaseFim <= raioBase)
|
||
candidatos.Add((i, TipoCondicaoRoboRua.SaiPeloFim));
|
||
}
|
||
|
||
return candidatos;
|
||
}
|
||
|
||
private (int idxCorredor, TipoCondicaoRoboRua condicaoCarro) SelecionarMelhorCorredor(List<(int idxCorredor, TipoCondicaoRoboRua condicaoCarro)> candidatos, GPSModel posRobo)
|
||
{
|
||
double melhorScore = double.MaxValue;
|
||
(int idxCorredor, TipoCondicaoRoboRua condicaoCarro) melhor = (-1, TipoCondicaoRoboRua.Indefinido);
|
||
|
||
foreach (var c in candidatos)
|
||
{
|
||
var corredor = Corredores[c.idxCorredor];
|
||
var ponto =
|
||
c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio ? corredor.First() :
|
||
c.condicaoCarro == TipoCondicaoRoboRua.EntraPeloFim || c.condicaoCarro == TipoCondicaoRoboRua.SaiPeloFim ? corredor.Last() :
|
||
corredor.First();
|
||
|
||
double comprimento = 0;
|
||
for (int i = 1; i < corredor.Count; i++)
|
||
comprimento += GPSUtils.DistanciaEntrePontos(corredor[i - 1], corredor[i]);
|
||
|
||
double score = comprimento + GPSUtils.DistanciaEntrePontos(posRobo, ponto);
|
||
|
||
if (score < melhorScore)
|
||
{
|
||
melhorScore = score;
|
||
melhor = c;
|
||
}
|
||
}
|
||
|
||
return melhor;
|
||
}
|
||
|
||
private List<GPSModel> GerarTrajetoriaViaCorredor(GPSModel posRobo, GPSModel posBase)
|
||
{
|
||
var candidatos = EncontrarCorredoresProximos(posRobo, posBase);
|
||
if (candidatos.Count == 0) return new List<GPSModel>();
|
||
|
||
(int idxCorredor, TipoCondicaoRoboRua condicaoCarro) = SelecionarMelhorCorredor(candidatos, posRobo);
|
||
if (idxCorredor < 0) return new List<GPSModel>();
|
||
|
||
var pontos = new List<GPSModel>();
|
||
var corredor = Corredores[idxCorredor];
|
||
|
||
bool sentidoCorreto = condicaoCarro == TipoCondicaoRoboRua.EntraPeloInicio || condicaoCarro == TipoCondicaoRoboRua.SaiPeloInicio;
|
||
GPSModel entrada = sentidoCorreto ? corredor.First() : corredor.Last();
|
||
GPSModel saida = sentidoCorreto ? corredor.Last() : corredor.First();
|
||
|
||
// 1️⃣ robô → entrada do corredor
|
||
pontos.AddRange(GerarTrechoReto(posRobo, entrada, DistanciaEntrePontos));
|
||
|
||
// 2️⃣ polilinha do corredor (no sentido correto)
|
||
if (sentidoCorreto)
|
||
pontos.AddRange(corredor);
|
||
else
|
||
pontos.AddRange(corredor.AsEnumerable().Reverse());
|
||
|
||
// 3️⃣ saída do corredor → base
|
||
pontos.AddRange(GerarTrechoReto(saida, posBase, DistanciaEntrePontos));
|
||
|
||
return pontos;
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region CASO 3
|
||
|
||
private List<GPSModel> GerarTrajetoriaPerimetral(GPSModel posRobo, GPSModel posBase, double margem_m = 12.0)
|
||
{
|
||
// 1) Coleta todos os pontos do mapa (corredores)
|
||
var ptsMapa = new List<GPSModel>();
|
||
foreach (var c in RuasPlantacao)
|
||
ptsMapa.AddRange(c);
|
||
|
||
// Se mapa não tiver pontos suficientes, não dá pra perímetro
|
||
if (ptsMapa.Count < 3) return new List<GPSModel>();
|
||
|
||
// 2) Constrói o perímetro (convex hull) e aplica offset pra fora (margem)
|
||
var perimetro = ConstruirPerimetroConvexoComOffset(ptsMapa, margem_m);
|
||
if (perimetro.Count < 3) return new List<GPSModel>();
|
||
|
||
// 3) Encontra os pontos de conexão (robô e base) no perímetro
|
||
var (idxRobo, pRoboPerim) = PontoMaisProximoNaPolilinha(perimetro, posRobo);
|
||
var (idxBase, pBasePerim) = PontoMaisProximoNaPolilinha(perimetro, posBase);
|
||
|
||
// 4) Caminho pelo perímetro: escolhe sentido menor
|
||
var arco1 = ExtrairArcoPerimetro(perimetro, idxRobo, idxBase, sentidoHorario: true);
|
||
var arco2 = ExtrairArcoPerimetro(perimetro, idxRobo, idxBase, sentidoHorario: false);
|
||
|
||
double dist1 = DistanciaPolilinha(arco1);
|
||
double dist2 = DistanciaPolilinha(arco2);
|
||
|
||
var arco = dist1 <= dist2 ? arco1 : arco2;
|
||
|
||
// 5) Monta a trajetória final
|
||
var outPts = new List<GPSModel>();
|
||
|
||
// robô -> ponto de entrada no perímetro
|
||
outPts.AddRange(GerarTrechoReto(posRobo, pRoboPerim, DistanciaEntrePontos));
|
||
|
||
// entra no perímetro (garante que comece exatamente do ponto projetado)
|
||
if (outPts.Count == 0 || GPSUtils.DistanciaEntrePontos(outPts[outPts.Count - 1], pRoboPerim) > 0.2)
|
||
outPts.Add(pRoboPerim);
|
||
|
||
// arco do perímetro
|
||
outPts.AddRange(arco);
|
||
|
||
// garante que termina exatamente no ponto projetado da base
|
||
if (outPts.Count == 0 || GPSUtils.DistanciaEntrePontos(outPts[outPts.Count - 1], pBasePerim) > 0.2)
|
||
outPts.Add(pBasePerim);
|
||
|
||
// perímetro -> base
|
||
outPts.AddRange(GerarTrechoReto(pBasePerim, posBase, DistanciaEntrePontos));
|
||
|
||
return outPts;
|
||
}
|
||
|
||
private List<GPSModel> ConstruirPerimetroConvexoComOffset(List<GPSModel> pts, double margem_m)
|
||
{
|
||
// Usa uma projeção local simples (equiretangular) pra converter lat/lon em XY(m)
|
||
var refLat = pts.Average(p => p.Latitude);
|
||
var refLon = pts.Average(p => p.Longitude);
|
||
|
||
var xy = pts.Select(p => (p, xy: LatLonToXY(p, refLat, refLon))).ToList();
|
||
var hullXY = ConvexHull(xy.Select(t => t.xy).ToList());
|
||
|
||
if (hullXY.Count < 3) return new List<GPSModel>();
|
||
|
||
// Centróide no plano XY
|
||
double cx = hullXY.Average(v => v.x);
|
||
double cy = hullXY.Average(v => v.y);
|
||
|
||
// Offset pra fora: empurra cada vértice pra longe do centróide
|
||
var perimetro = new List<GPSModel>();
|
||
foreach (var v in hullXY)
|
||
{
|
||
double vx = v.x - cx;
|
||
double vy = v.y - cy;
|
||
double norm = Math.Sqrt(vx * vx + vy * vy);
|
||
if (norm < 1e-6) continue;
|
||
|
||
double ox = v.x + (vx / norm) * margem_m;
|
||
double oy = v.y + (vy / norm) * margem_m;
|
||
perimetro.Add(XYToLatLon(ox, oy, refLat, refLon));
|
||
}
|
||
|
||
// Fecha o loop (opcional). Eu gosto de deixar sem repetir o primeiro,
|
||
// e tratar a polilinha como circular nos métodos de arco.
|
||
return perimetro;
|
||
}
|
||
|
||
private (double x, double y) LatLonToXY(GPSModel p, double refLat, double refLon)
|
||
{
|
||
const double R = 6378137.0;
|
||
double lat = p.Latitude * Math.PI / 180.0;
|
||
double lon = p.Longitude * Math.PI / 180.0;
|
||
double lat0 = refLat * Math.PI / 180.0;
|
||
double lon0 = refLon * Math.PI / 180.0;
|
||
|
||
double x = (lon - lon0) * Math.Cos(lat0) * R;
|
||
double y = (lat - lat0) * R;
|
||
return (x, y);
|
||
}
|
||
|
||
private GPSModel XYToLatLon(double x, double y, double refLat, double refLon)
|
||
{
|
||
const double R = 6378137.0;
|
||
double lat0 = refLat * Math.PI / 180.0;
|
||
double lon0 = refLon * Math.PI / 180.0;
|
||
|
||
double lat = (y / R) + lat0;
|
||
double lon = (x / (R * Math.Cos(lat0))) + lon0;
|
||
|
||
return new GPSModel
|
||
{
|
||
Latitude = lat * 180.0 / Math.PI,
|
||
Longitude = lon * 180.0 / Math.PI,
|
||
Momento = DateTime.UtcNow
|
||
};
|
||
}
|
||
|
||
private List<(double x, double y)> ConvexHull(List<(double x, double y)> pts)
|
||
{
|
||
// Remove duplicados grosseiros
|
||
pts = pts.Distinct().ToList();
|
||
if (pts.Count < 3) return pts;
|
||
|
||
// pivot: menor y, depois menor x
|
||
var pivot = pts.OrderBy(p => p.y).ThenBy(p => p.x).First();
|
||
|
||
// ordena por ângulo polar com pivot
|
||
var sorted = pts
|
||
.Where(p => p != pivot)
|
||
.OrderBy(p => Math.Atan2(p.y - pivot.y, p.x - pivot.x))
|
||
.ThenBy(p => Dist2(pivot, p))
|
||
.ToList();
|
||
|
||
var stack = new List<(double x, double y)>();
|
||
stack.Add(pivot);
|
||
stack.Add(sorted[0]);
|
||
|
||
for (int i = 1; i < sorted.Count; i++)
|
||
{
|
||
var p = sorted[i];
|
||
while (stack.Count >= 2 && Cross(stack[stack.Count - 2], stack[stack.Count - 1], p) <= 0)
|
||
stack.RemoveAt(stack.Count - 1);
|
||
|
||
stack.Add(p);
|
||
}
|
||
|
||
return stack;
|
||
}
|
||
|
||
private double Dist2((double x, double y) a, (double x, double y) b)
|
||
{
|
||
double dx = a.x - b.x, dy = a.y - b.y;
|
||
return dx * dx + dy * dy;
|
||
}
|
||
|
||
private double Cross((double x, double y) a, (double x, double y) b, (double x, double y) c)
|
||
{
|
||
// (b-a) x (c-a)
|
||
return (b.x - a.x) * (c.y - a.y) - (b.y - a.y) * (c.x - a.x);
|
||
}
|
||
|
||
private (int idx, GPSModel ponto) PontoMaisProximoNaPolilinha(List<GPSModel> poly, GPSModel alvo)
|
||
{
|
||
int best = 0;
|
||
double bestD = double.MaxValue;
|
||
|
||
for (int i = 0; i < poly.Count; i++)
|
||
{
|
||
double d = GPSUtils.DistanciaEntrePontos(alvo, poly[i]);
|
||
if (d < bestD)
|
||
{
|
||
bestD = d;
|
||
best = i;
|
||
}
|
||
}
|
||
|
||
return (best, poly[best]);
|
||
}
|
||
|
||
private List<GPSModel> ExtrairArcoPerimetro(List<GPSModel> perim, int idxIni, int idxFim, bool sentidoHorario)
|
||
{
|
||
var arco = new List<GPSModel>();
|
||
int n = perim.Count;
|
||
|
||
int i = idxIni;
|
||
while (true)
|
||
{
|
||
arco.Add(perim[i]);
|
||
if (i == idxFim) break;
|
||
|
||
i = sentidoHorario ? (i + 1) % n : (i - 1 + n) % n;
|
||
|
||
// segurança (evita loop infinito em caso de bug)
|
||
if (arco.Count > n + 2) break;
|
||
}
|
||
|
||
return arco;
|
||
}
|
||
|
||
private double DistanciaPolilinha(List<GPSModel> pts)
|
||
{
|
||
double sum = 0;
|
||
for (int i = 1; i < pts.Count; i++)
|
||
sum += GPSUtils.DistanciaEntrePontos(pts[i - 1], pts[i]);
|
||
return sum;
|
||
}
|
||
|
||
#endregion
|
||
|
||
|
||
|
||
private void IncluirDesvioNaTrajetoria(List<GPSModel> pontos, int idxPontoApartir, double larguraCorredor)
|
||
{
|
||
if (pontos == null || pontos.Count == 0)
|
||
return;
|
||
|
||
// 1. Achar o ponto base na trajetória fixa
|
||
var pontoBase = _TrajetoriaFixa.FirstOrDefault(x => x.idxPonto == idxPontoApartir);
|
||
if (pontoBase == null)
|
||
return;
|
||
|
||
// Conferir corredor
|
||
var corredor = _Corredores[pontoBase.idxCorredor];
|
||
|
||
// 2. Gerar novos pontos de desvio (sem se preocupar ainda com idxPonto / idxPontoCorredor)
|
||
var novosPontos = new List<PontoTrajetoriaModel>();
|
||
var pa = pontoBase;
|
||
|
||
foreach (var pontoGps in pontos)
|
||
{
|
||
var novoPonto = new PontoTrajetoriaModel(TipoPontoRua.Desvio)
|
||
{
|
||
// idxPonto e idxPontoCorredor serão ajustados depois
|
||
idxCorredor = pa.idxCorredor,
|
||
Posicao = pontoGps,
|
||
Direcao = pa.Direcao,
|
||
LarguraCorredor = larguraCorredor,
|
||
Orientacao = GPSUtils.CalcularOrientacao(pontoGps, pa.Posicao),
|
||
Visitado = false
|
||
};
|
||
|
||
novosPontos.Add(novoPonto);
|
||
pa = novoPonto;
|
||
}
|
||
|
||
// 3. Inserir na lista global a partir do ponto base
|
||
int insertIdxGlobal = _TrajetoriaFixa.IndexOf(pontoBase) + 1;
|
||
int insertIdxCorredor = corredor.Pontos.IndexOf(pontoBase) + 1;
|
||
|
||
foreach (var np in novosPontos)
|
||
{
|
||
_TrajetoriaFixa.Insert(insertIdxGlobal, np);
|
||
corredor.Pontos.Insert(insertIdxCorredor, np);
|
||
|
||
insertIdxGlobal++;
|
||
insertIdxCorredor++;
|
||
}
|
||
|
||
// 4. Reindexar listas
|
||
ReindexarTrajetoriaFixa();
|
||
}
|
||
|
||
private void ReindexarTrajetoriaFixa()
|
||
{
|
||
// 1. Reindexar idxPonto globalmente
|
||
for (int i = 0; i < _TrajetoriaFixa.Count; i++)
|
||
{
|
||
_TrajetoriaFixa[i].idxPonto = i;
|
||
}
|
||
|
||
// 2. Reindexar idxPontoCorredor para cada corredor
|
||
for (int c = 0; c < _Corredores.Count; c++)
|
||
{
|
||
var corr = _Corredores[c];
|
||
for (int i = 0; i < corr.Pontos.Count; i++)
|
||
{
|
||
corr.Pontos[i].idxPontoCorredor = i;
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
public class CorredorTrajetoriaModel
|
||
{
|
||
public int Idx { get; set; }
|
||
public double Largura { get; set; }
|
||
public double DistanciaTotal { get; set; }
|
||
[JsonProperty]
|
||
public double DistanciaPercorridaCorredor { get; private set; } = 0;
|
||
[JsonProperty]
|
||
public double DistanciaPercorridaTotal { get; private set; } = 0;
|
||
public void AtualizarDistancias(double distancia)
|
||
{
|
||
DistanciaPercorridaTotal += distancia;
|
||
if (Dentro)
|
||
{
|
||
DistanciaPercorridaCorredor += distancia;
|
||
}
|
||
}
|
||
public List<PontoTrajetoriaModel> Pontos { get; set; }
|
||
public int QtdPontos { get; set; }
|
||
[JsonProperty]
|
||
public bool Dentro { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaRestante { get; private set; }
|
||
[JsonProperty]
|
||
public double Progresso { get; private set; }
|
||
[JsonProperty]
|
||
public bool Concluido { get; private set; }
|
||
public bool Ultimo { get; set; }
|
||
public int idxRuaEsquerda { get; set; }
|
||
public int idxRuaDireita { get; set; }
|
||
public double FatorLarguraCorredor { get; set; }
|
||
public DadosVisuaisCorredor DadosVisuais { get; set; } = new DadosVisuaisCorredor();
|
||
|
||
public void AtualizarDados()
|
||
{
|
||
Dentro = false;
|
||
// Itera diretamente sem criar uma lista temporária
|
||
foreach (var ponto in Pontos)
|
||
{
|
||
// Precisa estar na margem do ponto e, ser um ponto de rua, que significa que já está dentro, ou então de borda, e que esteja se afastando do centro dele
|
||
if (
|
||
ponto.Visitado &&
|
||
ponto.NaMargem &&
|
||
(ponto.Tipo == TipoPontoRua.Rua ||
|
||
(
|
||
(ponto.Tipo == TipoPontoRua.BordaEntrada && !ponto.Aproximando) ||
|
||
(ponto.Tipo == TipoPontoRua.BordaSaida && ponto.Aproximando)
|
||
)
|
||
)
|
||
)
|
||
{
|
||
Dentro = true;
|
||
break; // Encontramos um ponto válido, podemos sair
|
||
}
|
||
}
|
||
|
||
//DistanciaRestante = DistanciaTotal - DistanciaPercorridaCorredor;
|
||
DistanciaRestante = GPSUtils.DistanciaDoTrecho(Pontos.Where(x => !x.Visitado).Select(x => x.Posicao).ToList());
|
||
Progresso = (DistanciaTotal > 0) ? FuncoesMatematicas.Clamp((DistanciaPercorridaCorredor / DistanciaTotal) * 100.0, 0, 100) : 0;
|
||
Concluido = !Pontos.Any(x => !x.Visitado);
|
||
}
|
||
|
||
public CorredorTrajetoriaModel Clone(bool clonarPontos = true)
|
||
{
|
||
var obj = new CorredorTrajetoriaModel()
|
||
{
|
||
Idx = Idx,
|
||
Largura = Largura,
|
||
DistanciaTotal = DistanciaTotal,
|
||
DistanciaPercorridaCorredor = DistanciaPercorridaCorredor,
|
||
DistanciaPercorridaTotal = DistanciaPercorridaTotal,
|
||
QtdPontos = QtdPontos,
|
||
Dentro = Dentro,
|
||
Concluido = Concluido,
|
||
idxRuaEsquerda = idxRuaEsquerda,
|
||
idxRuaDireita = idxRuaDireita,
|
||
DistanciaRestante = DistanciaRestante,
|
||
Progresso = Progresso,
|
||
Pontos = new List<PontoTrajetoriaModel>(),
|
||
Ultimo = Ultimo,
|
||
FatorLarguraCorredor = FatorLarguraCorredor,
|
||
DadosVisuais = DadosVisuais
|
||
};
|
||
if (clonarPontos)
|
||
{
|
||
obj.Pontos = new List<PontoTrajetoriaModel>(Pontos);
|
||
}
|
||
|
||
return obj;
|
||
}
|
||
|
||
|
||
public class DadosVisuaisCorredor
|
||
{
|
||
public bool JaAntecipado { get; set; } = false;
|
||
// Quanto já andou DENTRO do corredor (EntrandoRua, CaminhandoRua, SaindoRua)
|
||
public double TempoDentroRuaTotal { get; set; } = 0;
|
||
public double DistanciaDentroRuaTotal { get; set; } = 0;
|
||
|
||
// Quanto já andou FORA do corredor (Direcionando)
|
||
public double TempoForaRuaTotal { get; set; } = 0;
|
||
public double DistanciaForaRuaTotal { get; set; } = 0;
|
||
|
||
// Janela contínua atual fora do corredor (protege contra buracos curtos)
|
||
public double TempoForaRuaContinuo { get; set; } = 0;
|
||
public double DistanciaForaRuaContinuo { get; set; } = 0;
|
||
|
||
// Para acumular tempo/distância por intervalo entre chamadas
|
||
public DateTime UltimoUpdate { get; set; } = DateTime.MinValue;
|
||
public double DistanciaRestanteAnterior { get; set; } = -1;
|
||
|
||
// Flag: este corredor realmente existiu (teve cana por X tempo/distância)
|
||
public bool CorredorConfirmado { get; set; } = false;
|
||
}
|
||
|
||
}
|
||
|
||
public class PontoTrajetoriaModel
|
||
{
|
||
public int idxCorredor { get; set; }
|
||
public int idxPonto { get; set; }
|
||
public int idxPontoCorredor { get; set; }
|
||
public GPSModel Posicao { get; set; }
|
||
public double Orientacao { get; set; }
|
||
public DirecaoCarroRua Direcao { get; set; }
|
||
public TipoPontoRua Tipo { private set; get; }
|
||
public double LarguraCorredor { get; set; }
|
||
public double DistanciaMargem { get; private set; }
|
||
public bool Visitado { get; set; } = false;
|
||
|
||
// ✅ Agora, propriedades podem ser atualizadas com `set; private`
|
||
[JsonProperty]
|
||
public double OrientacaoAtual { get; private set; }
|
||
[JsonProperty]
|
||
public double OrientacaoAnterior { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaTrajeto { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaAtual { get; private set; }
|
||
[JsonProperty]
|
||
public double DistanciaAnterior { get; private set; }
|
||
[JsonProperty]
|
||
public bool Aproximando { get; private set; }
|
||
[JsonProperty]
|
||
public bool NaMargem { get; private set; }
|
||
[JsonProperty]
|
||
public bool PontoBorda { get; private set; }
|
||
[JsonProperty]
|
||
public bool PontoLigacao { get; private set; }
|
||
|
||
// ✅ Construtor: Calcula propriedades fixas uma única vez
|
||
public PontoTrajetoriaModel(TipoPontoRua tipo)
|
||
{
|
||
Tipo = tipo;
|
||
|
||
PontoBorda = Tipo == Enums.TipoPontoRua.BordaEntrada || Tipo == Enums.TipoPontoRua.BordaSaida;
|
||
PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida;
|
||
}
|
||
|
||
public void AlterarTipo(Enums.TipoPontoRua novo)
|
||
{
|
||
Tipo = novo;
|
||
PontoBorda = Tipo == Enums.TipoPontoRua.BordaEntrada || Tipo == Enums.TipoPontoRua.BordaSaida;
|
||
PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida;
|
||
}
|
||
|
||
public void AtualizarPropriedades()
|
||
{
|
||
AtualizarDistanciaAtual();
|
||
AtualizarDistanciaAnterior();
|
||
AtualizarOrientacaoAtual();
|
||
AtualizarOrientacaoAnterior();
|
||
AtualizarNaMargem();
|
||
AtualizarAproximando();
|
||
AtualizarDistanciaTrajeto();
|
||
AtualizarVisitado();
|
||
}
|
||
|
||
private void AtualizarVisitado()
|
||
{
|
||
if (Visitado) return; // Se já foi visitado, não faz nada
|
||
|
||
double distanciaEntrePontos =
|
||
Tipo == TipoPontoRua.CruvaEntreCorredores ? TrajetoriaMapaOperacaoModel.DistanciaEntrePontosCurva :
|
||
TrajetoriaMapaOperacaoModel.DistanciaEntrePontos;
|
||
|
||
// Define o limite de distância para considerar o ponto como visitado
|
||
double fatorAjuste = Tipo == TipoPontoRua.CruvaEntreCorredores ? 3.0 : 2.0; // Curvas podem ter menos tolerância
|
||
double limiteDistancia = Math.Max(0.1, Math.Ceiling(TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras / distanciaEntrePontos) * fatorAjuste * distanciaEntrePontos);
|
||
|
||
// Se o ponto está dentro da margem e dentro do limite de distância, marca como visitado
|
||
if (NaMargem && DistanciaTrajeto <= limiteDistancia)
|
||
{
|
||
Visitado = true;
|
||
}
|
||
}
|
||
|
||
private void AtualizarOrientacaoAtual()
|
||
{
|
||
OrientacaoAtual = GPSUtils.CalcularOrientacao(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, Posicao);
|
||
}
|
||
|
||
private void AtualizarOrientacaoAnterior()
|
||
{
|
||
OrientacaoAnterior = GPSUtils.CalcularOrientacao(GPSService.PenultimaLeitura, Posicao);
|
||
}
|
||
|
||
private void AtualizarDistanciaTrajeto()
|
||
{
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
|
||
if (!op.Trajetoria._TrajetoriaFixaDefinida)
|
||
return;
|
||
|
||
var pontoAtual = op.Trajetoria.PontoAtual;
|
||
if (pontoAtual == null)
|
||
{
|
||
DistanciaTrajeto = DistanciaAtual;
|
||
return;
|
||
}
|
||
|
||
var primeiroNaoVisitado = op.Trajetoria._TrajetoriaFixa.FirstOrDefault(x => !x.Visitado);
|
||
if (primeiroNaoVisitado == null)
|
||
{
|
||
DistanciaTrajeto = 0;
|
||
return;
|
||
}
|
||
|
||
int idxPontoAtual = pontoAtual.idxPonto + 1;
|
||
int qtdPontos = Math.Abs(idxPonto - idxPontoAtual) + 1; // Sempre positivo
|
||
|
||
if (qtdPontos == 1)
|
||
{
|
||
//DistanciaTrajeto = pontoAtual.Aproximando ? pontoAtual.DistanciaAtual : -pontoAtual.DistanciaAtual;
|
||
//DistanciaTrajeto = GPSUtils.DistanciaEntrePontos(op.Trajetoria._TrajetoriaDinamica.First().Posicao, Posicao);
|
||
DistanciaTrajeto = GPSUtils.DistanciaEntrePontos(primeiroNaoVisitado.Posicao, Posicao);
|
||
return;
|
||
}
|
||
|
||
// ✅ Definimos corretamente o intervalo da trajetória
|
||
int idxInicio = Math.Min(idxPontoAtual, idxPonto);
|
||
int idxFim = Math.Max(idxPontoAtual, idxPonto);
|
||
|
||
// ✅ Pega os pontos corretos, independentemente de direção
|
||
List<GPSModel> trecho = op.Trajetoria._TrajetoriaFixa
|
||
.Skip(idxInicio)
|
||
.Take(qtdPontos)
|
||
.ToList()
|
||
.ConvertAll(x => x.Posicao);
|
||
|
||
double distancia = GPSUtils.DistanciaDoTrecho(trecho);
|
||
|
||
// ✅ Agora decidimos se devemos somar ou subtrair a distância do ponto atual
|
||
double distanciaP0 = pontoAtual.Aproximando ? pontoAtual.DistanciaAtual : -pontoAtual.DistanciaAtual;
|
||
|
||
// ✅ Se o ponto está atrás, invertemos a distância final
|
||
if (idxPonto < idxPontoAtual)
|
||
{
|
||
distanciaP0 *= -1;
|
||
distancia *= -1;
|
||
}
|
||
|
||
DistanciaTrajeto = distancia + distanciaP0;
|
||
}
|
||
|
||
private void AtualizarDistanciaAtual()
|
||
{
|
||
DistanciaAtual = GPSUtils.DistanciaEntrePontos(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, Posicao);
|
||
}
|
||
|
||
private void AtualizarDistanciaAnterior()
|
||
{
|
||
DistanciaAnterior = GPSUtils.DistanciaEntrePontos(GPSService.PenultimaLeitura, Posicao);
|
||
}
|
||
|
||
private void AtualizarAproximando()
|
||
{
|
||
// ✅ Agora verificamos tudo de uma vez sem variáveis intermediárias
|
||
Aproximando = Math.Abs(OrientacaoAtual - OrientacaoAnterior) <= 10.0
|
||
&& DistanciaAtual < DistanciaAnterior
|
||
&& Math.Abs(OrientacaoAtual - OrientacaoAnterior) < 90.0;
|
||
}
|
||
|
||
private void AtualizarNaMargem()
|
||
{
|
||
DistanciaMargem = LarguraCorredor * 0.8;
|
||
NaMargem = DistanciaAtual < DistanciaMargem;
|
||
}
|
||
|
||
|
||
public PontoTrajetoriaModel Clone()
|
||
{
|
||
return new PontoTrajetoriaModel(Tipo)
|
||
{
|
||
Aproximando = Aproximando,
|
||
Direcao = Direcao,
|
||
DistanciaAnterior = DistanciaAnterior,
|
||
DistanciaAtual = DistanciaAtual,
|
||
DistanciaTrajeto = DistanciaTrajeto,
|
||
idxCorredor = idxCorredor,
|
||
idxPonto = idxPonto,
|
||
idxPontoCorredor = idxPontoCorredor,
|
||
LarguraCorredor = LarguraCorredor,
|
||
NaMargem = NaMargem,
|
||
Orientacao = Orientacao,
|
||
OrientacaoAnterior = OrientacaoAnterior,
|
||
OrientacaoAtual = OrientacaoAtual,
|
||
PontoBorda = PontoBorda,
|
||
PontoLigacao = PontoLigacao,
|
||
Posicao = Posicao?.Clone() ?? new GPSModel(),
|
||
Visitado = Visitado
|
||
};
|
||
}
|
||
}
|
||
|
||
|
||
public enum AutonomiaCorredorStatus
|
||
{
|
||
Desconhecido = 0,
|
||
OkCompleto, // Bateria + herbicida suficientes pra entrar e pulverizar
|
||
OkSomenteTransito, // Bateria ok, herbicida insuficiente pra pulverizar tudo (mas pode atravessar)
|
||
OkSupervisionado, // Liberado manualmente MESMO com parâmetro não OK (override humano)
|
||
AguardandoLiberacaoManual, // Travado após ficar crítico por X segundos, só sai com liberação humana
|
||
Critico // Nem a bateria garante atravessar com segurança (ou herbicida mandatório insuficiente)
|
||
}
|
||
|
||
public class AutonomiaCorredorModel
|
||
{
|
||
[Flags]
|
||
private enum MotivoTrava
|
||
{
|
||
Nenhum = 0,
|
||
Bateria = 1,
|
||
Herbicida = 2,
|
||
}
|
||
|
||
private sealed class SnapshotCorredor
|
||
{
|
||
public int Indice { get; set; }
|
||
public bool Liberado { get; set; }
|
||
public AutonomiaCorredorStatus Status { get; set; }
|
||
|
||
public string Motivo { get; set; } = "";
|
||
public List<string> Motivos { get; set; } = new List<string>();
|
||
public List<string> Avisos { get; set; } = new List<string>();
|
||
|
||
public double DistanciaCorredor_m { get; set; }
|
||
public double DistanciaRequeridaBateria_m { get; set; }
|
||
public double DistanciaRequeridaHerbicida_m { get; set; }
|
||
|
||
public double DistanciaSeguraBateria_m { get; set; }
|
||
public double DistanciaSeguraHerbicida_m { get; set; }
|
||
|
||
public double FolgaBateria_m { get; set; }
|
||
public double FolgaHerbicida_m { get; set; }
|
||
|
||
public bool DadosBateriaValidos { get; set; }
|
||
public bool DadosHerbicidaValidos { get; set; }
|
||
|
||
public bool BateriaSuficiente { get; set; }
|
||
public bool HerbicidaSuficiente { get; set; }
|
||
|
||
public bool BateriaOverrideAplicado { get; set; }
|
||
public bool HerbicidaOverrideAplicado { get; set; }
|
||
|
||
public string FonteBateria { get; set; } = "";
|
||
public string FonteAutonomiaBateria { get; set; } = "";
|
||
public string FonteAutonomiaHerbicida { get; set; } = "";
|
||
|
||
public double PercentualBateria { get; set; }
|
||
public double PercentualReservatorio { get; set; }
|
||
|
||
public bool PulverizacaoExigida { get; set; }
|
||
public bool ConfirmadoParaEntrada { get; set; }
|
||
|
||
public DateTime TimestampUtc { get; set; }
|
||
}
|
||
|
||
// ============================================================
|
||
// Configuração
|
||
// ============================================================
|
||
|
||
private readonly TimeSpan _tempoParaTravar;
|
||
|
||
/*
|
||
* Os contadores de bateria e herbicida já possuem:
|
||
* - reserva mínima;
|
||
* - margem de segurança;
|
||
* - referências conservadoras por metro.
|
||
*
|
||
* Portanto, os extras ficam em zero por padrão para evitar
|
||
* aplicar margem dupla escondida.
|
||
*/
|
||
public double DistanciaExtraBateria_m { get; set; }
|
||
public double DistanciaExtraHerbicida_m { get; set; }
|
||
|
||
/*
|
||
* Evita oscilações por erro numérico de poucos centímetros.
|
||
* Não substitui a margem dos contadores.
|
||
*/
|
||
public double ToleranciaComparacao_m { get; set; }
|
||
|
||
public double TempoParaTravar_s => _tempoParaTravar.TotalSeconds;
|
||
|
||
public AutonomiaCorredorModel(double tempoParaTravar_s = 10.0, double distanciaExtraBateria_m = 0.0, double distanciaExtraHerbicida_m = 0.0, double toleranciaComparacao_m = 0.50)
|
||
{
|
||
_tempoParaTravar = TimeSpan.FromSeconds(Math.Max(0, tempoParaTravar_s));
|
||
DistanciaExtraBateria_m = Math.Max(0, distanciaExtraBateria_m);
|
||
DistanciaExtraHerbicida_m = Math.Max(0, distanciaExtraHerbicida_m);
|
||
ToleranciaComparacao_m = Math.Max(0, toleranciaComparacao_m);
|
||
}
|
||
|
||
// ============================================================
|
||
// Estado público
|
||
// ============================================================
|
||
|
||
[JsonProperty]
|
||
public bool Iniciado { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public AutonomiaCorredorStatus Status { get; private set; } = AutonomiaCorredorStatus.Desconhecido;
|
||
|
||
[JsonProperty]
|
||
public bool Liberado { get; private set; } = true;
|
||
|
||
[JsonProperty]
|
||
public int IndiceCorredor { get; private set; } = -1;
|
||
|
||
[JsonProperty]
|
||
public double DistanciaCorredor_m { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public double DistanciaRequeridaBateria_m { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public double DistanciaRequeridaHerbicida_m { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public double DistanciaSeguraBateria_m { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public double DistanciaSeguraHerbicida_m { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public double FolgaBateria_m { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public double FolgaHerbicida_m { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public bool DadosBateriaValidos { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public bool DadosHerbicidaValidos { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public bool BateriaSuficiente { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public bool HerbicidaSuficiente { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public bool PulverizacaoExigida { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public bool BateriaOverrideAplicado { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public bool HerbicidaOverrideAplicado { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public string FonteBateria { get; private set; } = "";
|
||
|
||
[JsonProperty]
|
||
public string FonteAutonomiaBateria { get; private set; } = "";
|
||
|
||
[JsonProperty]
|
||
public string FonteAutonomiaHerbicida { get; private set; } = "";
|
||
|
||
[JsonProperty]
|
||
public double PercentualBateria { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public double PercentualReservatorio { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public bool ConfirmadoParaEntrada { get; private set; }
|
||
|
||
[JsonProperty]
|
||
public string Motivo { get; private set; } = "";
|
||
|
||
[JsonProperty]
|
||
public List<string> Motivos { get; private set; } = new List<string>();
|
||
|
||
[JsonProperty]
|
||
public List<string> Avisos { get; private set; } = new List<string>();
|
||
|
||
// ============================================================
|
||
// Estado interno por corredor
|
||
// ============================================================
|
||
|
||
/*
|
||
* Histórico é atualizado continuamente.
|
||
* Um corredor só é congelado quando ConfirmarEntradaCorredor()
|
||
* é chamado no ponto real de entrada.
|
||
*/
|
||
private readonly Dictionary<int, SnapshotCorredor> _historicoCorredores = new Dictionary<int, SnapshotCorredor>();
|
||
|
||
private readonly HashSet<int> _corredoresConfirmados = new HashSet<int>();
|
||
|
||
/*
|
||
* Latch específico por motivo.
|
||
* Uma trava de bateria não pode ser reconhecida pelo botão
|
||
* de reservatório, e vice-versa.
|
||
*/
|
||
private readonly Dictionary<int, MotivoTrava> _travasPorCorredor = new Dictionary<int, MotivoTrava>();
|
||
|
||
private readonly Dictionary<int, DateTime> _inicioCriticoUtc = new Dictionary<int, DateTime>();
|
||
|
||
/*
|
||
* Overrides assumidos permanecem válidos somente para o corredor
|
||
* em que foram concedidos.
|
||
*/
|
||
private readonly Dictionary<int, MotivoTrava> _overridesPorCorredor = new Dictionary<int, MotivoTrava>();
|
||
|
||
/*
|
||
* Os bools recebidos por AtualizarDados são tratados como sinais.
|
||
* Apenas a borda false -> true gera um novo pulso de autorização.
|
||
* Isso evita que uma flag esquecida em true libere corredores futuros.
|
||
*/
|
||
private bool _ultimoSinalLiberacaoBateria;
|
||
private bool _ultimoSinalLiberacaoReservatorio;
|
||
|
||
private int _ultimoIndiceAvaliado = -1;
|
||
|
||
private readonly Dictionary<int, string> _ultimaAssinaturaLog = new Dictionary<int, string>();
|
||
|
||
// ============================================================
|
||
// Ciclo de vida
|
||
// ============================================================
|
||
|
||
public void Iniciar()
|
||
{
|
||
Reiniciar();
|
||
|
||
Iniciado = Variaveis.OperacaoEmAndamento?.Parametros?.ControleAutomatico ?? false;
|
||
|
||
/*
|
||
* Fora do automático, esta classe não impede movimento.
|
||
*/
|
||
Liberado = !Iniciado;
|
||
}
|
||
|
||
public void Parar()
|
||
{
|
||
Iniciado = false;
|
||
Liberado = true;
|
||
Status = AutonomiaCorredorStatus.Desconhecido;
|
||
}
|
||
|
||
// ============================================================
|
||
// Atualização principal
|
||
// ============================================================
|
||
|
||
public void AtualizarDados(int idxCorredor, bool bateria_liberada, bool reservatorio_liberado)
|
||
{
|
||
bool pulsoLiberacaoBateria = bateria_liberada && !_ultimoSinalLiberacaoBateria;
|
||
|
||
bool pulsoLiberacaoReservatorio = reservatorio_liberado && !_ultimoSinalLiberacaoReservatorio;
|
||
|
||
_ultimoSinalLiberacaoBateria = bateria_liberada;
|
||
|
||
_ultimoSinalLiberacaoReservatorio = reservatorio_liberado;
|
||
|
||
if (!Iniciado)
|
||
{
|
||
Liberado = true;
|
||
Status = AutonomiaCorredorStatus.Desconhecido;
|
||
Motivo = "Autonomia de corredor inativa.";
|
||
Motivos = new List<string>();
|
||
Avisos = new List<string>();
|
||
return;
|
||
}
|
||
|
||
var op = Variaveis.OperacaoEmAndamento;
|
||
|
||
if (op == null)
|
||
{
|
||
AplicarFalhaEstrutural(
|
||
idxCorredor,
|
||
"Operação em andamento indisponível."
|
||
);
|
||
return;
|
||
}
|
||
|
||
int quantidadeCorredores = op.Trajetoria?._Corredores?.Count ?? 0;
|
||
|
||
if (idxCorredor < 0 || idxCorredor >= quantidadeCorredores)
|
||
{
|
||
AplicarFalhaEstrutural(
|
||
idxCorredor,
|
||
$"Índice de corredor inválido: " +
|
||
$"{idxCorredor}/{quantidadeCorredores}."
|
||
);
|
||
return;
|
||
}
|
||
|
||
/*
|
||
* Depois que a trajetória confirma a entrada real, a autorização
|
||
* daquele corredor é congelada. Até essa confirmação, continua
|
||
* sendo reavaliada em tempo real.
|
||
*/
|
||
if (
|
||
_corredoresConfirmados.Contains(idxCorredor) &&
|
||
_historicoCorredores.TryGetValue(
|
||
idxCorredor,
|
||
out SnapshotCorredor confirmado
|
||
)
|
||
)
|
||
{
|
||
AplicarSnapshot(confirmado);
|
||
return;
|
||
}
|
||
|
||
double distanciaCorredor = op.Trajetoria._Corredores[idxCorredor]?.DistanciaTotal ?? 0;
|
||
|
||
if (!ValorFinito(distanciaCorredor) || distanciaCorredor <= 0)
|
||
{
|
||
AplicarFalhaEstrutural(
|
||
idxCorredor,
|
||
$"Corredor {idxCorredor + 1} possui " +
|
||
$"distância inválida: {distanciaCorredor:F2} m."
|
||
);
|
||
return;
|
||
}
|
||
|
||
IndiceCorredor = idxCorredor;
|
||
_ultimoIndiceAvaliado = idxCorredor;
|
||
|
||
DistanciaCorredor_m = distanciaCorredor;
|
||
|
||
DistanciaRequeridaBateria_m = Math.Max(0, distanciaCorredor + DistanciaExtraBateria_m);
|
||
|
||
DistanciaRequeridaHerbicida_m = Math.Max(0, distanciaCorredor + DistanciaExtraHerbicida_m);
|
||
|
||
var bateria = op.DispSen?.Dados?.ContadorCarga;
|
||
|
||
var reservatorio = op.DispAtu?.Dados?.ContadorCarga;
|
||
|
||
AtualizarDiagnosticoBateria(bateria);
|
||
|
||
AtualizarDiagnosticoHerbicida(reservatorio);
|
||
|
||
bool pulverizadorAutomatico = op.Parametros?.Controle?.PulverizadorAutomatico ?? false;
|
||
|
||
bool atuMandatorio =
|
||
op
|
||
.Parametros?
|
||
.ModulosMandatorios?
|
||
.Any(
|
||
x =>
|
||
x.Dispositivo == T_Code.Atu &&
|
||
x.Mandatorio &&
|
||
x.Utilizar
|
||
)
|
||
?? false;
|
||
|
||
PulverizacaoExigida =
|
||
pulverizadorAutomatico &&
|
||
atuMandatorio;
|
||
|
||
BateriaSuficiente =
|
||
DadosBateriaValidos &&
|
||
DistanciaSeguraBateria_m +
|
||
ToleranciaComparacao_m >=
|
||
DistanciaRequeridaBateria_m;
|
||
|
||
HerbicidaSuficiente =
|
||
DadosHerbicidaValidos &&
|
||
DistanciaSeguraHerbicida_m +
|
||
ToleranciaComparacao_m >=
|
||
DistanciaRequeridaHerbicida_m;
|
||
|
||
FolgaBateria_m =
|
||
DistanciaSeguraBateria_m -
|
||
DistanciaRequeridaBateria_m;
|
||
|
||
FolgaHerbicida_m =
|
||
DistanciaSeguraHerbicida_m -
|
||
DistanciaRequeridaHerbicida_m;
|
||
|
||
AtualizarAvisos(
|
||
bateria,
|
||
reservatorio
|
||
);
|
||
|
||
MotivoTrava criticosAtuais =
|
||
MotivoTrava.Nenhum;
|
||
|
||
var motivosCriticos =
|
||
new List<string>();
|
||
|
||
if (!BateriaSuficiente)
|
||
{
|
||
criticosAtuais |=
|
||
MotivoTrava.Bateria;
|
||
|
||
motivosCriticos.Add(
|
||
MontarMotivoBateria()
|
||
);
|
||
}
|
||
|
||
if (
|
||
PulverizacaoExigida &&
|
||
!HerbicidaSuficiente
|
||
)
|
||
{
|
||
criticosAtuais |=
|
||
MotivoTrava.Herbicida;
|
||
|
||
motivosCriticos.Add(
|
||
MontarMotivoHerbicida()
|
||
);
|
||
}
|
||
|
||
/*
|
||
* Guarda qualquer override já assumido neste corredor.
|
||
*/
|
||
MotivoTrava overrides =
|
||
ObterFlags(
|
||
_overridesPorCorredor,
|
||
idxCorredor
|
||
);
|
||
|
||
if (
|
||
pulsoLiberacaoBateria &&
|
||
(
|
||
criticosAtuais.HasFlag(
|
||
MotivoTrava.Bateria
|
||
) ||
|
||
ObterFlags(
|
||
_travasPorCorredor,
|
||
idxCorredor
|
||
).HasFlag(
|
||
MotivoTrava.Bateria
|
||
)
|
||
)
|
||
)
|
||
{
|
||
overrides |=
|
||
MotivoTrava.Bateria;
|
||
}
|
||
|
||
if (
|
||
pulsoLiberacaoReservatorio &&
|
||
(
|
||
criticosAtuais.HasFlag(
|
||
MotivoTrava.Herbicida
|
||
) ||
|
||
ObterFlags(
|
||
_travasPorCorredor,
|
||
idxCorredor
|
||
).HasFlag(
|
||
MotivoTrava.Herbicida
|
||
)
|
||
)
|
||
)
|
||
{
|
||
overrides |=
|
||
MotivoTrava.Herbicida;
|
||
}
|
||
|
||
if (overrides != MotivoTrava.Nenhum)
|
||
{
|
||
_overridesPorCorredor[idxCorredor] =
|
||
overrides;
|
||
}
|
||
|
||
BateriaOverrideAplicado =
|
||
overrides.HasFlag(
|
||
MotivoTrava.Bateria
|
||
);
|
||
|
||
HerbicidaOverrideAplicado =
|
||
overrides.HasFlag(
|
||
MotivoTrava.Herbicida
|
||
);
|
||
|
||
AtualizarLatch(
|
||
idxCorredor,
|
||
criticosAtuais
|
||
);
|
||
|
||
MotivoTrava travas =
|
||
ObterFlags(
|
||
_travasPorCorredor,
|
||
idxCorredor
|
||
);
|
||
|
||
bool bateriaResolvida =
|
||
!criticosAtuais.HasFlag(
|
||
MotivoTrava.Bateria
|
||
) ||
|
||
BateriaOverrideAplicado;
|
||
|
||
bool herbicidaResolvido =
|
||
!criticosAtuais.HasFlag(
|
||
MotivoTrava.Herbicida
|
||
) ||
|
||
HerbicidaOverrideAplicado;
|
||
|
||
bool criticosAtuaisResolvidos =
|
||
bateriaResolvida &&
|
||
herbicidaResolvido;
|
||
|
||
/*
|
||
* Quando a condição já se recuperou, uma trava persistente ainda
|
||
* exige reconhecimento específico do motivo que a originou.
|
||
*/
|
||
bool travaBateriaReconhecida =
|
||
!travas.HasFlag(
|
||
MotivoTrava.Bateria
|
||
) ||
|
||
BateriaOverrideAplicado;
|
||
|
||
bool travaHerbicidaReconhecida =
|
||
!travas.HasFlag(
|
||
MotivoTrava.Herbicida
|
||
) ||
|
||
HerbicidaOverrideAplicado;
|
||
|
||
bool travasReconhecidas =
|
||
travaBateriaReconhecida &&
|
||
travaHerbicidaReconhecida;
|
||
|
||
AutonomiaCorredorStatus statusFinal;
|
||
bool liberadoFinal;
|
||
string motivoFinal;
|
||
var motivosBloqueio =
|
||
new List<string>();
|
||
|
||
if (
|
||
criticosAtuais != MotivoTrava.Nenhum &&
|
||
!criticosAtuaisResolvidos
|
||
)
|
||
{
|
||
bool travado =
|
||
travas != MotivoTrava.Nenhum;
|
||
|
||
statusFinal =
|
||
travado
|
||
? AutonomiaCorredorStatus
|
||
.AguardandoLiberacaoManual
|
||
: AutonomiaCorredorStatus
|
||
.Critico;
|
||
|
||
liberadoFinal = false;
|
||
|
||
motivosBloqueio.AddRange(
|
||
motivosCriticos
|
||
);
|
||
|
||
motivoFinal =
|
||
$"Corredor {idxCorredor + 1}: " +
|
||
(
|
||
travado
|
||
? "BLOQUEADO após condição crítica persistente. "
|
||
: "autonomia crítica. "
|
||
) +
|
||
string.Join(
|
||
"; ",
|
||
motivosCriticos
|
||
);
|
||
|
||
if (travado)
|
||
{
|
||
motivoFinal +=
|
||
" Aguardando liberação específica " +
|
||
"do operador.";
|
||
}
|
||
}
|
||
else if (
|
||
travas != MotivoTrava.Nenhum &&
|
||
!travasReconhecidas
|
||
)
|
||
{
|
||
statusFinal =
|
||
AutonomiaCorredorStatus
|
||
.AguardandoLiberacaoManual;
|
||
|
||
liberadoFinal = false;
|
||
|
||
motivosBloqueio.Add(
|
||
$"Corredor {idxCorredor + 1}: " +
|
||
"a autonomia se recuperou, mas existe " +
|
||
"uma trava persistente aguardando " +
|
||
"reconhecimento do operador."
|
||
);
|
||
|
||
motivoFinal =
|
||
motivosBloqueio[0];
|
||
}
|
||
else
|
||
{
|
||
if (
|
||
travas != MotivoTrava.Nenhum &&
|
||
travasReconhecidas
|
||
)
|
||
{
|
||
DestravarCorredor(
|
||
idxCorredor
|
||
);
|
||
}
|
||
|
||
bool houveOverride =
|
||
BateriaOverrideAplicado ||
|
||
HerbicidaOverrideAplicado;
|
||
|
||
if (houveOverride)
|
||
{
|
||
statusFinal =
|
||
AutonomiaCorredorStatus
|
||
.OkSupervisionado;
|
||
|
||
motivoFinal =
|
||
MontarMotivoSupervisionado(
|
||
idxCorredor
|
||
);
|
||
}
|
||
else if (
|
||
pulverizadorAutomatico &&
|
||
HerbicidaSuficiente
|
||
)
|
||
{
|
||
statusFinal =
|
||
AutonomiaCorredorStatus
|
||
.OkCompleto;
|
||
|
||
motivoFinal =
|
||
$"Corredor {idxCorredor + 1}: " +
|
||
"bateria e herbicida suficientes.";
|
||
}
|
||
else
|
||
{
|
||
statusFinal =
|
||
AutonomiaCorredorStatus
|
||
.OkSomenteTransito;
|
||
|
||
motivoFinal =
|
||
$"Corredor {idxCorredor + 1}: " +
|
||
"bateria suficiente para atravessar. " +
|
||
(
|
||
pulverizadorAutomatico
|
||
? "Pulverização completa não é mandatória ou não está garantida."
|
||
: "Pulverizador automático desativado."
|
||
);
|
||
}
|
||
|
||
liberadoFinal = true;
|
||
}
|
||
|
||
Status = statusFinal;
|
||
Liberado = liberadoFinal;
|
||
Motivo = motivoFinal;
|
||
|
||
Motivos =
|
||
motivosBloqueio
|
||
.Where(
|
||
x =>
|
||
!string.IsNullOrWhiteSpace(x)
|
||
)
|
||
.Distinct()
|
||
.ToList();
|
||
|
||
ConfirmadoParaEntrada = false;
|
||
|
||
SnapshotCorredor snapshot =
|
||
CriarSnapshot(
|
||
idxCorredor
|
||
);
|
||
|
||
_historicoCorredores[idxCorredor] =
|
||
snapshot;
|
||
|
||
LogarSeMudou(
|
||
op,
|
||
snapshot
|
||
);
|
||
}
|
||
|
||
// ============================================================
|
||
// Diagnóstico dos contadores revisados
|
||
// ============================================================
|
||
|
||
private void AtualizarDiagnosticoBateria(CoulombCounterService bateria)
|
||
{
|
||
DistanciaSeguraBateria_m = 0;
|
||
DadosBateriaValidos = false;
|
||
|
||
FonteBateria = "";
|
||
FonteAutonomiaBateria = "";
|
||
PercentualBateria = 0;
|
||
|
||
if (bateria == null)
|
||
return;
|
||
|
||
try
|
||
{
|
||
DistanciaSeguraBateria_m =
|
||
NumeroNaoNegativo(
|
||
(double)
|
||
bateria
|
||
.DistanciaEstimadaRestante_m
|
||
);
|
||
|
||
FonteBateria =
|
||
Convert.ToString(
|
||
bateria.FonteAtiva
|
||
)
|
||
?? "";
|
||
|
||
FonteAutonomiaBateria =
|
||
Convert.ToString(
|
||
bateria.FonteAutonomia
|
||
)
|
||
?? "";
|
||
|
||
PercentualBateria =
|
||
Clamp(
|
||
(double)
|
||
bateria.PercentualAtual,
|
||
0,
|
||
100
|
||
);
|
||
|
||
double whPorMetroReferencia =
|
||
NumeroNaoNegativo(
|
||
(double)
|
||
bateria.WhPorMetroReferencia
|
||
);
|
||
|
||
DadosBateriaValidos =
|
||
(bool)bateria.Iniciado &&
|
||
!string.IsNullOrWhiteSpace(
|
||
FonteBateria
|
||
) &&
|
||
!string.Equals(
|
||
FonteBateria,
|
||
"nenhuma",
|
||
StringComparison.OrdinalIgnoreCase
|
||
) &&
|
||
whPorMetroReferencia > 0 &&
|
||
ValorFinito(
|
||
DistanciaSeguraBateria_m
|
||
);
|
||
}
|
||
catch
|
||
{
|
||
DadosBateriaValidos = false;
|
||
}
|
||
}
|
||
|
||
private void AtualizarDiagnosticoHerbicida(HerbicideCounterService reservatorio)
|
||
{
|
||
DistanciaSeguraHerbicida_m = 0;
|
||
DadosHerbicidaValidos = false;
|
||
|
||
FonteAutonomiaHerbicida = "";
|
||
PercentualReservatorio = 0;
|
||
|
||
if (reservatorio == null)
|
||
return;
|
||
|
||
try
|
||
{
|
||
DistanciaSeguraHerbicida_m =
|
||
NumeroNaoNegativo(
|
||
(double)
|
||
reservatorio
|
||
.DistanciaEstimadaRestantePulverizando_m
|
||
);
|
||
|
||
FonteAutonomiaHerbicida =
|
||
Convert.ToString(
|
||
reservatorio
|
||
.FonteAutonomiaOperacional
|
||
)
|
||
?? "";
|
||
|
||
PercentualReservatorio =
|
||
Clamp(
|
||
(double)
|
||
reservatorio.PercentualAtual,
|
||
0,
|
||
100
|
||
);
|
||
|
||
double lPorMetroReferencia =
|
||
NumeroNaoNegativo(
|
||
(double)
|
||
reservatorio
|
||
.LPorMetroReferenciaOperacional
|
||
);
|
||
|
||
DadosHerbicidaValidos =
|
||
(bool)reservatorio.Iniciado &&
|
||
(bool)reservatorio
|
||
.PulverizadorLiberado &&
|
||
lPorMetroReferencia > 0 &&
|
||
ValorFinito(
|
||
DistanciaSeguraHerbicida_m
|
||
);
|
||
}
|
||
catch
|
||
{
|
||
DadosHerbicidaValidos = false;
|
||
}
|
||
}
|
||
|
||
private void AtualizarAvisos(CoulombCounterService bateria, HerbicideCounterService reservatorio)
|
||
{
|
||
var avisos =
|
||
new List<string>();
|
||
|
||
if (!DadosBateriaValidos)
|
||
{
|
||
avisos.Add(
|
||
"Autonomia de bateria indisponível ou inválida."
|
||
);
|
||
}
|
||
else
|
||
{
|
||
try
|
||
{
|
||
if (!(bool)bateria.FontesCoerentes)
|
||
{
|
||
avisos.Add(
|
||
$"Daly e AB36V divergentes: " +
|
||
$"ΔV={(double)bateria.DiferencaTensaoFontes_V:F2} V, " +
|
||
$"ΔI={(double)bateria.DiferencaCorrenteFontes_A:F2} A."
|
||
);
|
||
}
|
||
|
||
if (!(bool)bateria.AutonomiaComHistorico)
|
||
{
|
||
avisos.Add(
|
||
$"Bateria ainda usando referência inicial: " +
|
||
$"{FonteAutonomiaBateria}."
|
||
);
|
||
}
|
||
}
|
||
catch
|
||
{
|
||
// Campos diagnósticos opcionais.
|
||
}
|
||
}
|
||
|
||
if (
|
||
PulverizacaoExigida &&
|
||
!DadosHerbicidaValidos
|
||
)
|
||
{
|
||
avisos.Add(
|
||
"Autonomia operacional de herbicida indisponível ou inválida."
|
||
);
|
||
}
|
||
else if (DadosHerbicidaValidos)
|
||
{
|
||
try
|
||
{
|
||
if (
|
||
!(bool)reservatorio
|
||
.AutonomiaOperacionalComHistorico
|
||
)
|
||
{
|
||
avisos.Add(
|
||
$"Herbicida ainda usando referência inicial: " +
|
||
$"{FonteAutonomiaHerbicida}."
|
||
);
|
||
}
|
||
|
||
if (
|
||
!(bool)reservatorio
|
||
.LeituraReservatorioConfiavel
|
||
)
|
||
{
|
||
avisos.Add(
|
||
"Leitura instantânea do reservatório rejeitada; " +
|
||
"mantendo último volume filtrado."
|
||
);
|
||
}
|
||
}
|
||
catch
|
||
{
|
||
// Campos diagnósticos opcionais.
|
||
}
|
||
}
|
||
|
||
Avisos =
|
||
avisos
|
||
.Where(
|
||
x =>
|
||
!string.IsNullOrWhiteSpace(x)
|
||
)
|
||
.Distinct()
|
||
.ToList();
|
||
}
|
||
|
||
// ============================================================
|
||
// Latch e overrides
|
||
// ============================================================
|
||
|
||
private void AtualizarLatch(int idxCorredor, MotivoTrava criticosAtuais)
|
||
{
|
||
if (criticosAtuais == MotivoTrava.Nenhum)
|
||
{
|
||
_inicioCriticoUtc.Remove(
|
||
idxCorredor
|
||
);
|
||
return;
|
||
}
|
||
|
||
if (
|
||
!_inicioCriticoUtc.ContainsKey(
|
||
idxCorredor
|
||
)
|
||
)
|
||
{
|
||
_inicioCriticoUtc[idxCorredor] =
|
||
DateTime.UtcNow;
|
||
}
|
||
|
||
TimeSpan tempoCritico =
|
||
DateTime.UtcNow -
|
||
_inicioCriticoUtc[idxCorredor];
|
||
|
||
if (
|
||
tempoCritico >=
|
||
_tempoParaTravar
|
||
)
|
||
{
|
||
MotivoTrava travas =
|
||
ObterFlags(
|
||
_travasPorCorredor,
|
||
idxCorredor
|
||
);
|
||
|
||
_travasPorCorredor[idxCorredor] =
|
||
travas |
|
||
criticosAtuais;
|
||
}
|
||
}
|
||
|
||
private void DestravarCorredor(int idxCorredor)
|
||
{
|
||
_travasPorCorredor.Remove(
|
||
idxCorredor
|
||
);
|
||
|
||
_inicioCriticoUtc.Remove(
|
||
idxCorredor
|
||
);
|
||
}
|
||
|
||
private static MotivoTrava ObterFlags(Dictionary<int, MotivoTrava> origem, int idxCorredor)
|
||
{
|
||
return
|
||
origem.TryGetValue(
|
||
idxCorredor,
|
||
out MotivoTrava flags
|
||
)
|
||
? flags
|
||
: MotivoTrava.Nenhum;
|
||
}
|
||
|
||
// ============================================================
|
||
// Confirmação da entrada
|
||
// ============================================================
|
||
|
||
/*
|
||
* Chamar exatamente quando a trajetória efetivamente inicia a entrada
|
||
* no corredor. Antes disso a autonomia continua sendo reavaliada.
|
||
*
|
||
* Retorna false se o corredor não estiver liberado.
|
||
*/
|
||
public bool ConfirmarEntradaCorredor(int idxCorredor)
|
||
{
|
||
if (
|
||
idxCorredor !=
|
||
_ultimoIndiceAvaliado ||
|
||
!Liberado ||
|
||
!_historicoCorredores.TryGetValue(
|
||
idxCorredor,
|
||
out SnapshotCorredor snapshot
|
||
)
|
||
)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
snapshot.ConfirmadoParaEntrada =
|
||
true;
|
||
|
||
_historicoCorredores[idxCorredor] =
|
||
snapshot;
|
||
|
||
_corredoresConfirmados.Add(
|
||
idxCorredor
|
||
);
|
||
|
||
ConfirmadoParaEntrada = true;
|
||
|
||
return true;
|
||
}
|
||
|
||
public bool CorredorEstaConfirmado(int idxCorredor)
|
||
{
|
||
return
|
||
_corredoresConfirmados.Contains(
|
||
idxCorredor
|
||
);
|
||
}
|
||
|
||
// ============================================================
|
||
// Mensagens
|
||
// ============================================================
|
||
|
||
private string MontarMotivoBateria()
|
||
{
|
||
if (!DadosBateriaValidos)
|
||
{
|
||
return
|
||
"Bateria sem autonomia válida para garantir " +
|
||
"a conclusão do corredor.";
|
||
}
|
||
|
||
return
|
||
$"Bateria insuficiente: segura=" +
|
||
$"{DistanciaSeguraBateria_m:F1} m, " +
|
||
$"requerida={DistanciaRequeridaBateria_m:F1} m, " +
|
||
$"folga={FolgaBateria_m:F1} m, " +
|
||
$"SOC={PercentualBateria:F1}%, " +
|
||
$"fonte={FonteBateria}/{FonteAutonomiaBateria}.";
|
||
}
|
||
|
||
private string MontarMotivoHerbicida()
|
||
{
|
||
if (!DadosHerbicidaValidos)
|
||
{
|
||
return
|
||
"Herbicida sem autonomia operacional válida " +
|
||
"para garantir a pulverização completa.";
|
||
}
|
||
|
||
return
|
||
$"Herbicida insuficiente: seguro=" +
|
||
$"{DistanciaSeguraHerbicida_m:F1} m, " +
|
||
$"requerido={DistanciaRequeridaHerbicida_m:F1} m, " +
|
||
$"folga={FolgaHerbicida_m:F1} m, " +
|
||
$"reservatório={PercentualReservatorio:F1}%, " +
|
||
$"fonte={FonteAutonomiaHerbicida}.";
|
||
}
|
||
|
||
private string MontarMotivoSupervisionado(int idxCorredor)
|
||
{
|
||
var partes =
|
||
new List<string>();
|
||
|
||
if (BateriaOverrideAplicado)
|
||
{
|
||
partes.Add(
|
||
"bateria assumida pelo operador"
|
||
);
|
||
}
|
||
|
||
if (HerbicidaOverrideAplicado)
|
||
{
|
||
partes.Add(
|
||
"herbicida assumido pelo operador"
|
||
);
|
||
}
|
||
|
||
return
|
||
$"Corredor {idxCorredor + 1}: " +
|
||
"LIBERAÇÃO SUPERVISIONADA. " +
|
||
string.Join(
|
||
"; ",
|
||
partes
|
||
) +
|
||
".";
|
||
}
|
||
|
||
// ============================================================
|
||
// Snapshot e logs
|
||
// ============================================================
|
||
|
||
private SnapshotCorredor CriarSnapshot(int idxCorredor)
|
||
{
|
||
return
|
||
new SnapshotCorredor
|
||
{
|
||
Indice =
|
||
idxCorredor,
|
||
|
||
Liberado =
|
||
Liberado,
|
||
|
||
Status =
|
||
Status,
|
||
|
||
Motivo =
|
||
Motivo,
|
||
|
||
Motivos =
|
||
new List<string>(
|
||
Motivos ??
|
||
new List<string>()
|
||
),
|
||
|
||
Avisos =
|
||
new List<string>(
|
||
Avisos ??
|
||
new List<string>()
|
||
),
|
||
|
||
DistanciaCorredor_m =
|
||
DistanciaCorredor_m,
|
||
|
||
DistanciaRequeridaBateria_m =
|
||
DistanciaRequeridaBateria_m,
|
||
|
||
DistanciaRequeridaHerbicida_m =
|
||
DistanciaRequeridaHerbicida_m,
|
||
|
||
DistanciaSeguraBateria_m =
|
||
DistanciaSeguraBateria_m,
|
||
|
||
DistanciaSeguraHerbicida_m =
|
||
DistanciaSeguraHerbicida_m,
|
||
|
||
FolgaBateria_m =
|
||
FolgaBateria_m,
|
||
|
||
FolgaHerbicida_m =
|
||
FolgaHerbicida_m,
|
||
|
||
DadosBateriaValidos =
|
||
DadosBateriaValidos,
|
||
|
||
DadosHerbicidaValidos =
|
||
DadosHerbicidaValidos,
|
||
|
||
BateriaSuficiente =
|
||
BateriaSuficiente,
|
||
|
||
HerbicidaSuficiente =
|
||
HerbicidaSuficiente,
|
||
|
||
BateriaOverrideAplicado =
|
||
BateriaOverrideAplicado,
|
||
|
||
HerbicidaOverrideAplicado =
|
||
HerbicidaOverrideAplicado,
|
||
|
||
FonteBateria =
|
||
FonteBateria,
|
||
|
||
FonteAutonomiaBateria =
|
||
FonteAutonomiaBateria,
|
||
|
||
FonteAutonomiaHerbicida =
|
||
FonteAutonomiaHerbicida,
|
||
|
||
PercentualBateria =
|
||
PercentualBateria,
|
||
|
||
PercentualReservatorio =
|
||
PercentualReservatorio,
|
||
|
||
PulverizacaoExigida =
|
||
PulverizacaoExigida,
|
||
|
||
ConfirmadoParaEntrada =
|
||
ConfirmadoParaEntrada,
|
||
|
||
TimestampUtc =
|
||
DateTime.UtcNow,
|
||
};
|
||
}
|
||
|
||
private void AplicarSnapshot(SnapshotCorredor snapshot)
|
||
{
|
||
if (snapshot == null)
|
||
return;
|
||
|
||
IndiceCorredor =
|
||
snapshot.Indice;
|
||
|
||
Liberado =
|
||
snapshot.Liberado;
|
||
|
||
Status =
|
||
snapshot.Status;
|
||
|
||
Motivo =
|
||
snapshot.Motivo;
|
||
|
||
Motivos =
|
||
new List<string>(
|
||
snapshot.Motivos ??
|
||
new List<string>()
|
||
);
|
||
|
||
Avisos =
|
||
new List<string>(
|
||
snapshot.Avisos ??
|
||
new List<string>()
|
||
);
|
||
|
||
DistanciaCorredor_m =
|
||
snapshot.DistanciaCorredor_m;
|
||
|
||
DistanciaRequeridaBateria_m =
|
||
snapshot.DistanciaRequeridaBateria_m;
|
||
|
||
DistanciaRequeridaHerbicida_m =
|
||
snapshot.DistanciaRequeridaHerbicida_m;
|
||
|
||
DistanciaSeguraBateria_m =
|
||
snapshot.DistanciaSeguraBateria_m;
|
||
|
||
DistanciaSeguraHerbicida_m =
|
||
snapshot.DistanciaSeguraHerbicida_m;
|
||
|
||
FolgaBateria_m =
|
||
snapshot.FolgaBateria_m;
|
||
|
||
FolgaHerbicida_m =
|
||
snapshot.FolgaHerbicida_m;
|
||
|
||
DadosBateriaValidos =
|
||
snapshot.DadosBateriaValidos;
|
||
|
||
DadosHerbicidaValidos =
|
||
snapshot.DadosHerbicidaValidos;
|
||
|
||
BateriaSuficiente =
|
||
snapshot.BateriaSuficiente;
|
||
|
||
HerbicidaSuficiente =
|
||
snapshot.HerbicidaSuficiente;
|
||
|
||
BateriaOverrideAplicado =
|
||
snapshot.BateriaOverrideAplicado;
|
||
|
||
HerbicidaOverrideAplicado =
|
||
snapshot.HerbicidaOverrideAplicado;
|
||
|
||
FonteBateria =
|
||
snapshot.FonteBateria;
|
||
|
||
FonteAutonomiaBateria =
|
||
snapshot.FonteAutonomiaBateria;
|
||
|
||
FonteAutonomiaHerbicida =
|
||
snapshot.FonteAutonomiaHerbicida;
|
||
|
||
PercentualBateria =
|
||
snapshot.PercentualBateria;
|
||
|
||
PercentualReservatorio =
|
||
snapshot.PercentualReservatorio;
|
||
|
||
PulverizacaoExigida =
|
||
snapshot.PulverizacaoExigida;
|
||
|
||
ConfirmadoParaEntrada =
|
||
snapshot.ConfirmadoParaEntrada;
|
||
}
|
||
|
||
private void LogarSeMudou(OperacaoModel op, SnapshotCorredor snapshot)
|
||
{
|
||
if (
|
||
op == null ||
|
||
snapshot == null
|
||
)
|
||
{
|
||
return;
|
||
}
|
||
|
||
string assinatura =
|
||
$"{snapshot.Status}|" +
|
||
$"{snapshot.Liberado}|" +
|
||
$"{snapshot.Motivo}";
|
||
|
||
if (
|
||
_ultimaAssinaturaLog.TryGetValue(
|
||
snapshot.Indice,
|
||
out string anterior
|
||
) &&
|
||
string.Equals(
|
||
anterior,
|
||
assinatura,
|
||
StringComparison.Ordinal
|
||
)
|
||
)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_ultimaAssinaturaLog[snapshot.Indice] =
|
||
assinatura;
|
||
|
||
try
|
||
{
|
||
op.Sensoriamento.InserirLog(
|
||
T_Code.Trj,
|
||
snapshot.Liberado
|
||
? StatusModulo.Operante
|
||
: StatusModulo.Alerta,
|
||
snapshot.Liberado
|
||
? 100
|
||
: 0,
|
||
snapshot.Motivo
|
||
);
|
||
}
|
||
catch
|
||
{
|
||
// Log nunca deve derrubar a decisão de autonomia.
|
||
}
|
||
}
|
||
|
||
private void AplicarFalhaEstrutural(int idxCorredor, string motivo)
|
||
{
|
||
IndiceCorredor =
|
||
idxCorredor;
|
||
|
||
Liberado = false;
|
||
Status =
|
||
AutonomiaCorredorStatus.Critico;
|
||
|
||
Motivo =
|
||
motivo ?? "Falha estrutural na autonomia.";
|
||
|
||
Motivos =
|
||
new List<string>
|
||
{
|
||
Motivo
|
||
};
|
||
|
||
Avisos =
|
||
new List<string>();
|
||
|
||
ConfirmadoParaEntrada = false;
|
||
|
||
DistanciaCorredor_m = 0;
|
||
DistanciaRequeridaBateria_m = 0;
|
||
DistanciaRequeridaHerbicida_m = 0;
|
||
|
||
DistanciaSeguraBateria_m = 0;
|
||
DistanciaSeguraHerbicida_m = 0;
|
||
|
||
FolgaBateria_m = 0;
|
||
FolgaHerbicida_m = 0;
|
||
|
||
DadosBateriaValidos = false;
|
||
DadosHerbicidaValidos = false;
|
||
|
||
BateriaSuficiente = false;
|
||
HerbicidaSuficiente = false;
|
||
}
|
||
|
||
// ============================================================
|
||
// Reinício e clone
|
||
// ============================================================
|
||
|
||
public void Reiniciar()
|
||
{
|
||
Iniciado = false;
|
||
|
||
Status =
|
||
AutonomiaCorredorStatus.Desconhecido;
|
||
|
||
Liberado = false;
|
||
|
||
IndiceCorredor = -1;
|
||
|
||
DistanciaCorredor_m = 0;
|
||
DistanciaRequeridaBateria_m = 0;
|
||
DistanciaRequeridaHerbicida_m = 0;
|
||
|
||
DistanciaSeguraBateria_m = 0;
|
||
DistanciaSeguraHerbicida_m = 0;
|
||
|
||
FolgaBateria_m = 0;
|
||
FolgaHerbicida_m = 0;
|
||
|
||
DadosBateriaValidos = false;
|
||
DadosHerbicidaValidos = false;
|
||
|
||
BateriaSuficiente = false;
|
||
HerbicidaSuficiente = false;
|
||
|
||
PulverizacaoExigida = false;
|
||
|
||
BateriaOverrideAplicado = false;
|
||
HerbicidaOverrideAplicado = false;
|
||
|
||
FonteBateria = "";
|
||
FonteAutonomiaBateria = "";
|
||
FonteAutonomiaHerbicida = "";
|
||
|
||
PercentualBateria = 0;
|
||
PercentualReservatorio = 0;
|
||
|
||
ConfirmadoParaEntrada = false;
|
||
|
||
Motivo = "";
|
||
Motivos = new List<string>();
|
||
Avisos = new List<string>();
|
||
|
||
_historicoCorredores.Clear();
|
||
_corredoresConfirmados.Clear();
|
||
|
||
_travasPorCorredor.Clear();
|
||
_inicioCriticoUtc.Clear();
|
||
_overridesPorCorredor.Clear();
|
||
|
||
_ultimaAssinaturaLog.Clear();
|
||
|
||
_ultimoSinalLiberacaoBateria = false;
|
||
_ultimoSinalLiberacaoReservatorio = false;
|
||
|
||
_ultimoIndiceAvaliado = -1;
|
||
}
|
||
|
||
public AutonomiaCorredorModel Clone()
|
||
{
|
||
return
|
||
new AutonomiaCorredorModel(
|
||
tempoParaTravar_s:
|
||
TempoParaTravar_s,
|
||
|
||
distanciaExtraBateria_m:
|
||
DistanciaExtraBateria_m,
|
||
|
||
distanciaExtraHerbicida_m:
|
||
DistanciaExtraHerbicida_m,
|
||
|
||
toleranciaComparacao_m:
|
||
ToleranciaComparacao_m
|
||
)
|
||
{
|
||
Iniciado =
|
||
Iniciado,
|
||
|
||
Status =
|
||
Status,
|
||
|
||
Liberado =
|
||
Liberado,
|
||
|
||
IndiceCorredor =
|
||
IndiceCorredor,
|
||
|
||
DistanciaCorredor_m =
|
||
DistanciaCorredor_m,
|
||
|
||
DistanciaRequeridaBateria_m =
|
||
DistanciaRequeridaBateria_m,
|
||
|
||
DistanciaRequeridaHerbicida_m =
|
||
DistanciaRequeridaHerbicida_m,
|
||
|
||
DistanciaSeguraBateria_m =
|
||
DistanciaSeguraBateria_m,
|
||
|
||
DistanciaSeguraHerbicida_m =
|
||
DistanciaSeguraHerbicida_m,
|
||
|
||
FolgaBateria_m =
|
||
FolgaBateria_m,
|
||
|
||
FolgaHerbicida_m =
|
||
FolgaHerbicida_m,
|
||
|
||
DadosBateriaValidos =
|
||
DadosBateriaValidos,
|
||
|
||
DadosHerbicidaValidos =
|
||
DadosHerbicidaValidos,
|
||
|
||
BateriaSuficiente =
|
||
BateriaSuficiente,
|
||
|
||
HerbicidaSuficiente =
|
||
HerbicidaSuficiente,
|
||
|
||
PulverizacaoExigida =
|
||
PulverizacaoExigida,
|
||
|
||
BateriaOverrideAplicado =
|
||
BateriaOverrideAplicado,
|
||
|
||
HerbicidaOverrideAplicado =
|
||
HerbicidaOverrideAplicado,
|
||
|
||
FonteBateria =
|
||
FonteBateria,
|
||
|
||
FonteAutonomiaBateria =
|
||
FonteAutonomiaBateria,
|
||
|
||
FonteAutonomiaHerbicida =
|
||
FonteAutonomiaHerbicida,
|
||
|
||
PercentualBateria =
|
||
PercentualBateria,
|
||
|
||
PercentualReservatorio =
|
||
PercentualReservatorio,
|
||
|
||
ConfirmadoParaEntrada =
|
||
ConfirmadoParaEntrada,
|
||
|
||
Motivo =
|
||
Motivo,
|
||
|
||
Motivos =
|
||
new List<string>(
|
||
Motivos ??
|
||
new List<string>()
|
||
),
|
||
|
||
Avisos =
|
||
new List<string>(
|
||
Avisos ??
|
||
new List<string>()
|
||
),
|
||
};
|
||
}
|
||
|
||
// ============================================================
|
||
// Helpers
|
||
// ============================================================
|
||
|
||
private static double NumeroNaoNegativo(double valor)
|
||
{
|
||
if (!ValorFinito(valor))
|
||
return 0;
|
||
|
||
return Math.Max(0, valor);
|
||
}
|
||
|
||
private static bool ValorFinito(double valor)
|
||
{
|
||
return
|
||
!double.IsNaN(valor) &&
|
||
!double.IsInfinity(valor);
|
||
}
|
||
|
||
private static double Clamp(double valor, double minimo, double maximo)
|
||
{
|
||
return Math.Max(minimo, Math.Min(maximo, valor));
|
||
}
|
||
}
|
||
|
||
}
|