using AgroBase.Properties;
using AgroBase.Services;
using CefSharp.DevTools.Page;
using CefSharp.WinForms;
using Emgu.CV.Features2D;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Security.Cryptography;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using static AgroBase.Models.Enums;

namespace AgroBase.Models
{
    public class MapasModel
    {
        public ChromiumWebBrowser chromiumWebBrowser { get; set; }
        public Panel pnlMapa { get; set; }
        private PictureBox picLoading { get; set; }
        public Label lblMapa { get; set; }
        public MapasService mapaService { get; set; } = new MapasService();
        public List<List<GPSModel>> Trajetoria { get; set; } = new List<List<GPSModel>>();
        public List<GPSModel> TrajetoriaDinamica { get; set; } = new List<GPSModel>();
        public List<string> RuasPercorrer { get; set; } = new List<string>();
        public int IdxRuaAtual { get; set; } = 0;
        public int IdxUltimoPontoTrajetoria { get; set;} = 0;
        public int IdxUltimoPontoTrajetoriaDinamica { get; set;} = 0;
        public double DistanciaTotal
        {
            get
            {
                double distancia = 0.0;

                foreach (var trecho in Trajetoria)
                {
                    distancia += GPSService.DistanciaDoTrecho(trecho);
                }

                return distancia;
            }
        }
        public DirecaoCarroRua DirecaoCaminho
        {
            get
            {
                DirecaoCarroRua direcaoCaminho = DirecaoCarroRua.Parado;
                if (TrajetoriaDinamica.Any())
                {
                    (GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(TrajetoriaDinamica, TrajetoriaTipo.Dinamica);
                    int idxPontoMaisProximo = TrajetoriaDinamica.IndexOf(PontoMaisProximo);
                    direcaoCaminho = TrajetoriaDinamica[idxPontoMaisProximo].DirecaoTrecho;
                }
                else
                {
                    if (Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ExtremoMaisProximo == 0)
                    {
                        direcaoCaminho = DirecaoCarroRua.Ida;
                    }
                    else
                    {
                        direcaoCaminho = DirecaoCarroRua.Volta;
                    }
                }
                return direcaoCaminho;
            }
        }
        public bool DentroDaRua
        {
            get
            {
                if (mapaService.DadosMapa != null && RuasPercorrer.Count() > IdxRuaAtual)
                {
                    try
                    {
                        string IDruaAtual = RuasPercorrer[IdxRuaAtual];
                        var Rua = mapaService.DadosMapa.features.Where(x => x.geometry.id == IDruaAtual).FirstOrDefault();
                        if (Rua != null)
                        {
                            bool dentro = false;
                            List<GPSModel> Trecho = new List<GPSModel>();
                            List<GPSModel> PontosRua = new List<GPSModel>();
                            Trajetoria[IdxRuaAtual].ToList().ForEach(x => PontosRua.Add(x));
                            if (DirecaoCaminho == DirecaoCarroRua.Volta)
                            {
                                PontosRua.Reverse();
                            }
                            (GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(PontosRua, TrajetoriaTipo.Rua);
                            int idx = PontosRua.IndexOf(PontoMaisProximo);
                            foreach (var Ponto in PontosRua)
                            {
                                // Adiciona os pontos que o robô já passou por eles no trecho, e quando chegar no ponto mais próximo, substitui ele pela posição atual
                                int idxPonto = PontosRua.IndexOf(Ponto);
                                if (idxPonto < idx)
                                {
                                    Trecho.Add(Ponto);
                                }
                                else
                                {
                                    if (idx > 0)
                                    {
                                        Trecho.Add(GPSService.UltimaLeitura);
                                    }

                                    break;
                                }
                            }
                            double distTrecho = GPSService.DistanciaDoTrecho(Trecho);
                            dentro = (distTrecho > 0 && distTrecho < Rua.properties.Length) || (idx == 0 && !aproximando);

                            return dentro;
                        }
                        else
                        {
                            return false;
                        }
                    }
                    catch
                    {
                        return false;
                    }
                }
                else
                {
                    return false;
                }
            }
        }
        public GPSModel PrimeiroPontoRua
        {
            get
            {
                if (Trajetoria.Any(x => x.Any()) && Trajetoria.Count() > IdxRuaAtual)
                {
                    var primeiroPontoRua = Trajetoria.Skip(IdxRuaAtual).First().First();
                    return primeiroPontoRua;
                }
                else
                {
                    return new GPSModel();
                }

            }
        }
        public GPSModel UltimoPontoRua
        {
            get
            {
                if (Trajetoria.Any(x => x.Any()) && Trajetoria.Count() > IdxRuaAtual)
                {
                    var ultimoPontoRua = Trajetoria.Skip(IdxRuaAtual).First().Last();
                    return ultimoPontoRua;
                }
                else
                {
                    return new GPSModel();
                }
            }
        }
        public int ExtremoMaisProximo
        {
            get
            {
                double distPP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, PrimeiroPontoRua);
                double distUP = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, UltimoPontoRua);

                // 0 para mais próximo do primeiro ponto da rua (ENTRANDO)
                // 1 para mais próximo do último ponto da rua (SAINDO)
                return (distPP < distUP) ? 0 : 1;
            }
        }
        public double AnguloCaminho
        {
            get
            {
                try
                {
                    if (TrajetoriaDinamica.Any())
                    {
                        (GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(TrajetoriaDinamica, TrajetoriaTipo.Dinamica);
                        int idx = TrajetoriaDinamica.IndexOf(PontoMaisProximo);

                        if ((idx + 1) >= TrajetoriaDinamica.Count())
                        {
                            return 0;
                        }

                        double angulo = GPSService.CalcularOrientacao(TrajetoriaDinamica[idx], TrajetoriaDinamica[idx + 1]);
                        if (angulo == 0)
                        {
                            angulo = GPSService.CalcularOrientacao(TrajetoriaDinamica[idx + 1], TrajetoriaDinamica[idx + 2]);
                        }

                        return (!(angulo >= 0 || angulo <= 0)) ? 0 : angulo;
                    }
                    else
                    {
                        return 0;
                    }
                }
                catch
                {
                    return 0;
                }
            }
        }
        public double DistanciaEsquerda
        {
            get
            {
                if (IdxRuaAtual >= Trajetoria.Count())
                {
                    return 0;
                }

                double distancia = 0;

                List<GPSModel> RuaEsquerda = new List<GPSModel>();
                if (Trajetoria.Any())
                {
                    if (DirecaoCaminho == DirecaoCarroRua.Ida)
                    {
                        RuaEsquerda = Trajetoria[IdxRuaAtual];
                    }
                    else if (DirecaoCaminho == DirecaoCarroRua.Volta)
                    {
                        if (IdxRuaAtual > 0)
                        {
                            (GPSModel pontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual - 1], TrajetoriaTipo.Geral);
                            double distanciaP0 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoMaisProximo);
                            if (distanciaP0 < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua)
                            {
                                Trajetoria[IdxRuaAtual - 1].ForEach(x => RuaEsquerda.Add(x));
                                RuaEsquerda.Reverse();
                            }
                        }
                    }
                }

                if (RuaEsquerda.Any())
                {
                    distancia = DistanciaAteRua(RuaEsquerda, VariaveisEquipamento.LarguraEsquerda);
                }
                else
                {
                    distancia = DistanciaEntreRuas(Trajetoria) / 2;
                }

                return distancia;
            }
        }
        public double DistanciaDireita
        {
            get
            {
                if (IdxRuaAtual >= Trajetoria.Count())
                {
                    return 0;
                }

                double distancia = 0;

                List<GPSModel> RuaDireita = new List<GPSModel>();
                if (Trajetoria.Any())
                {
                    if (DirecaoCaminho == DirecaoCarroRua.Volta)
                    {
                        Trajetoria[IdxRuaAtual].ForEach(x => RuaDireita.Add(x));
                        RuaDireita.Reverse();
                    }
                    else if (DirecaoCaminho == DirecaoCarroRua.Ida)
                    {
                        if (IdxRuaAtual > 0)
                        {
                            (GPSModel pontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Trajetoria[IdxRuaAtual - 1], TrajetoriaTipo.Geral);
                            double distanciaP0 = GPSService.DistanciaEntrePontos(GPSService.UltimaLeitura, pontoMaisProximo);
                            if (distanciaP0 < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua)
                            {
                                RuaDireita = Trajetoria[IdxRuaAtual - 1];
                            }
                        }
                    }
                }
                
                if (RuaDireita.Any())
                {
                    distancia = DistanciaAteRua(RuaDireita, VariaveisEquipamento.LarguraDireita);
                }
                else
                {
                    distancia = DistanciaEntreRuas(Trajetoria) / 2;
                }

                return distancia;
            }
        }

        public void btnCarregar_Click(object sender, EventArgs e, string caminhoArquivo = "", bool monitoramento = false)
        {
            if (monitoramento)
            {
                mapaService.CarregarMapaMonitoramento(MapasVariaveisModel.NomeArquivoMapaInterativo, MapasVariaveisModel.NomeArquivoMapaInterativoSaida, caminhoArquivo);
            }
            else
            {
                mapaService.CarregarMapa(MapasVariaveisModel.NomeArquivoMapaInterativo, MapasVariaveisModel.NomeArquivoMapaInterativoSaida, caminhoArquivo);
            }
            if (mapaService.MapaUrl != "")
            {
                if (picLoading == null)
                {
                    picLoading = new PictureBox();
                }
                picLoading.Dock = DockStyle.Fill;
                picLoading.BackColor = System.Drawing.Color.Transparent;
                picLoading.Image = Resources.loading;
                picLoading.Name = "picLoading";
                picLoading.SizeMode = PictureBoxSizeMode.Zoom;
                picLoading.Visible = true;
                pnlMapa.Controls.Add(picLoading);

                Timer tmr = new Timer() { Interval = 500 };
                tmr.Tick += Tmr_Tick;
                tmr.Start();
            }
        }

        public void CarregarMapaGPS()
        {
            if (picLoading == null)
            {
                picLoading = new PictureBox()
                {
                    SizeMode = PictureBoxSizeMode.Zoom, 
                    Location = new System.Drawing.Point(0, 0), 
                    Size = new System.Drawing.Size(pnlMapa.Width, pnlMapa.Height)
                };
                pnlMapa.Controls.Add(picLoading);
            }
            mapaService.CarregarMapaGPS(MapasVariaveisModel.NomeArquivoMapaGPS);
            Timer tmr = new Timer() { Interval = 500 };
            tmr.Tick += Tmr_Tick;
            tmr.Start();
        }

        private void Tmr_Tick(object sender, EventArgs e)
        {
            if (mapaService.pythonProcess.HasExited)
            {
                ((Timer)sender).Stop();
                picLoading.Visible = false;
                mapaService.Carregado = true;

                if (mapaService.NomeArquivoMapa == MapasVariaveisModel.NomeArquivoMapaInterativo)
                {
                    mapaService.CarregarDadosMapa();
                    if (lblMapa != null)
                    {
                        lblMapa.Text = "Mapa carregado: " + mapaService.NomeArquivos;
                    }

                    PopularTrajetoria();
                }

                IniciarProcessamento();
            }
        }

        public void PopularTrajetoria()
        {
            if (mapaService.DadosMapa != null)
            {
                double OrientacaoRuas = -999;
                //DistanciaTotal = 0;
                /*if (!RuasPercorrer.Any())
                {
                    RuasPercorrer = mapaService.DadosMapa.features.Select(x => x.geometry.id).ToList();
                }*/
                Trajetoria = new List<List<GPSModel>>();

                foreach (var ID in RuasPercorrer)
                {
                    var rua = mapaService.DadosMapa.features.FirstOrDefault(x => ID == x.geometry.id);
                    if (rua != null)
                    {
                        Trajetoria.Add(new List<GPSModel>());
                        foreach (var ponto in rua.geometry.coordinates)
                        {
                            GPSModel item = new GPSModel()
                            {
                                Longitude = ponto[0],
                                Latitude = ponto[1],
                            };
                            Trajetoria[Trajetoria.Count - 1].Add(item);
                            /*if (Trajetoria[Trajetoria.Count - 1].Count > 1)
                            {
                                DistanciaTotal += GPSService.DistanciaEntrePontos(Trajetoria[Trajetoria.Count - 1][Trajetoria[Trajetoria.Count - 1].Count - 2], item);
                            }*/
                        }

                        var Rua = Trajetoria[Trajetoria.Count - 1];
                        if (OrientacaoRuas == -999)
                        {
                            OrientacaoRuas = GPSService.CalcularOrientacao(Rua[Rua.Count - 1], Rua[Rua.Count - 2]);
                        }
                        else
                        {
                            double MargemErroAngulo = 45;
                            double orientacaoRua = GPSService.CalcularOrientacao(Rua[Rua.Count - 1], Rua[Rua.Count - 2]);
                            double anguloOposto = (orientacaoRua + 180) % 360;
                            if ((anguloOposto + MargemErroAngulo) >= OrientacaoRuas && (anguloOposto - MargemErroAngulo) <= OrientacaoRuas)
                            {
                                Rua.Reverse();
                            }
                        }
                    }
                }
            }
        }

        private void IniciarProcessamento()
        {
            //APIService.Inicializar();

            chromiumWebBrowser = new ChromiumWebBrowser(mapaService.MapaUrl);
            pnlMapa.Controls.Add(chromiumWebBrowser);
            chromiumWebBrowser.Dock = DockStyle.Fill;
        }

        public void PararProcessamento()
        {
            //APIService.Parar();
            //GPSService.PararRecepcaoDados();
        }


        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 double DistanciaAteRua(List<GPSModel> Rua, double larguraRobo)
        {
            GPSModel pontoAtual = GPSService.UltimaLeitura;

            (GPSModel P0, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(Rua, TrajetoriaTipo.Geral);
            int idxP0 = Rua.IndexOf(P0);
            bool PontoProximo = idxP0 < Rua.Count() - 1;
            bool PontoAnterior = idxP0 > 0;
            GPSModel P1 = PontoProximo ? Rua[idxP0 + 1] : PontoAnterior ? Rua[idxP0 - 1] : pontoAtual;
            
            double a = GPSService.DistanciaEntrePontos(pontoAtual, P0);
            double b = GPSService.DistanciaEntrePontos(pontoAtual, P1);
            double c = GPSService.DistanciaEntrePontos(P0, P1);

            double distancia = FuncoesMatematicas.CalcularAlturaTriangulo(a, b, c);

            distancia -= (larguraRobo / 100.0);

            return distancia;
        }


        public void GerarTrajetoriaDinamica()
        {
            double distanciaEntreRuas = DistanciaEntreRuas(Trajetoria) / 2;
            List<GPSModel> Caminho = new List<GPSModel>();

            DirecaoCarroRua direcaoRua = TrajetoriaDinamica.Any() ? TrajetoriaDinamica[0].DirecaoTrecho : ExtremoMaisProximo == 0 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;

            for (int i = IdxRuaAtual; i < Trajetoria.Count(); i++)
            {
                List<GPSModel> RuaAtual = Trajetoria.Skip(i).First();

                double anguloProjetar = GPSService.CalcularOrientacao(RuaAtual.First(), RuaAtual.Skip(1).First()) + 90;

                List<GPSModel> RuaProjetada = ProjetarRua(RuaAtual, distanciaEntreRuas, anguloProjetar);

                if (Caminho.Any())
                {
                    double dist0 = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], RuaProjetada[0]);
                    double dist1 = GPSService.DistanciaEntrePontos(Caminho[Caminho.Count() - 1], RuaProjetada[RuaProjetada.Count() - 1]);
                    direcaoRua = dist0 < dist1 ? DirecaoCarroRua.Ida : DirecaoCarroRua.Volta;
                }

                RuaProjetada.ForEach(x =>
                {
                    x.idxRua = i;
                    x.DirecaoTrecho = direcaoRua;
                });
                if (direcaoRua == DirecaoCarroRua.Volta)
                {
                    RuaProjetada.Reverse();
                }
                double anguloProjetar1 = GPSService.CalcularOrientacao(RuaProjetada.Skip(1).First(), RuaProjetada.First());
                GPSModel PrimeiroPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.First(), 3, anguloProjetar1);
                double anguloProjetar2 = GPSService.CalcularOrientacao(RuaProjetada.Skip(RuaProjetada.Count() - 2).First(), RuaProjetada.Last());
                GPSModel UltimoPonto = GPSService.ProjetarPontoDeslocado(RuaProjetada.Last(), 3, anguloProjetar2);

                if (!Caminho.Any())
                {
                    (GPSModel PontoMaisProximo, bool aproximando) = GPSService.PontoMaisProximoRuaProjetada(RuaProjetada, TrajetoriaTipo.Rua);
                    int idxPontoMaisProximo = RuaProjetada.IndexOf(PontoMaisProximo);

                    GPSModel inicio = GPSService.UltimaLeitura;

                    List<GPSModel> _caminho = new List<GPSModel>();
                    if (idxPontoMaisProximo == 0 && aproximando)
                    {
                        //_caminho.Add(PrimeiroPonto);
                        List<GPSModel> _pontos = GPSService.InterpolarPontos(inicio, PrimeiroPonto, 1.0);
                        if (_pontos.Count() == 1 || GPSService.CompararDirecaoTrajeto(_pontos, RuaProjetada))
                        {
                            _caminho.AddRange(_pontos);
                        }
                    }

                    //bool dentroRua = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.DentroDaRua;
                    bool dentroRua = Variaveis.OperacaoEmAndamento.Mapa.DentroDaRua;

                    // Se estiver manobrando e a manobra ainda não tiver sido concluída, OU, o ponto mais próximo é o ultimo ponto da rua, e já esta fora da rua
                    if (!Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.ManobraConcluida || ((idxPontoMaisProximo == (RuaProjetada.Count() - 1)) && !dentroRua))
                    {
                        _caminho.Add(inicio);
                    }
                    else
                    {
                        _caminho.AddRange(RuaProjetada.Skip(idxPontoMaisProximo));
                        _caminho.Add(UltimoPonto);
                    }
                    _caminho.ForEach(x => x.DirecaoTrecho = direcaoRua);
                    Caminho.AddRange(_caminho);

                    //direcaoRua = direcaoRua == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
                }
                else
                {
                    List<GPSModel> _caminho = new List<GPSModel>();
                    // 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(Caminho[Caminho.Count() - 1], PrimeiroPonto);
                    bool distanciaMinima = distancia < Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.MargemLimiteEntradaRua * 2;
                    
                    // Verifica se o angulo formado entre a rua anterior e a rua atual é grande o bastante para gerar a curva de manobra
                    double anguloEntreRuas = GPSService.CalcularOrientacao(Caminho.Last(), PrimeiroPonto);
                    double anguloFinalRuaAnterior = GPSService.CalcularOrientacao(Caminho.Count() > 1 ? Caminho[Caminho.Count() - 2] : Caminho.First(), Caminho.Last());
                    double difAngulo = anguloFinalRuaAnterior - anguloEntreRuas;
                    bool anguloMinimo = difAngulo >= -45 && difAngulo <= 45;

                    if (distanciaMinima && !anguloMinimo)
                    {
                        GPSModel P0 = CalcularPontoControle(Caminho.Last(), PrimeiroPonto, distanciaEntreRuas * 2);
                        List<GPSModel> CurvaConexao = GerarCurvaConexao(Caminho.Last(), PrimeiroPonto, P0, 6);

                        _caminho.AddRange(CurvaConexao);
                        _caminho.ForEach(x => x.DirecaoTrecho = DirecaoCarroRua.Manobra);
                    }
                    else
                    {
                        _caminho.Add(PrimeiroPonto);
                    }
                    //_caminho.AddRange(GPSService.InterpolarPontos(inicio, PrimeiroPonto, 1.0));
                    _caminho.AddRange(RuaProjetada);
                    _caminho.Add(UltimoPonto);
                    _caminho.ForEach(x => x.DirecaoTrecho = x.DirecaoTrecho != DirecaoCarroRua.Manobra ? direcaoRua : x.DirecaoTrecho);
                    Caminho.AddRange(_caminho);

                    //direcaoRua = direcaoRua == DirecaoCarroRua.Ida ? DirecaoCarroRua.Volta : DirecaoCarroRua.Ida;
                }
            }


            // Limpa a trajetória dinâmica existente e adiciona os novos pontos
            TrajetoriaDinamica.Clear();
            Caminho.ForEach(x =>
            {
                // Impede inclusão de pontos duplicados
                if (!TrajetoriaDinamica.Contains(x))
                {
                    TrajetoriaDinamica.Add(x);
                }
            });
            Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoriaDinamica = 0;

            if (Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.DesvioNecessario)
            {
                AjustarTrajetoriaParaDesvio(Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.ObstaculoCritico);
            }

            GPSService.AtualizarTrajetoriaDinamica();
        }

        public List<GPSModel> ProjetarRua(List<GPSModel> ruaOriginal, double offset, double anguloOffset)
        {
            // Certifique-se de que a lista da rua original não está vazia
            if (ruaOriginal == null || ruaOriginal.Count == 0)
            {
                return new List<GPSModel>();
            }

            List<GPSModel> RuaProjetada = new List<GPSModel>();

            foreach (var Ponto in  ruaOriginal)
            {
                int idx = ruaOriginal.IndexOf(Ponto);
                GPSModel Ponto2 = idx + 1 < ruaOriginal.Count() ? ruaOriginal[idx + 1] : ruaOriginal[idx - 1];
                double angulo = 0;
                if (idx + 1 < ruaOriginal.Count())
                {
                    angulo = GPSService.CalcularOrientacao(Ponto, Ponto2);
                }
                else
                {
                    angulo = GPSService.CalcularOrientacao(Ponto2, Ponto);
                }
                RuaProjetada.Add(GPSService.ProjetarPontoDeslocado(Ponto, offset, angulo + 90));
            }

            return RuaProjetada;
        }


        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 static GPSModel CalcularPontoControle(GPSModel ultimoPontoRuaAtual, GPSModel primeiroPontoProximaRua, double distancia)
        {
            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,
            };
            // Projetar um ponto a uma certa distância na direção calculada
            return GPSService.ProjetarPontoDeslocado(pontoMedio, distancia, angulo + 270);
        }

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





        // Método para gerar o caminho entre dois pontos
        public void GerarCaminhoEntreDoisPontos(GPSModel destino)
        {
            //PopularTrajetoriaComOffset(destino);
            //return;
            // Inicialização do caminho com o ponto de início
            GPSModel inicio = GPSService.UltimaLeitura;

            // Gera waypoints intermediários
            var caminho = GPSService.InterpolarPontos(inicio, destino, 1.0); // 1 metro de distância entre os pontos

            // Limpa a trajetória dinâmica existente e adiciona os novos pontos
            TrajetoriaDinamica.Clear();
            caminho.ForEach(x => TrajetoriaDinamica.Add(x));

            if (Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.DesvioNecessario)
            {
                AjustarTrajetoriaParaDesvio(Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.ObstaculoCritico);
            }

            GPSService.AtualizarTrajetoriaDinamica();
        }

        public void AjustarTrajetoriaParaDesvio(SonarObstaculoDetectadoModel obstaculo)
        {
            GPSModel posicaoAtual = GPSService.UltimaLeitura;

            // Identificar o waypoint mais próximo para iniciar o desvio
            int indiceInicioDesvio = EncontrarIndiceInicioDesvio(TrajetoriaDinamica, posicaoAtual, ((double)obstaculo.DistanciaMedia / 100));

            // Calcular os waypoints de desvio
            var waypointsDesvio = CalcularWaypointsDesvio(posicaoAtual, obstaculo, indiceInicioDesvio);

            // Determinar o intervalo de waypoints a serem removidos
            int indiceFinalDesvio = EncontrarIndiceFinalDesvio(TrajetoriaDinamica, waypointsDesvio.Last(), ((double)obstaculo.DistanciaMedia / 100) + ((double)obstaculo.DeslocamentoNecessarioDesvio * 2 / 100));
            int quantidadeWaypointsRemover = indiceFinalDesvio - indiceInicioDesvio;

            // Remover waypoints obsoletos
            if (quantidadeWaypointsRemover > 0)
            {
                TrajetoriaDinamica.RemoveRange(indiceInicioDesvio + 1, quantidadeWaypointsRemover);
            }

            // Inserir waypoints de desvio na trajetória
            TrajetoriaDinamica.InsertRange(indiceInicioDesvio + 1, waypointsDesvio);
        }

        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
        }

        private int EncontrarIndiceInicioDesvio(List<GPSModel> trajetoria, GPSModel posicaoAtual, double distanciaObstaculo)
        {
            // Este é um método simplificado. Você precisará implementar a lógica para encontrar o waypoint mais adequado.
            return trajetoria.FindIndex(wp => GPSService.DistanciaEntrePontos(wp, posicaoAtual) < distanciaObstaculo);
        }

        private List<GPSModel> CalcularWaypointsDesvio(GPSModel posicaoAtual, SonarObstaculoDetectadoModel obstaculo, int indiceInicioDesvio)
        {
            List<GPSModel> waypointsDesvio = new List<GPSModel>();

            double anguloCaminho = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;

            // Calcular o ângulo e distância para o desvio
            double anguloDesvio = anguloCaminho + ((obstaculo.DirecaoDesvio == Enums.Direcao.Direita ? 270 : 90) + obstaculo.AnguloParaDesvio);
            double deslocamento = (double)obstaculo.DistanciaMedia / 100; // Math.Sqrt(Math.Pow(((double)obstaculo.DeslocamentoNecessarioDesvio / 100), 2) + Math.Pow(((double)obstaculo.DistanciaMedia / 100), 2));

            // Waypoint de início do desvio
            GPSModel waypointInicioDesvio = CalcularNovoWaypoint(posicaoAtual, anguloDesvio, deslocamento);
            waypointsDesvio.Add(waypointInicioDesvio);

            // Waypoint de contorno do obstáculo
            double anguloContorno = 270 - anguloCaminho; // Ajustar conforme a orientação do desvio
            GPSModel waypointContornoObstaculo = CalcularNovoWaypoint(waypointInicioDesvio, anguloContorno, ((double)obstaculo.Largura / 100)); //((double)obstaculo.Largura / 100) 
            waypointsDesvio.Add(waypointContornoObstaculo);

            // Waypoint de retorno à trajetória
            double anguloRetorno = anguloCaminho + ((obstaculo.DirecaoDesvio == Enums.Direcao.Direita ? 90 : 270) - obstaculo.AnguloParaDesvio); //anguloDesvio - obstaculo.AnguloParaDesvio - 180;// Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.AnguloCaminho + (obstaculo.AnguloParaDesvio); // Pode precisar ajustar baseado na orientação do desvio
            GPSModel waypointRetorno = CalcularNovoWaypoint(waypointContornoObstaculo, anguloRetorno, deslocamento);
            waypointsDesvio.Add(waypointRetorno);

            return waypointsDesvio;
        }

        private GPSModel CalcularNovoWaypoint(GPSModel posicaoAtual, double angulo, double distancia)
        {
            double anguloRad = FuncoesMatematicas.ConvertDegreesToRadians(angulo);
            double deltaLatitude = Math.Cos(anguloRad) * distancia / 111111; // Conversão para graus
            double deltaLongitude = Math.Sin(anguloRad) * distancia / (111111 * Math.Cos(FuncoesMatematicas.ConvertDegreesToRadians(posicaoAtual.Latitude)));

            return new GPSModel
            {
                Latitude = posicaoAtual.Latitude + deltaLatitude,
                Longitude = posicaoAtual.Longitude + deltaLongitude
            };
        }



        public void PopularTrajetoriaComOffset(GPSModel destino)
        {
            if (mapaService.DadosMapa != null)
            {
                GPSModel inicio = GPSService.UltimaLeitura;

                // Coleta os primeiros pontos de cada rua com base na orientação e posição atual do robô
                var pontosExtremosComOffset = new List<GPSModel>();

                foreach (var rua in mapaService.DadosMapa.features)
                {
                    var primeiroPonto = new GPSModel()
                    {
                        Latitude = rua.geometry.coordinates.First()[1], 
                        Longitude = rua.geometry.coordinates.First()[0]
                    };
                    var ultimoPonto = new GPSModel()
                    {
                        Latitude = rua.geometry.coordinates.Last()[1], 
                        Longitude = rua.geometry.coordinates.Last()[0]
                    };
                    GPSModel pontoInicial = new GPSModel();
                    GPSModel pontoSecundario = new GPSModel();

                    if (GPSService.DistanciaEntrePontos(inicio, primeiroPonto) < GPSService.DistanciaEntrePontos(inicio, ultimoPonto))
                    {
                        pontoInicial = primeiroPonto;
                        pontoSecundario = new GPSModel()
                        {
                            Latitude = rua.geometry.coordinates.Skip(1).First()[1], 
                            Longitude = rua.geometry.coordinates.Skip(1).First()[0]
                        };
                    }
                    else
                    {
                        pontoInicial = ultimoPonto;
                        pontoSecundario = new GPSModel()
                        {
                            Latitude = rua.geometry.coordinates.Skip(rua.geometry.coordinates.Count - 2).First()[1], 
                            Longitude = rua.geometry.coordinates.Skip(rua.geometry.coordinates.Count - 2).First()[0]
                        };
                    }

                    var orientacaoRua = GPSService.CalcularOrientacao(pontoInicial, pontoSecundario);

                    // Calcula o ponto com offset baseado no ângulo oposto à orientação da rua
                    var anguloOffset = (orientacaoRua + 180) % 360;
                    var distanciaOffset = 10; // Defina o offset desejado aqui

                    // Adiciona o ponto com offset à lista
                    pontosExtremosComOffset.Add(new GPSModel
                    {
                        Latitude = pontoInicial.Latitude,// + (Math.Cos(anguloOffset * (Math.PI / 180)) * distanciaOffset),
                        Longitude = pontoInicial.Longitude// + (Math.Sin(anguloOffset * (Math.PI / 180)) * distanciaOffset),
                    });
                }

                var pontosRelevantes = FiltrarPontosRelevantes(inicio, destino, pontosExtremosComOffset);

                // Ordena os pontos com offset pela proximidade com a posição atual
                var pontosOrdenados = pontosRelevantes.OrderBy(p => GPSService.DistanciaEntrePontos(inicio, p)).ToList();

                // Agora, conecta os pontos ordenados para formar a trajetória
                TrajetoriaDinamica.Clear();
                GPSModel pontoAnterior = inicio;
                foreach (var ponto in pontosOrdenados)
                {
                    //var caminho = GPSService.InterpolarPontos(pontoAnterior, ponto, 1.0);
                    //caminho.ForEach(x => TrajetoriaDinamica.Add(x));
                    //pontoAnterior = ponto;
                    TrajetoriaDinamica.Add(ponto);
                }

                // Atualiza a trajetória dinâmica
                GPSService.AtualizarTrajetoriaDinamica();
            }
        }
        private List<GPSModel> FiltrarPontosRelevantes(GPSModel inicio, GPSModel destino, List<GPSModel> todasRuas)
        {
            var ruasRelevantes = new List<GPSModel>();

            // Define os limites do quadrante
            double minLatitude = Math.Min(inicio.Latitude, destino.Latitude);
            double maxLatitude = Math.Max(inicio.Latitude, destino.Latitude);
            double minLongitude = Math.Min(inicio.Longitude, destino.Longitude);
            double maxLongitude = Math.Max(inicio.Longitude, destino.Longitude);

            // Filtra previamente os pontos que estão dentro do quadrante definido
            var pontosNoQuadrante = todasRuas.Where(ponto => ponto.Latitude >= minLatitude && ponto.Latitude <= maxLatitude
                                                            && ponto.Longitude >= minLongitude && ponto.Longitude <= maxLongitude).ToList();

            // Continua a filtragem dos pontos relevantes como antes, mas agora apenas com os pontos dentro do quadrante
            GPSModel pontoMaisProximoInicio = IdentificarPontoMaisProximo(inicio, pontosNoQuadrante);
            if (pontoMaisProximoInicio != null && !ruasRelevantes.Contains(pontoMaisProximoInicio))
            {
                ruasRelevantes.Add(pontoMaisProximoInicio);
            }

            GPSModel ultimoPontoRelevante = pontoMaisProximoInicio;

            foreach (var ponto in pontosNoQuadrante)
            {
                if (ponto.Equals(inicio) || ponto.Equals(pontoMaisProximoInicio)) continue;

                double margem = 20;
                double distanciaPontoAnterior = GPSService.DistanciaEntrePontos(ultimoPontoRelevante, destino);
                double distanciaPontoAtual = GPSService.DistanciaEntrePontos(ponto, destino);
                double diferenca = (distanciaPontoAnterior - distanciaPontoAtual);
                bool relevante = diferenca > 0 && diferenca < margem;

                if (relevante || distanciaPontoAtual == 0 || true)
                {
                    ruasRelevantes.Add(ponto);
                    ultimoPontoRelevante = ponto;
                }

                if (distanciaPontoAtual == 0)
                {
                    break;
                }
            }

            return ruasRelevantes;
        }

        // Método auxiliar para identificar o ponto mais próximo a um dado ponto de referência
        private GPSModel IdentificarPontoMaisProximo(GPSModel referencia, List<GPSModel> pontos)
        {
            GPSModel pontoMaisProximo = null;
            double distanciaMinima = double.MaxValue;

            foreach (var ponto in pontos)
            {
                double distancia = GPSService.DistanciaEntrePontos(referencia, ponto);
                if (distancia < distanciaMinima)
                {
                    distanciaMinima = distancia;
                    pontoMaisProximo = ponto;
                }
            }

            return pontoMaisProximo;
        }



    }

    public class MapaFeatureCollectionModel
    {
        public string type { get; set; }
        public List<MapaFeatureModel> features { get; set; }
    }

    public class MapaFeatureModel
    {
        public string type { get; set; }
        public MapaFeaturePropertiesModel properties { get; set; }
        public MapaFeatureGeometryModel geometry { get; set; }
    }

    public class MapaFeaturePropertiesModel
    {
        public string Id { get; set; }
        public string Name { get; set; }
        public double Length { get; set; }
        public double Dist1 { get; set; }
        public double Dist2 { get; set; }
    }

    public class MapaFeatureGeometryModel
    {
        public string id { get; set; }
        public string type { get; set; }
        public List<List<double>> coordinates { get; set; }
    }



}