agrobot_base/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs

using AgroBase.Services;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Linq;
using static AgroBase.Models.Enums;

namespace AgroBase.Models
{
    public class TrajetoriaMapaOperacaoModel
    {
        public TrajetoriaMapaOperacaoModel(List<List<GPSModel>> RuasMapa)
        {
            RuasPlantacao = new List<List<GPSModel>>(RuasMapa);
        }

        #region PARAMETROS

        public double DistanciaProjecaoRua { get; set; } = 0.8; // 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
        private double DistanciaPrimeirosPontosProjecao { get; set; } = 3.0; // Distancia máxima para projetar os primeiros pontos com distancia menor
        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

        #endregion

        public DateTime UltimaAtualizacaoDados { get; set; } = DateTime.MinValue;
        [JsonProperty]
        public double TempoEntreLeituras { get; private set; }
        private void AtuaalizarTempoEntreLeituras()
        {
            double dt = Math.Min((1.0 / GPSService.TaxaAmostragemHz), (UltimaAtualizacaoDados - GPSService.UltimaLeitura.Momento).TotalSeconds);
            TempoEntreLeituras = dt;
            UltimaAtualizacaoDados = GPSService.UltimaLeitura.Momento;
        }

        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.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(GPSService.UltimaLeitura, PrimeiroPontoRuaMapa);
                double distUP = GPSUtils.DistanciaEntrePontos(GPSService.UltimaLeitura, 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;
            }
        }

        [JsonProperty]
        public List<int> CorredoresNaoFinalizados { get; private set; }
        private void AtualizarCorredoresNaoFinalizados()
        {
            // ✅ Retorna lista vazia imediatamente se não houver pontos
            if (_Corredores == null || _Corredores.Count == 0)
            {
                CorredoresNaoFinalizados = new List<int>();
                return;
            }

            // ✅ Obtém apenas os corredores com pontos não visitados, sem duplicatas
            CorredoresNaoFinalizados = _Corredores
                .Where(p => !p.Concluido) // Filtra apenas pontos não visitados
                .Select(p => p.Idx) // Pega apenas o id do corredor
                .ToList(); // Converte para lista
        }

        public List<PontoTrajetoriaModel> _TrajetoriaFixa { get; set; }
        private 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 = GPSService.UltimaLeitura
                }
            };

            // 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()
        {
            if (!_TrajetoriaFixaDefinida) return;

            idxInicialJanela = Math.Max(0, (PontoAtual?.idxPonto ?? 0) - JanelaAtualizacaoDePontos / 2);
            idxFinalJanela = Math.Min(_TrajetoriaFixa.Count - 1, (PontoAtual?.idxPonto ?? 0) + JanelaAtualizacaoDePontos / 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 ((CorredorAtual?.Idx ?? 0) != idxNovoCorredor)
            {
                CorredorAtual?.AtualizarDados();
            }

            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()
        {
            GPSModel pontoComparar = ProximoPonto != null ? ProximoPonto.Posicao : PontoAtual.Posicao;
            AnguloCaminho = GPSUtils.CalcularOrientacao(GPSService.UltimaLeitura, pontoComparar);
        }
        [JsonProperty]
        public double DistanciaEsquerda { get; private set; }
        [JsonProperty]
        public double DistanciaDireita { get; private set; }
        private void AtualizarDistanciasLaterais()
        {
            DistanciaEsquerda = CalcularDistanciaLateral(true);
            DistanciaDireita = CalcularDistanciaLateral(false);
        }

        private double CalcularDistanciaLateral(bool ladoEsquerdo)
        {
            // Verifica se o corredor atual existe
            if (!_CorredorAtualDefinido) return 0;

            var rua = ladoEsquerdo ? new List<GPSModel>(RuasPlantacao[CorredorAtual.idxRuaEsquerda]) : new List<GPSModel>(RuasPlantacao[CorredorAtual.idxRuaDireita]);

            double anguloAprojetar = GPSUtils.CalcularOrientacao(rua[rua.Count - 1], rua[0]);
            double anguloBprojetar = GPSUtils.CalcularOrientacao(rua[0], rua[rua.Count - 1]);
            GPSModel pontoA = GPSUtils.GerarPontoDeslocado(rua[0], anguloAprojetar, 10);
            GPSModel pontoB = GPSUtils.GerarPontoDeslocado(rua[rua.Count - 1], anguloBprojetar, 10);

            rua.Insert(0, pontoA);
            rua.Add(pontoB);

            int qtdPontosRua = Convert.ToInt32(GPSUtils.DistanciaDoTrecho(rua));
            rua = GPSUtils.InterpolarRota(rua, qtdPontosRua);
            double d = GPSUtils.DistanciaDoTrecho(rua);

            // Calcula a menor distância entre o robô e a rua
            double distancia = GPSUtils.CalcularMenorDistanciaAteTrecho(GPSService.UltimaLeitura, 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) * (GPSService.UltimaLeitura.Latitude - pontoA.Latitude) -
                (pontoB.Latitude - pontoA.Latitude) * (GPSService.UltimaLeitura.Longitude - pontoA.Longitude);

            // Ajusta a distância com base no sentido de deslocamento do robô
            if ((PontoAtual.Direcao == DirecaoCarroRua.Volta && ladoEsquerdo) || (PontoAtual.Direcao == 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()
        {
            // ✅ Evita processamento desnecessário se a lista for nula ou vazia
            if (!_TrajetoriaFixaDefinida)
            {
                PontoAtual = null;
                return;
            }

            // Busca eficiente: percorre do começo ao fim para achar o primeiro não visitado
            for (int i = idxFinalJanela; i >= idxInicialJanela; i--)
            {
                if (_TrajetoriaFixa[i].Visitado)
                {
                    PontoAtual = _TrajetoriaFixa[i];
                    return;
                }
            }

            PontoAtual = null;
        }
        [JsonProperty]
        public PontoTrajetoriaModel ProximoPonto { get; private set; }
        private void DefinirProximoPonto()
        {
            if (!_TrajetoriaFixaDefinida)
            {
                ProximoPonto = null;
                return;
            }

            if (PontoAtual.idxPonto == _TrajetoriaFixa.Count() - 1)
            {
                ProximoPonto = PontoAtual;
            }
            else
            {
                int idx = (_TrajetoriaJanela.IndexOf(PontoAtual) + 1);
                ProximoPonto = _TrajetoriaJanela[idx];
            }

            /*// Busca eficiente: percorre do começo ao fim para achar o primeiro não visitado
            for (int i = idxInicialJanela; i <= idxFinalJanela; i++)
            {
                if (!_TrajetoriaFixa[i].Visitado)
                {
                    ProximoPonto = _TrajetoriaFixa[i];
                    return;
                }
            }

            ProximoPonto = PontoAtual; // Se não houver pontos não visitados, retorna o último ponto visitado
            */
        }
        [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 && CorredorAtual != null)
            {
                if (!CorredorAtual.Dentro && ProximoPonto.DistanciaAtual > DistanciaManobraEntreRuas)
                {
                    StatusAtual = StatusCarroMapa.Direcionando;
                }
                else if (!CorredorAtual.Dentro && (PontoAtual.PontoBorda || PontoAtual.PontoLigacao || ProximoPonto.Tipo == TipoPontoRua.LigacaoEntrada || PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores))
                {
                    StatusAtual = StatusCarroMapa.Manobrando;
                }
                else if (CorredorAtual.Dentro && !NaMargemDoCorredor)
                {
                    StatusAtual = StatusCarroMapa.CaminhandoRua;
                }
                else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaEntrada))
                {
                    StatusAtual = StatusCarroMapa.EntrandoRua;
                }
                else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == TipoPontoRua.BordaSaida))
                {
                    StatusAtual = StatusCarroMapa.SaindoRua;
                }
                else if (!CorredorAtual.Dentro)
                {
                    StatusAtual = StatusCarroMapa.Direcionando;
                }
            }
        }
        [JsonProperty]
        public double DistanciaTotal { get; private set; }
        [JsonProperty]
        public double DistanciaPercorrida { get; private set; }
        private void AtualizarDistanciaPercorrida()
        {
            DistanciaPercorrida = _Corredores?.Sum(x => x.DistanciaPercorridaTotal) ?? 0;
        }
        [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)
        {
            if (!velocidadeSemErvasMs.HasValue)
            {
                velocidadeSemErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeMax) * 0.8;
            }
            if (!velocidadeComErvasMs.HasValue)
            {
                velocidadeComErvasMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeMin);
            }

            // Supondo que metade do percurso tem ervas e a outra metade não tem
            double distanciaComErvas = DistanciaTotal * VariaveisOperacao.PercentualComErvas;
            double distanciaSemErvas = DistanciaTotal * VariaveisOperacao.PercentualSemErvas;

            // 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 * VariaveisOperacao.PercentualSemErvas;
            double distanciaComErvasRestante = distanciaRestante * VariaveisOperacao.PercentualComErvas;

            // 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");
        }

        private void MarcarPontosIntermediariosNaoVisitados()
        {
            if (!_TrajetoriaFixaDefinida) return;

            for (int i = PontoAtual.idxPonto; i >= Math.Max(0, PontoAtual.idxPonto - JanelaAtualizacaoDePontos); i--)
            {
                var ponto = _TrajetoriaFixa[i];
                if (!ponto.Visitado)
                {
                    ponto.Visitado = true;
                }
            }
        }


        private void AtualizarPropriedadesPontosTrajetoria()
        {
            if (!_TrajetoriaFixaDefinida) return;

            // Atualiza apenas os pontos dentro do intervalo definido
            foreach (var ponto in _TrajetoriaJanela)
            {
                ponto.AtualizarPropriedades();
            }
        }

        private void AtualizarDadosTrajetoria()
        {
            if (!_TrajetoriaFixaDefinida) return;

            AtuaalizarTempoEntreLeituras();
            AtualizarDistanciaMaximaEntreLeituras();

            AtualizarIndicesJanela();
            AtualizarTrajetoriaJanela();

            DefinirPontoAtual();
            AtualizarCorredorAtual();
            DefinirProximoPonto();
            MarcarPontosIntermediariosNaoVisitados();
            DefinirPontoMaisProximo();
            AtualizarAnguloMedioCorredor();
            AtualizarAnguloCaminho();
            AtualizarDistanciasLaterais();
            AtualizarNaMargemDoCorredor();
            AtualizarManobrandoEntreRuas();
            AtualizarStatusAtual();
            AtualizarCorredoresNaoFinalizados();
            AtualizarDistanciaPercorrida();
            AtualizarDistanciaRestante();
            AtualizarPercentualTrajetoria();
            AtualizarTempoEstimado();

            AtualizarTrajetoriaDinamica();
        }

        

        public void LoopAtualizaDados()
        {
            AtualizarPropriedadesPontosTrajetoria();

            AtualizarDadosTrajetoria();

            Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados();

            Variaveis.OperacaoEmAndamento.AtualizaInformacoesControleOperacao();
        }


        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 deslocamentoLateralGPS = ((VariaveisEquipamento.LarguraEsquerda - VariaveisEquipamento.LarguraDireita) / larguraTotal) / 2.0;

            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, deslocamentoLateralGPS);
                    var pontoMedioFinal = GPSUtils.PontoMedioComOffset(ponto2Rua1, ponto2Rua2, deslocamentoLateralGPS);

                    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);
        }


        // 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()
        {
            List<PontoTrajetoriaModel> _trajetoriaFixa = new List<PontoTrajetoriaModel>();
            (List<List<GPSModel>> corredores, List<double> CorredoresLarguras) = ProjetarCorredores();
            Corredores = corredores;

            Enums.DirecaoCarroRua direcaoAtual = ExtremoMaisProximoMapa == 0 ? Enums.DirecaoCarroRua.Ida : Enums.DirecaoCarroRua.Volta;
            //Enums.DirecaoCarroRua direcaoAtual = Enums.DirecaoCarroRua.Ida;

            double larguraCorredorMenor = LarguraCorredorPadrao * 0.9;

            PontoTrajetoriaModel PontoRobo = new PontoTrajetoriaModel(TipoPontoRua.PosicaoRobo)
            {
                idxCorredor = 0,
                idxPonto = _trajetoriaFixa.Count(),
                idxPontoCorredor = 0,
                Posicao = GPSService.UltimaLeitura,
                Direcao = direcaoAtual,
                Orientacao = 0,
                Visitado = true,
                LarguraCorredor = 1.0
            };
            _trajetoriaFixa.Add(PontoRobo);

            int idxEsq = 0;
            int idxDir = 0;

            double anguloAcrescentar = idxDir % 2 == 0 && direcaoAtual == DirecaoCarroRua.Ida ? 20 : -20;

            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)
                {
                    (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);
                }

                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)
                {
                    CorredorAtual.Reverse();
                }

                double anguloProjetar1 = GPSUtils.CalcularOrientacao(CorredorAtual.Skip(1).First(), CorredorAtual.First());
                GPSModel PrimeiroPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.First(), DistanciaProjecaoRua, anguloProjetar1);

                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;
                }
                GPSModel UltimoPonto = GPSUtils.ProjetarPontoDeslocado(CorredorAtual.Last(), !ultimoCorredor ? DistanciaProjecaoRua : (DistanciaProjecaoRua * 2.0), anguloProjetar2);

                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 = LarguraCorredorPadrao
                };

                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)
                    {
                        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);

                        GPSModel P0 = CalcularPontoControle(_penultimoPonto, _ultimoPonto, PrimeiroPonto, DistanciaProjecaoRua * 2.0);
                        List<GPSModel> CurvaConexao = GerarCurvaConexao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, P0, pontosAdicionr);

                        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
                                });
                            }
                            
                        }
                        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 = 
                            distanciaCorredor < DistanciaPrimeirosPontosProjecao && !bordaEntrada && !bordaSaida ? larguraCorredorMenor : 
                            CorredoresLarguras[idx]
                    };

                    _trajetoriaFixa.Add(PontoTrajetoria);
                }

                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 = larguraCorredorMenor
                };
                _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
                .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(),
                        DistanciaPercorridaCorredor = 0,
                        DistanciaPercorridaTotal = 0,
                        idxRuaDireita = idxDir + incDir,
                        idxRuaEsquerda = idxEsq + incEsq
                    };

                    return obj;
                })
                .ToList();

            DistanciaTotal = GPSUtils.DistanciaDoTrecho(_trajetoriaFixa.Where(x => x.Tipo != TipoPontoRua.PosicaoRobo).Select(x => x.Posicao).ToList());

            AtualizarDadosTrajetoria();

            LoopAtualizaDados();
        }

        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(GPSModel penultimoPontoRuaAtual, GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
        {
            double anguloCurva = GPSUtils.CalcularOrientacao(penultimoPontoRuaAtual, ultimoPontoRuaAtual);
            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 = 0; 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)
        {
            GPSModel posicaoAtual = GPSService.UltimaLeitura;

            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(posicaoAtual, 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
            bool rua1EstaEsquerda = EstaEsquerda(robo, proximoPonto, Centroide(rua1));

            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.
        }

    }

    public class CorredorTrajetoriaModel
    {
        public int Idx { get; set; }
        public double Largura { get; set; }
        public double DistanciaTotal { get; set; }
        public double DistanciaPercorridaCorredor { get; set; }
        public double DistanciaPercorridaTotal { get; set; }
        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 int idxRuaEsquerda { get; set; }
        public int idxRuaDireita { get; set; }
        public double FatorLarguraCorredor { get; private set; }

        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 == Enums.TipoPontoRua.Rua || 
                        (
                            (ponto.Tipo == TipoPontoRua.BordaEntrada && !ponto.Aproximando) || 
                            (ponto.Tipo == TipoPontoRua.BordaSaida && ponto.Aproximando)
                        )
                    )
                )
                {
                    Dentro = true;
                    break; // Encontramos um ponto válido, podemos sair
                }
            }

            FatorLarguraCorredor = Largura / TrajetoriaMapaOperacaoModel.LarguraCorredorPadrao;
            DistanciaRestante = DistanciaTotal - DistanciaPercorridaTotal;
            Progresso = (DistanciaTotal > 0) ? FuncoesMatematicas.Clamp((DistanciaPercorridaTotal / 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>()
            };
            if (clonarPontos)
            {
                obj.Pontos = new List<PontoTrajetoriaModel>(Pontos);
            }

            return obj;
        }
    }

    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 Enums.DirecaoCarroRua Direcao { get; set; }
        public Enums.TipoPontoRua Tipo { get; }
        public double LarguraCorredor { get; set; }
        public double DistanciaMargem { get; 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 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.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(GPSService.UltimaLeitura, Posicao);
        }

        private void AtualizarOrientacaoAnterior()
        {
            OrientacaoAnterior = GPSUtils.CalcularOrientacao(GPSService.PenultimaLeitura, Posicao);
        }

        private void AtualizarDistanciaTrajeto()
        {
            var pontoAtual = Variaveis.OperacaoEmAndamento.Trajetoria.PontoAtual;
            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(Variaveis.OperacaoEmAndamento.Trajetoria._TrajetoriaDinamica.First().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 = Variaveis.OperacaoEmAndamento.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(GPSService.UltimaLeitura, 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
            };
        }
    }

}