agrobot_base/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs

using AgroBase.Models;
using AgroBase.Services;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.IO;
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;
        MapaDinamicoModel MapaDinamico;
        bool parar = false;
        double tempoGps = (1000.0 / GPSService.TaxaAmostragemHz);
        int ticksControleMax = 0;
        int ticksControle = 0;
        List<GPSModel> passosOperacao = new List<GPSModel>();
        int idxPassoOperacao = 0;
        double velocidadeCarroMs = 0.0;

        DateTime UltimaAtualizacaoGps = DateTime.MinValue;

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

        List<MPCSimulacaoModel> simulacao = new List<MPCSimulacaoModel>();

        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)
        {
            cmbTipoControleDirecional.Items.Clear();
            cmbTipoControleDirecional.Items.AddRange(Enum.GetNames(typeof(Enums.TiposControladorDirecional)));
            cmbTipoControleDirecional.SelectedIndex = (int)Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional;

            cmbStatusCorredor.Items.Clear();
            cmbStatusCorredor.Items.AddRange(Enum.GetNames(typeof(Enums.StatusCarroMapa)));
            cmbStatusCorredor.SelectedIndex = (int)StatusCarroMapa.Parado;

            Variaveis.OperacaoEmAndamento.ReiniciarSimulacao();

            tmrLeitura = new AsyncTaskTimerModel("tmrLeitura", tmrLeitura_Tick, (int)tempoGps, this);
            tmrLeitura.Start();

            txtTempoGPS.Text = tempoGps.ToString();

            Variaveis.OperacaoEmAndamento.Mapa = new MapasModel()
            {
                pnlMapa = pnlMapa,
            };
        }

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

        DateTime t0 = DateTime.Now;
        private async Task tmrLeitura_Tick()
        {
            double latencia_tick = (DateTime.Now - t0).TotalSeconds;
            t0 = DateTime.Now;
            int.TryParse(txtTempoGPS.Text, out int tempo);
            if (tempo > 0)
            {
                tempoGps = tempo;
            }

            Func<bool> funcAtt = new Func<bool>(() =>
            {
                AtualizarDadosTela();
                return true;
            });
            Task.Run(() => FuncoesGlobais.ExecutarMetodoComVerificacaoCrossThread(this, funcAtt));

            if (chbAutomatico.Checked)
            {
                btnAcrescentarGPS_Click(btnAcrescentarGPS, new EventArgs());
            }

            DateTime t1 = DateTime.Now;
            double latencia_exec = (t1 - t0).TotalSeconds;
            double frequencia_exec = 1.0 / Math.Max(latencia_exec, 1e-6);
            double frequencia_tick = 1.0 / Math.Max(latencia_tick, 1e-6);
            //Console.WriteLine($"Tempo por tick GPS: {Math.Round(latencia_exec, 4)} s, Frequencia: {Math.Round(frequencia_exec, 2)} Hz | Tempo entre tick GPS: {Math.Round(latencia_tick, 4)}, Frequencia: {Math.Round(frequencia_tick, 2)} Hz");
            double novoDelay = Math.Max(tempoGps - (latencia_exec * 1000.0), 1);
            tmrLeitura.SetInterval((int)novoDelay);
        }

        bool AtualizandoTela = false;
        private void AtualizarDadosTela()
        {
            if (AtualizandoTela)
                return;
            AtualizandoTela = true;

            var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;
            var _Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria;

            lblStatusOperacao.Text = "Operação: " + Enum.GetName(typeof(StatusOperacao), _Sensoriamento.StatusOperacao);
            lblTipoMovimento.Text = _Sensoriamento.Controle.TipoMovimento.ToString();

            if (_Trajetoria == null)
            {
                Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas();
                return;
            }

            lblStatus.Text = "Carro: " + Enum.GetName(typeof(StatusCarroMapa), _Sensoriamento.Trajetoria.StatusCarro);
            lblDirecao.Text = "Direção: " + Enum.GetName(typeof(DirecaoCarroRua), _Sensoriamento.Trajetoria.DirecaoCaminho);
            lblDistanciaOperacao.Text = "Distância Operação: " + _Sensoriamento.Trajetoria.DistanciaPercorrida.ToString("0.00") + " m de " + _Sensoriamento.Trajetoria.DistanciaTotal.ToString("0.00") + " m";
            lblDistanciaFinal.Text = "Distância Restante: " + _Sensoriamento.Trajetoria.DistanciaRestante.ToString("0.00") + " m";
            lblDistanciaLateral.Text = "Esq: " + _Sensoriamento.Trajetoria.DistanciaEsquerda.ToString("0.00") + " m - Dir: " + _Sensoriamento.Trajetoria.DistanciaDireita.ToString("0.00") + " m";
            lblRua.Text = "Corredor: " + (_Sensoriamento.Trajetoria.CorredorAtual.Dentro ? "Dentro" : "Fora");
            lblMargem.Text = "Margem: " + (_Sensoriamento.Trajetoria.NaMargemDoCorredor ? "Sim" : "Não");
            lblProximoPonto.Text = "Próximo Ponto: " + (_Sensoriamento.Trajetoria.ProximoPonto.idxPonto);
            lblAproximando.Text = (_Sensoriamento.Trajetoria.ProximoPonto.Aproximando ? "Aproximando" : "Afastando");
            lblDistProx.Text = "Distância Próximo Ponto: " + _Sensoriamento.Trajetoria.ProximoPonto.DistanciaTrajeto.ToString("0.00") + " m";
            lblDistAnt.Text = "Distância Ponto Anterior: " + _Sensoriamento.Trajetoria.PontoMaisProximo.DistanciaTrajeto.ToString("0.00") + " m";
            lblTempoRestante.Text = "Tempo Restante: " + _Sensoriamento.Trajetoria.TempoEstimadoRestante;
            lblTempoOperacao.Text = "Tempo: " + _Sensoriamento.TempoDecorrido.ToString("HH:mm:ss") + " de " + _Sensoriamento.Trajetoria.TempoEstimadoOperacao;
            lblPercentualRua.Text = "Rua: " + _Sensoriamento.Trajetoria.CorredorAtual.Progresso.ToString("0.00") + "%";
            lblPercentualOperacao.Text = "Operação: " + _Sensoriamento.Trajetoria.ProgressoTrajeto.ToString("0.00") + "%";


            pnlOrientacaoTrajeto.Invalidate();
            pnlAnguloControle.Invalidate();
            pnlOrientacaoCarro.Invalidate();

            if (_Sensoriamento.Controle.SimulacaoMPC != null)
            {
                simulacao = _Sensoriamento.Controle.SimulacaoMPC;
            }

            if (_Trajetoria?.CorredorAtual != null)
            {
                MapaDinamico.AtualizarDados(
                    KinectService.Iniciado && KinectService.Leitura.StatusDetecao ? KinectService.Leitura.ObstaculoCritico : null,
                    (float)_Sensoriamento.Trajetoria.AnguloCaminho,
                    (float)_Sensoriamento.Gps.AnguloCarroDefinido,
                    _Sensoriamento.Gps,
                    _Trajetoria._TrajetoriaDinamica,
                    _Trajetoria.CorredorAtual.Pontos,
                    Variaveis.OperacaoEmAndamento.GPSTrajetoria,
                    _Trajetoria.RuasPlantacao,
                    simulacao
                );
            }


            if (!Variaveis.OperacaoEmAndamento.Iniciado)
            {
                if (btnIniciarSimulacao.Text.Contains("Parar"))
                {
                    parar = true;
                    btnIniciarSimulacao_Click(new object(), new EventArgs());
                    parar = false;
                }

                Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas();
            }

            AtualizandoTela = false;
        }


        

        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.ReadOnly = true;
            txtLongitude.ReadOnly = true;
        }

        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(".", ",")),
                };
            }

            GPSService.AtualizarCoordenadasGPS();
        }

        private async void btnAcrescentarGPS_Click(object sender, EventArgs e)
        {
            GPSModel novaPosicao = new GPSModel();
            double velocidadeMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle);
            velocidadeCarroMs = FuncoesMatematicas.ConverteMsParaKmh(velocidadeMs);

            Variaveis.OperacaoEmAndamento.DispMvd.Dados.VelocidadeMedia = velocidadeMs;

            double tempoPassado =
                    !chbAutomatico.Checked || UltimaAtualizacaoGps == DateTime.MinValue ? // esta no modo manual
                    (tempoGps / 1000.0) : // 1 tick do tempo gps
                    (DateTime.Now - UltimaAtualizacaoGps).TotalSeconds; // tempo decorrido entre ticks do timer de leitura

            // Obtém os parâmetros
            double anguloAtual = double.Parse(txtAnguloGPS.Text);

            var _Gps = Variaveis.OperacaoEmAndamento.Sensoriamento.Gps;

            double anguloControle = 0.0;
            TipoMovimentoDirecional tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento;

            var tipoControle = Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional;

            if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando)
            {
                novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)];
                idxPassoOperacao++;
            }
            else
            {
                if (Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Concluido || Variaveis.OperacaoEmAndamento.StatusAtual == StatusOperacao.Aguardando)
                {
                    return;
                }

                anguloControle = double.Parse(txtAnguloControle.Text);

                double.TryParse(txtErroTempoComando.Text, out double fatorErro);
                int ticksMax = (int)(ticksControleMax * Math.Max(1, fatorErro));

                bool atualizarAnguloControle = (ticksControle >= ticksMax);
                if (atualizarAnguloControle)
                {
                    ticksControle = 0;
                }
                else
                {
                    ticksControle++;
                }

                if (atualizarAnguloControle)
                {
                    btnCalcularAngulo_Click(new object(), new EventArgs());
                    anguloControle = double.Parse(txtAnguloControle.Text);
                }
            }

            // Calcula a nova posicao do robo
            //GPSModel pos = NovaPosicaoSimulacao(velocidadeMs, tempoPassado);
            if (true || novaPosicao == null)
            {
                novaPosicao = VariaveisEquipamento.SimularNovaPosicao(tipoMovimento, anguloControle, velocidadeMs, tempoPassado, anguloAtual, _Gps);
            }

            //Console.WriteLine($"C#: {DadosPosicao(novaPosicao)}");
            //Console.WriteLine($"Py: {DadosPosicao(pos)}");

            // Atualiza a interface gráfica
            txtAnguloGPS.Text = novaPosicao.OrientacaoReal.ToString("0.00");
            txtVelocidade.Text = velocidadeCarroMs.ToString("0.00");
            txtDistanciaGPS.Text = novaPosicao.Distancia.ToString("0.0000");
            txtLatitude.Text = novaPosicao.Latitude.ToString();
            txtLongitude.Text = novaPosicao.Longitude.ToString();

            UltimaAtualizacaoGps = DateTime.Now;

            AtualizarPosicaoGPS();

            Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados();

            if (!chbAutomatico.Checked)
            {
                AtualizarDadosTela();
            }
        }

        private GPSModel NovaPosicaoSimulacao(double velocidadeMs, double tempo)
        {
            if (Variaveis.OperacaoEmAndamento.Controle.SimulacaoMPC.Count() > 1)
            {
                double distancia = velocidadeMs * tempo;
                MPCSimulacaoModel posicao = Variaveis.OperacaoEmAndamento.Controle.SimulacaoMPC[1];
                GPSModel novaPosicao = new GPSModel()
                {
                    Latitude = posicao.latitude,
                    Longitude = posicao.longitude,
                    OrientacaoReal = posicao.orientacao,
                    Distancia = distancia,
                };
                return novaPosicao;
            }
            return null;
        }

        private string DadosPosicao(GPSModel pos)
        {
            if (pos == null) return "";
            return $"Lat: {pos.Latitude}, Long: {pos.Longitude}, Theta: {pos.OrientacaoReal}, Distancia: {pos.Distancia}";
        }

        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 btnCarregarOperacao_Click(object sender, EventArgs e)
        {
            passosOperacao = new List<GPSModel>();
            txtTempoGPS.Text = tempoGps.ToString();
            idxPassoOperacao = 0;
            lblCarregado.Text = "Simulador";

            OpenFileDialog ofd = new OpenFileDialog();
            ofd.Filter = "Arquivos de Operação|*.lgop";
            ofd.InitialDirectory = Application.StartupPath + "\\" + Variaveis.CaminhoOperacoes;
            if (ofd.ShowDialog() == DialogResult.OK)
            {
                OperacaoDadosModel data = JsonConvert.DeserializeObject<OperacaoDadosModel>(File.ReadAllText(ofd.FileName));

                passosOperacao = JsonConvert.DeserializeObject<GPSModel[]>(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, T_Code.Gps.ToString())).ToList();

                txtTempoGPS.Text = "1000";
                lblCarregado.Text = "Operação";
            }
        }

        private void btnCarregarTrajetoria_Click(object sender, EventArgs e)
        {
            passosOperacao = new List<GPSModel>();
            txtTempoGPS.Text = tempoGps.ToString();
            idxPassoOperacao = 0;
            lblCarregado.Text = "Simulador";

            OpenFileDialog ofd = new OpenFileDialog();
            ofd.Filter = "Arquivos de Trajetória|*.json";
            ofd.InitialDirectory = Application.StartupPath + "\\" + Variaveis.CaminhoMapasConvertidos;
            if (ofd.ShowDialog() == DialogResult.OK)
            {
                var DadosMapa = JsonConvert.DeserializeObject<MapaFeatureCollectionModel>(File.ReadAllText(ofd.FileName));

                passosOperacao = DadosMapa.features[0].geometry.coordinates
                    .Select(x => new GPSModel()
                    {
                        Latitude = x[1],
                        Longitude = x[0]
                    })
                    .ToList();

                txtTempoGPS.Text = "200";
                lblCarregado.Text = "Trajetória";
            }
        }

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

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

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

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

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

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

            double anguloControle = Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle;

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

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

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

        private void chbSonarVirando_CheckedChanged(object sender, EventArgs e)
        {
            
        }

        private void chbAutomatico_CheckedChanged(object sender, EventArgs e)
        {
            UltimaAtualizacaoGps = DateTime.MinValue;
        }

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

            this.Close();
        }

        private void cmbTipoControleDirecional_SelectedIndexChanged(object sender, EventArgs e)
        {
            Variaveis.OperacaoEmAndamento.Controle.TipoControleDirecional = (TiposControladorDirecional)cmbTipoControleDirecional.SelectedIndex;
        }

        private void cmbStatusCorredor_SelectedIndexChanged(object sender, EventArgs e)
        {
            RedisService.AtualizarCampos(
                CtxKey.DadosVisualWorker,
                ("segmentacao.status_corredor", cmbStatusCorredor.SelectedIndex)
            );
        }
    }

    
}