agrobot_base/AgroBase/AgroBase/Forms/frmSimulacaoMapaGPS.cs

using AgroBase.Models;
using AgroBase.Services;
using CefSharp;
using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using static AgroBase.Models.Enuns;

namespace AgroBase.Forms
{
    public partial class frmSimulacaoMapaGPS : Form
    {
        Timer tmrSonar;
        StatusCarroMapa StatusCarro = StatusCarroMapa.Parado;

        public frmSimulacaoMapaGPS()
        {
            InitializeComponent();

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

            tmrSonar = new Timer() { Interval = 100 };
            tmrSonar.Tick += tmrSonar_Tick;
            tmrSonar.Start();
        }

        

        private void Tmr_Tick(object sender, EventArgs e)
        {
            StatusCarro = Variaveis.OperacaoEmAndamento.OpMapaGPS.ReferencialRuaGPS.StatusAtual;

            lblStatus.Text = "Status: " + 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";

            if (Variaveis.OperacaoEmAndamento.Iniciado)
            {
                if (StatusCarro != StatusCarroMapa.Manobrando && StatusCarro != StatusCarroMapa.SaindoRua)
                {
                    Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
                }

                pnlOrientacaoTrajeto.Invalidate();
            }
            else
            {
                Variaveis.OperacaoEmAndamento.OpMapaGPS.AtualizarRuasSelecionadas();
            }
        }


        private void frmSimulacaoMapaGPS_Load(object sender, EventArgs e)
        {
            Variaveis.OperacaoEmAndamento.Mapa = new MapasModel()
            {
                pnlMapa = pnlMapa,
            };
            Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal = new SonarModel(picSonar);
        }

        private void frmSimulacaoMapaGPS_FormClosing(object sender, FormClosingEventArgs e)
        {
            tmrSonar.Stop();
        }

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

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

            GPSService.AtualizarCoordenadasGPS();
        }

        private void btnAcrescentarGPS_Click(object sender, EventArgs e)
        {
            double angulo = Variaveis.OperacaoEmAndamento.Mapa.AnguloCaminho;
            if (txtAnguloGPS.Text != "")
            {
                angulo = double.Parse(txtAnguloGPS.Text);
            }
            double distancia = double.Parse(txtDistanciaGPS.Text); // Distância em metros
            //angulo -= 180; // Ângulo em graus

            double latitude = GPSService.UltimaLeitura.Latitude;
            double longitude = GPSService.UltimaLeitura.Longitude;

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

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

            AtualizarPosicaoGPS();

            if (chbMoverObstaculo.Checked && ObjetoDetectado && double.TryParse(txtDistanciaSonar.Text, out double distSonar))
            {
                distSonar -= (distancia * 100);
                txtDistanciaSonar.Text = distSonar.ToString();
            }

            btnCalcularAngulo_Click(sender, e);

            btnAtualizarLeitura_Click(sender, e);
        }

        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;
            }
            Variaveis.OperacaoEmAndamento.Iniciado = !Variaveis.OperacaoEmAndamento.Iniciado;
            btnIniciarSimulacao.Text = Variaveis.OperacaoEmAndamento.Iniciado ? "Parar Simulação" : "Iniciar Simulação";
            Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica();
        }

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


        bool ObjetoDetectado = false;
        Direcao dirSonar = Direcao.Esquerda;
        int angulo = 0;
        int posicao = 1;
        int distanciaMaxima = 356;
        private void tmrSonar_Tick(object sender, EventArgs e)
        {
            if (Variaveis.OperacaoEmAndamento.DispSen.Dados.Conectado && Variaveis.OperacaoEmAndamento.DispSen.Dados.Sensores.Any(x => x.Componente == S_Code.sSNR && x.Inicializado) || !chbSonarVirando.Checked)
            {
                return;
            }

            int.TryParse(txtAnguloInicial.Text, out int anguloInicial);
            int.TryParse(txtAnguloFinal.Text, out int anguloFinal);
            int distancia = ObjetoDetectado && angulo >= anguloInicial && angulo <= anguloFinal ? int.Parse(txtDistanciaSonar.Text) : distanciaMaxima;

            var posArr = Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.Deteccoes.FirstOrDefault(x => x.Posicao == posicao);
            if (posArr == null)
            {
                posArr = new SonarRegistroModel()
                {
                    Posicao = posicao,
                    Leituras = new List<SonarLeituraModel>()
                };
                Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.Deteccoes.Add(posArr);
            }
            var leituras = posArr.Leituras;
            if (leituras.Any(x => x.Angulo == angulo))
            {
                leituras.First(x => x.Angulo == angulo).Momento = DateTime.Now;
                leituras.First(x => x.Angulo == angulo).Distancia = distancia;
            }
            else
            {
                leituras.Add(new SonarLeituraModel(angulo, distancia));
            }
            Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.PosicaoAtual = posicao;
            Variaveis.OperacaoEmAndamento.Sensoriamento.SonarFrontal.AtualizarSonar();

            angulo += dirSonar == Direcao.Esquerda ? SonarModel.anguloAcrescentar : -SonarModel.anguloAcrescentar;

            if (angulo >= 180)
            {
                dirSonar = Direcao.Direita;
                //posArr.Leituras.Clear();
            }
            else if (angulo <= 0)
            {
                dirSonar = Direcao.Esquerda;
                //posArr.Leituras.Clear();
            }
        }

        private void btnAdicionarDeteccao_Click(object sender, EventArgs e)
        {
            ObjetoDetectado = !ObjetoDetectado;
            btnAdicionarDeteccao.Text = ObjetoDetectado ? "Del" : "Add";
        }

        private void btnAtualizarLeitura_Click(object sender, EventArgs e)
        {
            Tmr_Tick(new Timer(), e);
        }

        private void btnCalcularAngulo_Click(object sender, EventArgs e)
        {
            Variaveis.OperacaoEmAndamento.OpMapaGPS.CalculaDadosMovimentacaoAutonoma();

            double anguloAtual = double.Parse(txtAnguloGPS.Text);
            double novoAngulo = (anguloAtual + Variaveis.OperacaoEmAndamento.OpMapaGPS.Controle.Angulo) % 360;

            txtAnguloGPS.Text = novoAngulo.ToString("0.00");

            double distancia = Math.Round((double)Variaveis.OperacaoEmAndamento.OpMapaGPS.Controle.RPM / 45.0, 2);
            txtDistanciaGPS.Text = distancia.ToString("0.00");

            pnlOrientacaoTrajeto.Invalidate();
        }
    }
}