agrobot_base/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs

using AgroBase.Models;
using AgroBase.Services;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Threading.Tasks;
using System.Windows.Forms;
using static AgroBase.Models.Enums;

namespace AgroBase.Forms
{
    public partial class frmSimulacaoMapaGPS : Form
    {
        AsyncTaskTimerModel tmrLeitura;
        StatusCarroMapa StatusCarro = StatusCarroMapa.Parado;
        MapaDinamicoModel MapaDinamico;
        bool calc = false;
        bool parar = false;
        DateTime UltimaAtualizacao = DateTime.MinValue;
        int tempoGps = 1000;
        DateTime UltimaAtualizacaoGps = DateTime.MinValue;
        double ultimaLat = 0;
        double ultimaLon = 0;
        double ultimoAnguloGps = 0;

        Queue<double> historicoAngulosControle = new Queue<double>();
        Queue<double> historicoAngulosCarro = new Queue<double>();

        public frmSimulacaoMapaGPS()
        {
            InitializeComponent();

            // Ativa o double buffering
            this.DoubleBuffered = true;
            this.SetStyle(ControlStyles.AllPaintingInWmPaint, true);
            this.SetStyle(ControlStyles.UserPaint, true);
            this.SetStyle(ControlStyles.OptimizedDoubleBuffer, true);

            MapaDinamico = new MapaDinamicoModel(this, pnlZoomMapa);
        }

        private void frmSimulacaoMapaGPS_Load(object sender, EventArgs e)
        {
            tmrLeitura = new AsyncTaskTimerModel("tmrLeitura", tmrLeitura_Tick, 500, this);
            tmrLeitura.Start();

            Variaveis.OperacaoEmAndamento.Mapa = new MapasModel()
            {
                pnlMapa = pnlMapa,
            };
            //Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal = new SonarModel(picSonar);
            KinectService.InicializarKinect(null, picSonar, null, null);

            txtKd.Text = Variaveis.OperacaoEmAndamento.Controle.PIDdirecional.Kd.ToString();
            txtKi.Text = Variaveis.OperacaoEmAndamento.Controle.PIDdirecional.Ki.ToString();
            txtKp.Text = Variaveis.OperacaoEmAndamento.Controle.PIDdirecional.Kp.ToString();
        }

        private void frmSimulacaoMapaGPS_FormClosing(object sender, FormClosingEventArgs e)
        {
            tmrLeitura.Dispose();
            Variaveis.OperacaoEmAndamento.Iniciado = false;
        }

        private async Task tmrLeitura_Tick()
        {
            if (chbAutomatico.Checked)
            {
                btnAcrescentarGPS_Click(btnAcrescentarGPS, new EventArgs());
            }
        }

        private void AtualizarDadosTela()
        {
            btnCalcularAngulo_Click(new object(), new EventArgs());

            StatusCarro = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.StatusAtual;

            lblStatusOperacao.Text = "Operação: " + Enum.GetName(typeof(StatusOperacao), Variaveis.OperacaoEmAndamento.StatusAtual);
            lblStatus.Text = "Carro: " + Enum.GetName(typeof(StatusCarroMapa), StatusCarro);
            lblDirecao.Text = "Direção: " + Enum.GetName(typeof(DirecaoCarroRua), Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.Direcao);
            lblDistanciaOperacao.Text = "Distância Operação: " + (Variaveis.OperacaoEmAndamento.Mapa == null ? 0 : Variaveis.OperacaoEmAndamento.Mapa.DistanciaTotal).ToString("00.00") + " m";
            lblDistanciaFinal.Text = "Distância Final: " + (GPSService.DistanciaDoTrecho(Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica)).ToString("00.00") + " m";
            lblDistanciaLateral.Text = "Esq: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaEsquerda.ToString("0.00") + " m - Dir: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaDireita.ToString("0.00") + " m";

            lblRua.Text = "Rua: " + (Variaveis.OperacaoEmAndamento.Mapa.DentroDaRua ? "Dentro" : "Fora");
            lblMargem.Text = "Margem: " + (Variaveis.OperacaoEmAndamento.Mapa.NaMargemEntradaRua ? "Sim" : "Não");
            lblProximoPonto.Text = "Próximo Ponto: " + (Variaveis.OperacaoEmAndamento.Mapa.IdxUltimoPontoTrajetoria + 1);
            lblAproximando.Text = (Variaveis.OperacaoEmAndamento.Mapa.AproximandoUltimoPontoTrajetoria ? "Aproximando" : "Afastando");
            lblDistProx.Text = "Distância Próximo Ponto: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaAteProximoPonto.ToString("0.00") + " m";
            lblDistAnt.Text = "Distância Ponto Anterior: " + Variaveis.OperacaoEmAndamento.Mapa.DistanciaAtePontoAnterior.ToString("0.00") + " m";

            lblTempoEstimado.Text = "Tempo Estimado: " + Variaveis.OperacaoEmAndamento.TempoEstimadoRestante();

            if (Variaveis.OperacaoEmAndamento.Iniciado)
            {
                pnlOrientacaoTrajeto.Invalidate();
                pnlAnguloControle.Invalidate();
                pnlOrientacaoCarro.Invalidate();

                MapaDinamico.AtualizarDados(
                    KinectService.Iniciado && KinectService.Leitura.StatusDetecao ? KinectService.Leitura.ObstaculoCritico : null,
                    (float)Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho,
                    (float)Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro,
                    GPSService.UltimaLeitura,
                    Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaDinamica,
                    Variaveis.OperacaoEmAndamento.Mapa.TrajetoriaProjetada.FirstOrDefault(),
                    Variaveis.OperacaoEmAndamento.GPSTrajetoria
                );
            }
            else
            {
                if (btnIniciarSimulacao.Text.Contains("Parar"))
                {
                    parar = true;
                    btnIniciarSimulacao_Click(new object(), new EventArgs());
                    parar = false;
                }

                Variaveis.OperacaoEmAndamento.OpMapaGPS.AtualizarRuasSelecionadas();
            }
        }


        

        private void btnCarregar_Click(object sender, EventArgs e)
        {
            Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click((Control) sender, e);
        }

        private void btnIniciarGPS_Click(object sender, EventArgs e)
        {
            AtualizarPosicaoGPS();
            btnAcrescentarGPS.Enabled = true;
            txtAnguloGPS.Enabled = true;
            txtDistanciaGPS.Enabled = true;
            btnIniciarGPS.Enabled = false;
            txtLatitude.Enabled = false;
            txtLongitude.Enabled = false;
        }

        private void AtualizarPosicaoGPS()
        {
            GPSService.PenultimaLeitura = new GPSModel()
            {
                DataHora = GPSService.UltimaLeitura.DataHora,
                Longitude = GPSService.UltimaLeitura.Longitude,
                Latitude = GPSService.UltimaLeitura.Latitude,
            };

            if (!GPSService.Iniciado || (txtLongitude.Text != "" && txtLatitude.Text != ""))
            {
                GPSService.UltimaLeitura = new GPSModel()
                {
                    DataHora = DateTime.Now,
                    Longitude = double.Parse(txtLongitude.Text.Replace(".", ",")),
                    Latitude = double.Parse(txtLatitude.Text.Replace(".", ",")),
                };
            }

            ultimaLat = GPSService.UltimaLeitura.Latitude;
            ultimaLon = GPSService.UltimaLeitura.Longitude;

            GPSService.AtualizarCoordenadasGPS();
        }

        private void btnAcrescentarGPS_Click(object sender, EventArgs e)
        {
            bool atualizarCoordenadas = UltimaAtualizacaoGps.AddMilliseconds(tempoGps) < DateTime.Now;

            double angulo = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
            if (txtAnguloGPS.Text != "")
            {
                angulo = atualizarCoordenadas ? double.Parse(txtAnguloGPS.Text) : ultimoAnguloGps;
            }
            if (Variaveis.OperacaoEmAndamento.Controle.TipoMovimento == TipoMovimentoDirecional.MovimentoArco)
            {
                angulo += double.Parse(txtAnguloControle.Text);
            }


            double velocidade = FuncoesMatematicas.CalculaVelocidadeRPM(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle);
            double velocidadeMs = FuncoesMatematicas.ConverteKmhParaMs(velocidade);
            txtVelocidade.Text = velocidade.ToString("0.00");

            TimeSpan tempoPassado = !chbAutomatico.Checked || UltimaAtualizacao == DateTime.MinValue ? new TimeSpan(5000000) : DateTime.Now - UltimaAtualizacao;
            // Calcula a distância com base no tempo passado (em segundos)
            double distancia = velocidadeMs * tempoPassado.TotalSeconds;
            
            if (distancia == 0 && double.Parse(txtDistanciaGPS.Text) > 0)
            {
                distancia = double.Parse(txtDistanciaGPS.Text);
            }
            else
            {
                txtDistanciaGPS.Text = distancia.ToString("0.00");
            }
            //double distancia = double.Parse(txtDistanciaGPS.Text); // Distância em metros
            //angulo -= 180; // Ângulo em graus

            double latitude = ultimaLat;
            double longitude = ultimaLon;

            // Conversão de ângulo de graus para radianos
            double anguloRad = angulo * (Math.PI / 180);

            // Raio da Terra em metros
            double raioTerra = GPSService.RaioDaTerra;

            // Calcular deslocamento de latitude em radianos
            double deltaLat = distancia * Math.Cos(anguloRad) / raioTerra;
            // Converter de radianos para graus
            double _latAtt = latitude + deltaLat * (180 / Math.PI);

            // Calcular deslocamento de longitude em radianos
            double deltaLong = distancia * Math.Sin(anguloRad) / (raioTerra * Math.Cos(latitude * (Math.PI / 180)));
            // Converter de radianos para graus
            double _longAtt = longitude + deltaLong * (180 / Math.PI);

            if (atualizarCoordenadas)
            {
                if (double.IsNaN(_latAtt) || double.IsNaN(_longAtt))
                {

                }

                // Atualizar os TextBoxes com as novas coordenadas
                txtLatitude.Text = _latAtt.ToString();
                txtLongitude.Text = _longAtt.ToString();

                UltimaAtualizacaoGps = DateTime.Now;

                ultimoAnguloGps = angulo;

                AtualizarPosicaoGPS();

                calc = true;
            }
            else
            {
                ultimaLat = _latAtt;
                ultimaLon = _longAtt;
            }

            AtualizarDadosTela();
            calc = false;
        }

        private void btnIniciarSimulacao_Click(object sender, EventArgs e)
        {
            if (!Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Any())
            {
                MessageBox.Show("Selecione as ruas para realizar a operação!");
                return;
            }
            
            if (!Variaveis.OperacaoEmAndamento.Iniciado && !parar)
            {
                Variaveis.OperacaoEmAndamento.IniciarSimulacao();
            }

            btnIniciarSimulacao.Text = Variaveis.OperacaoEmAndamento.Iniciado ? "Parar Simulação" : "Iniciar Simulação";
            if (Variaveis.OperacaoEmAndamento.Iniciado)
            {
                GPSService.PenultimaLeitura.Latitude = GPSService.UltimaLeitura.Latitude;
                GPSService.PenultimaLeitura.Longitude = GPSService.UltimaLeitura.Longitude;
                Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();

                AtualizarDadosTela();
            }
        }

        private void btnCarregarMapa_Click(object sender, EventArgs e)
        {
            Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click((Control)sender, e);
        }

        private void pnlOrientacaoTrajeto_Paint(object sender, PaintEventArgs e)
        {
            try
            {
                double angulo = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
                DesenharInclinacao((Panel)sender, e, (float)angulo, 90);
            }
            catch { }
        }

        private void pnlOrientacaoCarro_Paint(object sender, PaintEventArgs e)
        {
            try
            {
                double angulo = Variaveis.OperacaoEmAndamento.Sensoriamento.AnguloCarro;
                DesenharInclinacao((Panel)sender, e, (float)angulo, 90);
            }
            catch { }
        }

        private void pnlAnguloControle_Paint(object sender, PaintEventArgs e)
        {
            try
            {
                double angulo = Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle;
                DesenharInclinacao((Panel)sender, e, (float)angulo, 90);
            }
            catch { }
        }

        private void DesenharInclinacao(Panel pnlBussola, PaintEventArgs e, float anguloInclinacao, float anguloSub)
        {
            if (!(anguloInclinacao >= 0 || anguloInclinacao <= 0))
            {
                anguloInclinacao = 0;
            }

            Graphics g = e.Graphics;
            // Desenha o círculo da bússola
            int diameter = Math.Min(pnlBussola.Width - 1, pnlBussola.Height - 1);
            Point centro = new Point((pnlBussola.Width - 1) / 2, (pnlBussola.Height - 1) / 2);
            //g.DrawEllipse(Pens.Black, centro.X - diameter / 2, centro.Y - diameter / 2, diameter, diameter);

            // Escreve o ângulo no centro do círculo
            string anguloTexto = $"{anguloInclinacao.ToString("0")}°";
            Font fonteAngulo = new Font("Arial", 7); // Ajuste o tamanho da fonte conforme necessário
            SizeF textSize = e.Graphics.MeasureString(anguloTexto, fonteAngulo);

            // Calcula a posição para o texto ser centralizado no círculo
            float textX = 0;
            float textY = 0;

            // Define a cor do texto
            Brush textBrush = Brushes.Black;

            // Desenha o texto no Graphics do painel
            g.DrawString(anguloTexto, fonteAngulo, textBrush, textX, textY);

            // A linha será desenhada do centro do círculo
            PointF pontoCentral = new PointF(centro.X, centro.Y);
            // O comprimento da linha será metade do diâmetro do círculo (raio)
            float comprimentoLinha = diameter / 3;

            // Ajustar o ângulo para que 0 graus esteja para cima e aumente no sentido horário
            //float anguloRadianos = (anguloInclinacao - 90) * (float)(Math.PI / 180.0);
            float anguloRadianos = (anguloInclinacao - anguloSub) * (float)(Math.PI / 180.0);

            // Calcular o ponto final da linha baseado no ângulo de inclinação
            PointF pontoFinal = new PointF(
                pontoCentral.X + comprimentoLinha * (float)Math.Cos(anguloRadianos),
                pontoCentral.Y + comprimentoLinha * (float)Math.Sin(anguloRadianos)
            );

            // Desenhar a linha de inclinação
            g.DrawLine(Pens.Red, pontoCentral, pontoFinal);

            // Opcional: Desenhar uma ponta de seta na linha para indicar direção
            using (AdjustableArrowCap bigArrow = new AdjustableArrowCap(4, 4))
            {
                using (Pen pen = new Pen(Color.Red, 2))
                {
                    pen.CustomEndCap = bigArrow;
                    g.DrawLine(pen, pontoCentral, pontoFinal);
                }
            }
        }

        private void btnAtualizarLeitura_Click(object sender, EventArgs e)
        {
            AtualizarDadosTela();
        }

        private void btnCalcularAngulo_Click(object sender, EventArgs e)
        {
            Variaveis.OperacaoEmAndamento.Controle.PIDdirecional.Kd = double.Parse(txtKd.Text);
            Variaveis.OperacaoEmAndamento.Controle.PIDdirecional.Ki = double.Parse(txtKi.Text);
            Variaveis.OperacaoEmAndamento.Controle.PIDdirecional.Kp = double.Parse(txtKp.Text);

            Variaveis.OperacaoEmAndamento.OpMapaGPS.CalculaDadosMovimentacaoAutonoma();

            int errosConsiderar = int.Parse(txtQtdErro.Text);

            double anguloControle = Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle;

            if (calc)
            {
                double anguloAtual = double.Parse(txtAnguloGPS.Text);
                double novoAngulo = (anguloAtual + anguloControle) % 360;

                if (historicoAngulosCarro.Count >= (errosConsiderar + 1))
                    historicoAngulosCarro.Dequeue(); // Remove o valor mais antigo

                historicoAngulosCarro.Enqueue(novoAngulo);
                double anguloCarro = historicoAngulosCarro.Peek(); // Valor de 5 ciclos atrás

                if (double.IsNaN(anguloCarro))
                {

                }

                txtAnguloGPS.Text = anguloCarro.ToString("0.00");
            }

            if (historicoAngulosControle.Count >= errosConsiderar && errosConsiderar > 0)
                historicoAngulosControle.Dequeue(); // Remove o valor mais antigo

            historicoAngulosControle.Enqueue(anguloControle);
            double anguloAtrasado = historicoAngulosControle.Peek(); // Valor de 5 ciclos atrás

            txtAnguloControle.Text = anguloAtrasado.ToString("0.00");

            UltimaAtualizacao = DateTime.Now;

            pnlOrientacaoTrajeto.Invalidate();
            pnlOrientacaoCarro.Invalidate();
        }

        private void chbSonarVirando_CheckedChanged(object sender, EventArgs e)
        {
            Variaveis.OperacaoEmAndamento.Controle.SonarAtivado = ((CheckBox)sender).Checked;
        }

        private void chbAutomatico_CheckedChanged(object sender, EventArgs e)
        {
            if (chbAutomatico.Checked)
            {
                UltimaAtualizacao = DateTime.Now.AddSeconds(-1);
            }
        }

        private void btnReiniciar_Click(object sender, EventArgs e)
        {
            Variaveis.OperacaoEmAndamento.ReiniciarSimulacao();
            frmSimulacaoMapaGPS frm = new frmSimulacaoMapaGPS();
            frm.Show();

            this.Close();
        }
    }
}