agrobot_base/AgroBase/AgroBase/Models/TrajetoriaMapaOperacaoModel.cs

using AgroBase.Services;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Net;
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;
        public static double DistanciaEntrePontos { get; set; } = 1.0;
        public static double DistanciaEntrePontosCurva { get; set; } = 0.25;
        private double DistanciaManobraEntreRuas { get; set; } = 3.0;
        private const int RANGE_ATUALIZACAO = 100; // Define o tamanho da janela de pontos a atualizar

        #endregion

        public DateTime UltimaAtualizacaoDados { get; set; } = DateTime.MinValue;

        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 (IdxCorredorAtual < RuasPlantacao.Count && RuasPlantacao[IdxCorredorAtual].Count > 0)
                {
                    return RuasPlantacao[IdxCorredorAtual][0]; // Acessa diretamente o primeiro ponto da rua
                }

                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 (IdxCorredorAtual < RuasPlantacao.Count && RuasPlantacao[IdxCorredorAtual].Count > 0)
                {
                    return RuasPlantacao[IdxCorredorAtual][RuasPlantacao.Count - 1]; // Acessa diretamente o ultimo ponto da rua
                }

                return new GPSModel(); // Pode ser null se preferir
            }
        }
        public int ExtremoMaisProximoMapa
        {
            get
            {
                double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRuaMapa);
                double distUP = GPSService.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;
            }
        }

        private List<int> _corredoresNaoFinalizados;
        public List<int> CorredoresNaoFinalizados
        {
            get
            {
                return _corredoresNaoFinalizados;
            }
        }
        private void AtualizarCorredoresNaoFinalizados()
        {
            var corredores = new HashSet<int>();

            foreach (var ponto in _TrajetoriaFixa)
            {
                if (!ponto.Visitado)
                {
                    corredores.Add(ponto.idxCorredor); // Adiciona apenas corredores com pontos não visitados
                }
            }

           _corredoresNaoFinalizados = corredores.ToList();
        }

        private List<double> CorredoresLarguras { get; set; }
        public List<PontoTrajetoriaModel> _TrajetoriaFixa { get; set; }
        public Dictionary<int, List<PontoTrajetoriaModel>> _Corredores { get; set; }
        public List<GPSModel> TrajetoriaFixa => _TrajetoriaFixa.ConvertAll(x => x.Posicao);
        public List<GPSModel> TrajetoriaDinamica
        {
            get
            {
                // Cria a lista com a última leitura do robô
                var trajetoria = new List<GPSModel> { GPSService.UltimaLeitura };

                // Se a trajetória fixa estiver vazia, retorna apenas a leitura atual
                if (_TrajetoriaFixa.Count == 0)
                {
                    return trajetoria;
                }

                // Adiciona os pontos não visitados, já ordenados por idxPonto
                foreach (var ponto in _TrajetoriaFixa)
                {
                    if (!ponto.Visitado)
                    {
                        trajetoria.Add(ponto.Posicao);
                    }
                }

                return trajetoria;
            }
        }

        public static double DistanciaMaximaEntreLeituras { get; private set; }
        public void AtualizarDistanciaMaximaEntreLeituras(double velocidadeCarroMs)
        {
            double distanciaMaxima = (velocidadeCarroMs / GPSService.TaxaAmostragemHz);
            DistanciaMaximaEntreLeituras = distanciaMaxima;
        }

        private void AtualizarPropriedadesPontosTrajetoria()
        {
            if (_TrajetoriaFixa.Count == 0) return;

            // Define os limites da janela de atualização
            int idxInicial = Math.Max(0, PontoAtual.idxPonto - RANGE_ATUALIZACAO / 2);
            int idxFinal = Math.Min(_TrajetoriaFixa.Count, PontoAtual.idxPonto + RANGE_ATUALIZACAO / 2);

            // Atualiza apenas os pontos dentro do intervalo definido
            for (int i = idxInicial; i < idxFinal; i++)
            {
                _TrajetoriaFixa[i].AtualizarPropriedades();
            }
        }


        public int IdxCorredorAtual
        {
            get
            {
                return PontoAtual?.idxCorredor ?? 0;
            }
        }
        
        public Enums.DirecaoCarroRua DirecaoCaminho
        {
            get
            {
                return PontoAtual?.Direcao ?? Enums.DirecaoCarroRua.Parado;
            }
        }
        public double AnguloMedioCorredor
        {
            get
            {
                return GPSService.CalcularOrientacao(PontoAtual.Posicao, ProximoPonto.Posicao);
            }
        }
        public double AnguloCaminho
        {
            get
            {
                GPSModel pontoComparar = ProximoPonto != null ? ProximoPonto.Posicao : PontoAtual.Posicao;
                double angulo = GPSService.CalcularOrientacao(GPSService.UltimaLeitura, pontoComparar);
                return angulo;
            }
        }

        
        private double _distanciaEsquerda;
        public double DistanciaEsquerda
        {
            get
            {
                return _distanciaEsquerda;
            }
        }
        
        private double _distanciaDireita;
        public double DistanciaDireita
        {
            get
            {
                return _distanciaDireita;
            }
        }
        private void AtualizarDistanciasLaterais()
        {
            _distanciaEsquerda = CalcularDistanciaLateral(true);
            _distanciaDireita = CalcularDistanciaLateral(false);
        }
        private double CalcularDistanciaLateral(bool ladoEsquerdo)
        {
            // Verifica se o corredor atual existe
            if (!_Corredores.ContainsKey(IdxCorredorAtual))
            {
                return 0;
            }

            var pontosDoCorredor = _Corredores[IdxCorredorAtual];
            if (pontosDoCorredor.Count == 0)
            {
                return 0;
            }

            // Pega o primeiro ponto do corredor atual
            var pontoCorredor = pontosDoCorredor[0];

            // Calcula as distâncias até os extremos da rua
            double distPP = GPSService.DistanciaEntrePontos(pontoCorredor.Posicao, PrimeiroPontoRuaMapa);
            double distUP = GPSService.DistanciaEntrePontos(pontoCorredor.Posicao, UltimoPontoRuaMapa);

            int extremoInicial = (distPP < distUP) ? 0 : 1;

            // Determina a rua correta de acordo com o lado e a posição do extremo inicial
            List<GPSModel> rua;
            if (ladoEsquerdo)
            {
                rua = (extremoInicial == 0) ? RuasPlantacao[IdxCorredorAtual + 1] : RuasPlantacao[IdxCorredorAtual];
            }
            else
            {
                rua = (extremoInicial == 0) ? RuasPlantacao[IdxCorredorAtual] : RuasPlantacao[IdxCorredorAtual + 1];
            }

            // Evita erro de índice (se for o último corredor, não há IdxCorredorAtual + 1)
            int idxMax = RuasPlantacao.Count - 1;
            int idxRua = Math.Min(IdxCorredorAtual + (ladoEsquerdo ? 1 : 0), idxMax);
            rua = RuasPlantacao[idxRua];

            // Calcula a menor distância entre o robô e a rua
            double distancia = GPSService.CalcularMenorDistanciaAteTrecho(GPSService.UltimaLeitura, rua);

            // Ajusta a largura do robô no cálculo final
            double larguraAjuste = ladoEsquerdo ? VariaveisEquipamento.LarguraEsquerda : VariaveisEquipamento.LarguraDireita;
            distancia -= (larguraAjuste / 100.0);

            return distancia;
        }


        private PontoTrajetoriaModel _pontoAtual;
        public PontoTrajetoriaModel PontoAtual
        {
            get
            {
                return _pontoAtual;
            }
        }
        private void DefinirPontoAtual()
        {
            if (_TrajetoriaFixa == null)
            {
                _pontoAtual = null;
            }

            // Busca eficiente: percorre de trás para frente para achar o último visitado
            for (int i = _TrajetoriaFixa.Count - 1; i >= 0; i--)
            {
                if (_TrajetoriaFixa[i].Visitado)
                {
                    _pontoAtual = _TrajetoriaFixa[i];
                    return;
                }
            }

            _pontoAtual = null;
        }

        private PontoTrajetoriaModel _proximoPonto;
        public PontoTrajetoriaModel ProximoPonto
        {
            get
            {
                return _proximoPonto;
            }
        }
        private void DefinirProximoPonto()
        {
            if (_TrajetoriaFixa == null)
            {
                _proximoPonto = null;
            }

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

            _proximoPonto = PontoAtual; // Se não houver pontos não visitados, retorna o último ponto visitado
        }

        private bool _naMargemDoCorredor;
        public bool NaMargemDoCorredor
        {
            get
            {
                return _naMargemDoCorredor;
            }
        }
        private void AtualizarNaMargemDoCorredor()
        {
            _naMargemDoCorredor = false;
            foreach (var ponto in _TrajetoriaFixa)
            {
                if (ponto.NaMargem && ponto.PontoBorda)
                {
                    _naMargemDoCorredor = true; // Paramos a busca assim que encontramos um ponto válido
                    return;
                }
            }
        }

        private bool _dentroCorredor;
        public bool DentroCorredor
        {
            get
            {
                return _dentroCorredor;
            }
        }
        private void AtualizaDentroCorredor()
        {
            _dentroCorredor = false;
            if (_Corredores == null || !_Corredores.ContainsKey(IdxCorredorAtual))
                return;

            // Itera diretamente sem criar uma lista temporária
            foreach (var ponto in _Corredores[IdxCorredorAtual])
            {
                if (ponto.NaMargem && (ponto.Tipo == Enums.TipoPontoRua.Rua || ponto.PontoBorda))
                {
                    _dentroCorredor = true;
                    return; // Encontramos um ponto válido, podemos sair
                }
            }
        }

        private bool _manobrandoEntreRuas;
        public bool ManobrandoEntreRuas
        {
            get
            {
                return _manobrandoEntreRuas;
            }
        }
        private void AtualizarManobrandoEntreRuas()
        {
            bool proximoAentrada = (ProximoPonto.PontoBorda || ProximoPonto.PontoLigacao) && ProximoPonto.DistanciaAtual < DistanciaManobraEntreRuas;
            _manobrandoEntreRuas = (proximoAentrada && StatusAtual == StatusCarroMapa.Direcionando) || NaMargemDoCorredor;
        }

        private Enums.StatusCarroMapa _statusAtual = Enums.StatusCarroMapa.Parado;
        public Enums.StatusCarroMapa StatusAtual
        {
            get
            {
                return _statusAtual;
            }
        }
        private void AtualizarStatusAtual()
        {
            _statusAtual = Enums.StatusCarroMapa.Parado;
            if (ProximoPonto != null)
            {
                if (!DentroCorredor && (PontoAtual.Tipo == TipoPontoRua.PosicaoRobo || PontoAtual.PontoLigacao || ProximoPonto.PontoLigacao))
                {
                    _statusAtual = Enums.StatusCarroMapa.Direcionando;
                }
                
                else if (DentroCorredor && !NaMargemDoCorredor)
                {
                    _statusAtual = Enums.StatusCarroMapa.CaminhandoRua;
                }
                
                else if (!DentroCorredor && (!PontoAtual.PontoLigacao && !ProximoPonto.PontoLigacao))
                {
                    _statusAtual = Enums.StatusCarroMapa.Manobrando;
                }

                else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == Enums.TipoPontoRua.BordaEntrada))
                {
                    _statusAtual = Enums.StatusCarroMapa.EntrandoRua;
                }

                else if (_TrajetoriaFixa.Any(x => x.NaMargem && x.Tipo == Enums.TipoPontoRua.BordaSaida))
                {
                    _statusAtual = Enums.StatusCarroMapa.SaindoRua;
                }
            }
        }

        private void AtualizarDadosTrajetoria()
        {
            DefinirPontoAtual();
            DefinirProximoPonto();
            AtualizarDistanciasLaterais();
            AtualizarNaMargemDoCorredor();
            AtualizaDentroCorredor();
            AtualizarManobrandoEntreRuas();
            AtualizarStatusAtual();
            AtualizarCorredoresNaoFinalizados();

            double velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP);
            AtualizarDistanciaMaximaEntreLeituras(velocidadeCarroMs);
        }

        public double DistanciaTotal { get; private set; }
        public double DistanciaPercorrida
        {
            get
            {
                double percorrido = GPSService.DistanciaDoTrecho(Variaveis.OperacaoEmAndamento.GPSTrajetoria);
                return percorrido;
            }
        }
        public double DistanciaRestante
        {
            get
            {
                double restante = GPSService.DistanciaDoTrecho(TrajetoriaDinamica);
                return restante;
            }
        }

        public string TempoEstimadoOperacao(double velocidadeSemErvas, double velocidadeComErvas)
        {
            // Conversão de velocidades para m/s
            double velocidadeSemErvasMs = ((velocidadeSemErvas * 1000.0) / 3600.0) * 0.8; // 80%
            double velocidadeComErvasMs = (velocidadeComErvas * 1000.0) / 3600.0;

            // 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 > 0 ? distanciaSemErvas / velocidadeSemErvasMs : 0;
            double tempoComErvas = velocidadeComErvasMs > 0 ? distanciaComErvas / velocidadeComErvasMs : 0;

            // Tempo total em segundos
            double tempoTotalSegundos = tempoSemErvas + tempoComErvas;

            // Conversão para horas, minutos e segundos
            TimeSpan tempoTotal = TimeSpan.FromSeconds(tempoTotalSegundos);

            return $"{tempoTotal.Hours:D2}:{tempoTotal.Minutes:D2}:{tempoTotal.Seconds:D2}";
        }

        public string TempoEstimadoRestante()
        {
            // Velocidades em RPM
            double velocidadeSemErvas = FuncoesMatematicas.CalculaVelocidadeRPM(Variaveis.OperacaoEmAndamento.Controle.RPM_Max) * 0.8;
            double velocidadeComErvas = FuncoesMatematicas.CalculaVelocidadeRPM(Variaveis.OperacaoEmAndamento.Controle.RPM_Min);

            // Conversão de velocidades para m/s
            double velocidadeSemErvasMs = (velocidadeSemErvas * 1000) / 3600;
            double velocidadeComErvasMs = (velocidadeComErvas * 1000) / 3600;

            // 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;
            double tempoRestanteComErvas = distanciaComErvasRestante / velocidadeComErvasMs;

            // Tempo total restante em segundos
            double tempoTotalRestanteSegundos = tempoRestanteSemErvas + tempoRestanteComErvas;

            // Conversão para horas, minutos e segundos
            TimeSpan tempoTotalRestante = TimeSpan.FromSeconds(tempoTotalRestanteSegundos);

            return $"{tempoTotalRestante.Hours:D2}:{tempoTotalRestante.Minutes:D2}:{tempoTotalRestante.Seconds:D2}";
        }


        public void LoopAtualizaDados()
        {
            double dt = Math.Min((1.0 / GPSService.TaxaAmostragemHz), (UltimaAtualizacaoDados - GPSService.UltimaLeitura.Momento).TotalSeconds);






            #region ATUALIZAR PONTOS VISITADOS

            /*if (_TrajetoriaFixa.Any(x => x.NaMargem && !x.Visitado))
            {
                double velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP);

                // Distância máxima percorrida entre leituras
                double distanciaMaxima = (velocidadeCarroMs / GPSService.TaxaAmostragemHz);

                double distanciaEntrePontos = PontoAtual.Tipo == TipoPontoRua.CruvaEntreCorredores ? DistanciaEntrePontosCurva : DistanciaEntrePontos;

                // Quantidade de pontos que podem ser cobertos na margem
                int pontosCobertos = (int)Math.Ceiling(distanciaMaxima / distanciaEntrePontos) * 2;

                // Marcar como visitados todos os pontos dentro da margem
                double limiteDistancia = pontosCobertos * distanciaEntrePontos;


                // Melhor filtro: Seleciona diretamente os pontos não visitados e que estão na margem
                var pontosParaAtualizar = _TrajetoriaFixa
                    .Where(x => x.NaMargem && !x.Visitado && x.DistanciaTrajeto <= limiteDistancia)
                    .Take(pontosCobertos)
                    .ToList();

                // Atualiza diretamente os pontos visitados
                foreach (var ponto in pontosParaAtualizar)
                {
                    ponto.Visitado = true;
                }

                
            }*/

            double velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.Controle.PercentualVelocidadeSP);
            AtualizarDistanciaMaximaEntreLeituras(velocidadeCarroMs);

            AtualizarPropriedadesPontosTrajetoria();

            AtualizarDadosTrajetoria();

            #endregion

            UltimaAtualizacaoDados = GPSService.UltimaLeitura.Momento;

            Variaveis.OperacaoEmAndamento.AtualizaInformacoesControleOperacao();

            GPSService.AtualizarTrajetoriaDinamica();
        }


        private (List<List<GPSModel>>, List<double>) ProjetarCorredores()
        {
            var corredores = new List<List<GPSModel>>();
            var larguras = new List<double>();

            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 = GPSService.InterpolarRota(rua2, rua1.Count);
                    }
                    else
                    {
                        rua1 = GPSService.InterpolarRota(rua1, rua2.Count);
                    }
                }

                var corredor = new List<GPSModel>();

                // 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 = GPSService.PontoMedio(ponto1Rua1, ponto1Rua2);
                    var pontoMedioFinal = GPSService.PontoMedio(ponto2Rua1, ponto2Rua2);

                    var distancia = GPSService.DistanciaEntrePontos(pontoMedioInicial, pontoMedioFinal);

                    int numInterpolacoes = (int)(distancia / DistanciaEntrePontos);

                    for (int k = 0; k <= numInterpolacoes; k++)
                    {
                        double t = k / (double)numInterpolacoes;
                        var pontoInterpolado = GPSService.InterpolarPonto(pontoMedioInicial, pontoMedioFinal, t);
                        corredor.Add(pontoInterpolado);
                    }
                }

                // Remover pontos que estão a menos de 25 cm de distância
                corredor = RemoverPontosMuitoProximos(corredor, DistanciaEntrePontos);

                // 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
        private List<GPSModel> RemoverPontosMuitoProximos(List<GPSModel> pontos, double distanciaMinima)
        {
            if (pontos == null || pontos.Count == 0)
                return pontos;

            var resultado = new List<GPSModel> { pontos[0] }; // Sempre incluir o primeiro ponto

            for (int i = 1; i < pontos.Count; i++)
            {
                var distancia = GPSService.DistanciaEntrePontos(resultado.Last(), pontos[i]);
                if (distancia >= distanciaMinima)
                {
                    resultado.Add(pontos[i]);
                }
            }

            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 = GPSService.InterpolarRota(rua1, Convert.ToInt32(GPSService.DistanciaDoTrecho(rua1)));
            List<GPSModel> rua2Interpolada = GPSService.InterpolarRota(rua2, Convert.ToInt32(GPSService.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 => GPSService.DistanciaEntrePontos(ponto1, ponto2)).First();

                double distancia = GPSService.DistanciaEntrePontos(ponto1, pontoMaisProximo);

                somaDistancias += distancia;
                totalPontos++;
            }

            // 3. Retorna a largura média do corredor
            return totalPontos > 0 ? somaDistancias / totalPontos : 0;
        }

        public double DistanciaEntreRuas(List<List<GPSModel>> ruas)
        {
            double distanciaTotal = 0;

            if (ruas.Count < 2)
            {
                // Se houver menos de duas ruas, não há distância a calcular
                return distanciaTotal;
            }

            double menorDistancia = double.MaxValue;

            for (int i = 0; i < ruas.Count - 1; i++)
            {
                List<GPSModel> ruaAtual = ruas[i];
                List<GPSModel> proximaRua = ruas[i + 1];

                // Calcule a distância entre as duas ruas (rua atual e próxima rua)
                // Aqui você pode usar o método GPSService.CalcularDistanciaPontos
                // ou uma função personalizada para calcular a distância entre as ruas


                foreach (GPSModel pontoAtual in ruaAtual)
                {
                    foreach (GPSModel pontoProxima in proximaRua)
                    {
                        // Calcular a distância entre pontoAtual e pontoProxima
                        double distancia = GPSService.DistanciaEntrePontos(pontoAtual, pontoProxima);

                        // Verifique se esta é a menor distância
                        if (distancia < menorDistancia)
                        {
                            menorDistancia = distancia;
                        }
                    }
                }

                // Adiciona a menor distância encontrada à distância total
                distanciaTotal += menorDistancia;
            }

            double distanciaMedia = distanciaTotal / ruas.Count();

            return menorDistancia;
        }

       



        public List<PontoTrajetoriaModel> ProjetarTrajetoriaFixa()
        {
            List<PontoTrajetoriaModel> _trajetoriaFixa = new List<PontoTrajetoriaModel>();
            (Corredores, CorredoresLarguras) = ProjetarCorredores();

            double larguraCorredorPadrao = 1.5;
            Enums.DirecaoCarroRua direcaoAtual = ExtremoMaisProximoMapa == 0 ? Enums.DirecaoCarroRua.Ida : Enums.DirecaoCarroRua.Volta;

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

            for (int idx = 0; idx < Corredores.Count(); idx++) 
            {
                List<GPSModel> CorredorAtual = Corredores[idx];

                if (idx % 2 != 0)
                {
                    CorredorAtual.Reverse();
                }

                double anguloProjetar1 = GPSService.CalcularOrientacao(CorredorAtual.Skip(1).First(), CorredorAtual.First());
                GPSModel PrimeiroPonto = GPSService.ProjetarPontoDeslocado(CorredorAtual.First(), DistanciaProjecaoRua, anguloProjetar1);
                double anguloProjetar2 = GPSService.CalcularOrientacao(CorredorAtual.Skip(CorredorAtual.Count() - 2).First(), CorredorAtual.Last());
                GPSModel UltimoPonto = GPSService.ProjetarPontoDeslocado(CorredorAtual.Last(), DistanciaProjecaoRua, 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 = GPSService.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 = GPSService.DistanciaEntrePontos(ultimoPontoTrajetoria.Posicao, PontoInicial.Posicao);
                    bool distanciaMinima = distancia < DistanciaManobraEntreRuas;

                    if (distanciaMinima)
                    {
                        int pontosAdicionr = Convert.ToInt32(CorredoresLarguras[idx] / DistanciaEntrePontosCurva);
                        bool CurvaParaEsquerda = (ExtremoMaisProximoMapa == 0 && direcaoAtual == DirecaoCarroRua.Volta) || (ExtremoMaisProximoMapa == 1 && direcaoAtual == DirecaoCarroRua.Ida);
                        GPSModel P0 = CalcularPontoControle(_trajetoriaFixa[_trajetoriaFixa.Count() - 2].Posicao, ultimoPontoTrajetoria.Posicao, PrimeiroPonto, DistanciaProjecaoRua);
                        List<GPSModel> CurvaConexao = GerarCurvaConexao(ultimoPontoTrajetoria.Posicao, PrimeiroPonto, P0, pontosAdicionr);

                        foreach (var ponto in CurvaConexao)
                        {
                            bool pontoLigacao = CurvaConexao.IndexOf(ponto) == CurvaConexao.Count() - 1;
                            _trajetoriaFixa.Add(new PontoTrajetoriaModel(pontoLigacao ? TipoPontoRua.LigacaoEntrada : TipoPontoRua.CruvaEntreCorredores)
                            {
                                idxCorredor = idx,
                                idxPonto = _trajetoriaFixa.Count(),
                                idxPontoCorredor = _trajetoriaFixa.Count(x => x.idxCorredor == idx),
                                Posicao = ponto,
                                Direcao = pontoLigacao ? direcaoAtual : DirecaoCarroRua.Manobra,
                                Orientacao = GPSService.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, ponto),
                                Visitado = false,
                                LarguraCorredor = larguraCorredorPadrao
                            });
                            pontoLigacao = false;
                        }
                        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;

                    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 = GPSService.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, pontoAtual),
                        Visitado = false,
                        LarguraCorredor = 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 = GPSService.CalcularOrientacao(_trajetoriaFixa.Last().Posicao, UltimoPonto),
                    Visitado = false,
                    LarguraCorredor = larguraCorredorPadrao
                };
                _trajetoriaFixa.Add(PontoFinal);

                direcaoAtual = direcaoAtual == Enums.DirecaoCarroRua.Ida ? Enums.DirecaoCarroRua.Volta : Enums.DirecaoCarroRua.Ida;
            }

            _TrajetoriaFixa = new List<PontoTrajetoriaModel>(_trajetoriaFixa);
            _Corredores = _TrajetoriaFixa.GroupBy(x => x.idxCorredor).ToDictionary(g => g.Key, g => g.ToList());

            DistanciaTotal = GPSService.DistanciaDoTrecho(_trajetoriaFixa.Select(x => x.Posicao).ToList());

            AtualizarDadosTrajetoria();
            AtualizarPropriedadesPontosTrajetoria();

            return _TrajetoriaFixa;
        }

        public static GPSModel CalcularPontoControle(GPSModel penultimoPontoRuaAtual, GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
        {
            double anguloCurva = GPSService.CalcularOrientacao(penultimoPontoRuaAtual, ultimoPontoRuaAtual);
            double angulo = GPSService.CalcularOrientacao(ultimoPontoRuaAtual, primeiroPontoProximaRua);
            double lat = (ultimoPontoRuaAtual.Latitude + primeiroPontoProximaRua.Latitude) / 2;
            double lon = (ultimoPontoRuaAtual.Longitude + primeiroPontoProximaRua.Longitude) / 2;
            GPSModel pontoMedio = new GPSModel()
            {
                Latitude = lat,
                Longitude = lon,
            };

            bool ParaDentro = (angulo - anguloCurva) > 0;

            // Ajuste o ângulo com base na direção da curva
            double ajusteAngulo = (ParaDentro ? 270 : 90);

            // Projetar um ponto a uma certa distância na direção ajustada
            return GPSService.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 = GPSService.GerarPontoDeslocado(posicaoAtual, anguloDesvioInicial, distanciaObstaculo);
            GPSModel ponto2 = GPSService.GerarPontoDeslocado(ponto1, anguloCaminho, larguraObstaculo);
            GPSModel ponto3 = GPSService.GerarPontoDeslocado(ponto2, anguloDesvioFinal, distanciaObstaculo);
            List<GPSModel> lista = new List<GPSModel>()
            {
                ponto1,
                ponto2,
                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 = GPSService.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
        }

    }

    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 bool Visitado { get; set; } = false;

        // ✅ Agora, propriedades podem ser atualizadas com `set; private`
        public double OrientacaoAtual { get; private set; }
        public double OrientacaoAnterior { get; private set; }
        public double DistanciaTrajeto { get; private set; }
        public double DistanciaAtual { get; private set; }
        public double DistanciaAnterior { get; private set; }
        public bool Aproximando { get; private set; }
        public bool NaMargem { get; private set; }
        public bool PontoBorda { get; private set; }
        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 * 1.0) : 
                TrajetoriaMapaOperacaoModel.DistanciaEntrePontos;

            // Define o limite de distância para considerar o ponto como visitado
            double limiteDistancia = Math.Ceiling(TrajetoriaMapaOperacaoModel.DistanciaMaximaEntreLeituras / distanciaEntrePontos) * 2 * 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 = GPSService.CalcularOrientacao(GPSService.UltimaLeitura, Posicao);
        }

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

        private void AtualizarDistanciaTrajeto()
        {
            var pontoAtual = Variaveis.OperacaoEmAndamento.Trajetoria.PontoAtual;
            int idxPontoAtual = pontoAtual.idxPonto;
            int qtdPontos = Math.Abs(idxPonto - idxPontoAtual) + 1; // Sempre positivo

            if (qtdPontos == 1)
            {
                DistanciaTrajeto = pontoAtual.Aproximando ? pontoAtual.DistanciaAtual : -pontoAtual.DistanciaAtual; 
                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 = GPSService.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 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, Posicao);
        }

        private void AtualizarDistanciaAnterior()
        {
            DistanciaAnterior = GPSService.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()
        {
            NaMargem = DistanciaAtual < (LarguraCorredor / 1.2);
        }
    }

}