diff --git a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs index ad4ed3ed0..e64f45e90 100644 --- a/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs +++ b/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs @@ -773,7 +773,7 @@ namespace AgroBase.Models { if (CorredorAtual == null) return; - bool modoSimulador = false && Variaveis.OperacaoEmAndamento.Simulando; + bool modoSimulador = true && Variaveis.OperacaoEmAndamento.Simulando; // Limiar de "margem" para considerar proximidade do fim double limiarDistanciaMargem = DistanciaErroMapaPlantacao > -1 ? DistanciaErroMapaPlantacao : 5.0; @@ -880,69 +880,74 @@ namespace AgroBase.Models bool podeUsarSegmentacao = modoSimulador || (dadoFresco && dv.CorredorConfirmado); - bool abrirCurva = false; // mantém seu flag por enquanto + bool fimAntecipado = podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente; - if (podeUsarSegmentacao && pertoDoFim && foraDoCorredorAgora && foraContinuoSuficiente) + Console.WriteLine($"podeUsarSegmentacao: {podeUsarSegmentacao}, pertoDoFim: {pertoDoFim}, foraDoCorredorAgora: {foraDoCorredorAgora}, foraContinuoSuficiente: {foraContinuoSuficiente}"); + + if (fimAntecipado) { - // 4) Aqui consideramos que o corredor acabou antes do fim do mapa - double anguloControle = Variaveis.OperacaoEmAndamento.Controle.Angulo; - - if (abrirCurva && !CorredorAtual.Ultimo) - { - anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda - ? -Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo - : Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo; - } - - int pontosMarcar = 0; - foreach (var ponto in CorredorAtual.Pontos.Where(x => !x.Visitado)) - { - ponto.Visitado = true; - pontosMarcar++; - } + List pontosRemover = CorredorAtual.Pontos.Where(x => !x.Visitado).ToList(); + pontosRemover.ForEach(x => x.Visitado = true); if (!CorredorAtual.Ultimo) { - // Marca ponto de ligação de entrada do próximo corredor como visitado - _TrajetoriaFixa[CorredorAtual.Pontos[CorredorAtual.QtdPontos - 1].idxPonto + 1].Visitado = true; + foreach (var ponto in pontosRemover) + { + CorredorAtual.Pontos.Remove(ponto); + _TrajetoriaFixa.Remove(ponto); + } - // Avança bem a janela para "pular" o restinho do corredor - AtualizarIndicesJanela((pontosMarcar + 2) * 2); - AtualizarTrajetoriaJanela(); - DefinirPontoAtual(); - AtualizarCorredorAtual(); + if (CorredorAtual.Pontos.Count >= 2) + { + var pls = CorredorAtual.Pontos[CorredorAtual.Pontos.Count - 1]; + 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); + } + + double anguloControle = CorredorAtual.idxRuaDireita > CorredorAtual.idxRuaEsquerda ? -Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo : Variaveis.OperacaoEmAndamento.Parametros.Controle.DirAnguloMaximo; + double anguloDesvio = PontoAtual.Orientacao + anguloControle; + var pontoDesvio = GPSUtils.GerarPontoDeslocado(PontoAtual.Posicao, anguloDesvio, 1.5); + IncluirDesvioNaTrajetoria(new List() { pontoDesvio }, PontoAtual.idxPonto); // Garante que os primeiros pontos do corredor atual estão como visitados - for (int i = 0; i < pontosMarcar && i < CorredorAtual.Pontos.Count; i++) + var pc = _Corredores[CorredorAtual.Idx + 1]; + + // acha o ponto mais próximo + (GPSModel pp, double distanciaAtual) = GPSUtils.CalcularPontoMaisProximoTrajetoria(Variaveis.OperacaoEmAndamento.Sensoriamento.Gps, 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); + if (_pp != null) { - CorredorAtual.Pontos[i].Visitado = true; - } + int idxProx = _pp.idxPontoCorredor; - AtualizarIndicesJanela(); - AtualizarTrajetoriaJanela(); - DefinirPontoAtual(); - DefinirProximoPonto(); + // 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)); - if (abrirCurva) - { - var pControle = Variaveis.OperacaoEmAndamento.Parametros.Controle; + var pontosRemoverProx = pc.Pontos.Take(qtdRemoverProx).ToList(); - if (pControle.MovimentoAutomatico) + foreach (var p in pontosRemoverProx) { - Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP = Variaveis.OperacaoEmAndamento.Parametros.Controle.MovVelocidadeCErvasPercent; - Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DelayEnvioComandoExtra = 2000; - Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List() { T_Code.Mov }); + pc.Pontos.Remove(p); + _TrajetoriaFixa.Remove(p); } - if (pControle.DirecionalAutomatico) + if (pc.Pontos.Count >= 2) { - Console.WriteLine($"Angulo de controle definido de {Variaveis.OperacaoEmAndamento.Controle.Angulo} para {anguloControle}"); - Variaveis.OperacaoEmAndamento.Controle.Angulo = anguloControle; - Variaveis.OperacaoEmAndamento.Controle.TipoMovimento = TipoMovimentoDirecional.MovimentoArco; - Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Dir).DelayEnvioComandoExtra = 1500; - Variaveis.OperacaoEmAndamento.AtualizarDadosControle(false, new List() { T_Code.Dir }); + 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(); + + RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false)); + Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados(); } } } @@ -2300,6 +2305,77 @@ namespace AgroBase.Models #endregion + + private void IncluirDesvioNaTrajetoria(List pontos, int idxPontoApartir) + { + 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(); + 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 = pa.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 @@ -2426,7 +2502,7 @@ namespace AgroBase.Models public GPSModel Posicao { get; set; } public double Orientacao { get; set; } public DirecaoCarroRua Direcao { get; set; } - public TipoPontoRua Tipo { get; } + public TipoPontoRua Tipo { private set; get; } public double LarguraCorredor { get; set; } public double DistanciaMargem { get; private set; } public bool Visitado { get; set; } = false; @@ -2460,6 +2536,11 @@ namespace AgroBase.Models PontoLigacao = Tipo == Enums.TipoPontoRua.LigacaoEntrada || Tipo == Enums.TipoPontoRua.LigacaoSaida; } + public void AlterarTipo(Enums.TipoPontoRua novo) + { + Tipo = novo; + } + public void AtualizarPropriedades() { AtualizarDistanciaAtual();