diff --git a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs index fcddcc1ab..785033b10 100644 --- a/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs +++ b/AgroBase/AgroBase/Forms/Simuladores/frmSimulacaoMapaGPS.cs @@ -1,597 +1,1702 @@ -using AgroBase.Models; +using AgroBase.Models; using AgroBase.Services; using Newtonsoft.Json; using Newtonsoft.Json.Linq; using System; using System.Collections.Generic; -using System.Threading; +using System.Diagnostics; +using System.Globalization; using System.IO; using System.Linq; +using System.Threading; using System.Threading.Tasks; using System.Windows.Forms; using static AgroBase.Models.Enums; -using System.Diagnostics; -using Emgu.CV; namespace AgroBase.Forms { + /// + /// Simulador de operação por mapa/GNSS. + /// + /// A simulação física nunca depende da thread da interface: + /// - o AsyncTaskTimerModel executa em FixedRateSkipMissed, sem uiControl; + /// - o passo usa duração nominal limitada, sem salto após travamento visual; + /// - a interface consome apenas o snapshot mais recente em baixa frequência; + /// - o mapa recebe trajetórias decimadas exclusivamente para visualização; + /// - nunca existem dois passos de simulação simultâneos. + /// + /// Os nomes dos handlers e as APIs públicas usadas pelo Designer foram + /// preservados para permitir substituição direta do code-behind atual. + /// 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 passosOperacao = new List(); - int idxPassoOperacao = 0; - double velocidadeCarroMs = 0.0; + #region Configuração - DateTime UltimaAtualizacaoGps = DateTime.MinValue; + private const int IntervaloMinimoGpsMs = 20; + private const int IntervaloMaximoGpsMs = 10_000; + private const int IntervaloTelaMs = 250; // 4 FPS. A física continua independente. + private const int IntervaloTrajetoriaWebMs = 1_000; - Queue historicoAngulosControle = new Queue(); - GPSModel posicaoAtual = new GPSModel(); + private const int MaxPontosTrajetoriaVisual = 800; + private const int MaxPontosCorredorVisual = 600; + private const int MaxPontosPorRuaVisual = 350; + private const int MaxPontosTrilhaVisual = 1_500; - List simulacao = new List(); + #endregion + #region Serviços e sincronização + private AsyncTaskTimerModel tmrLeitura; + private System.Windows.Forms.Timer tmrTela; + private MapaDinamicoModel MapaDinamico; + + private readonly SemaphoreSlim _passoSimulacaoGate = + new SemaphoreSlim(1, 1); + + private readonly object _estadoSimulacaoLock = new object(); + private readonly Queue historicoAngulosControle = + new Queue(); + private readonly Queue _trilhaVisual = + new Queue(); + + private int _encerrando; private int _atualizacaoTelaPendente; - private int _tickEmExecucao; + private int _passoManualEmExecucao; + private int _mapaWebEmExecucao; + private int _forcarTrajetoriaWeb = 1; + private int _publicacaoMapaGpsConfigurada; + private bool _publicacaoMapaGpsAnterior = true; - private readonly Stopwatch _cronometroTela = Stopwatch.StartNew(); + #endregion - private const int IntervaloTelaMs = 100; // 10 FPS para a UI + #region Estado da simulação + private bool parar; + + private double tempoGps = + 1000.0 / Math.Max(1, GPSService.TaxaAmostragemHz); + + private int _tempoGpsConfiguradoMs; + private int _automatico; + private int _delayComandoPassos; + private int _delayPosicaoPassos; + + private int ticksControleMax; + private int ticksControle; + + private List passosOperacao = new List(); + private int idxPassoOperacao; + + private double velocidadeCarroMs; + private double _ultimoAnguloControleAplicado; + private GPSModel posicaoAtual = new GPSModel(); + + // Mantida para compatibilidade e para os dados apresentados na tela. + private List simulacao = + new List(); + + private readonly Stopwatch _cronometroTrajetoriaWeb = + Stopwatch.StartNew(); + + private object _ruasFonteCache; + private int _ruasFonteQuantidadeRuas = -1; + private List> _ruasVisualCache = + new List>(); + + #endregion 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); + DoubleBuffered = true; + SetStyle(ControlStyles.AllPaintingInWmPaint, true); + SetStyle(ControlStyles.UserPaint, true); + SetStyle(ControlStyles.OptimizedDoubleBuffer, true); MapaDinamico = new MapaDinamicoModel(this, pnlZoomMapa); } + #region Ciclo de vida + private void frmSimulacaoMapaGPS_Load(object sender, EventArgs e) { cmbTipoControleDirecional.Items.Clear(); - cmbTipoControleDirecional.Items.AddRange(Enum.GetNames(typeof(Enums.TiposControladorDirecional))); - cmbTipoControleDirecional.SelectedIndex = (int)Variaveis.OperacaoEmAndamento.Parametros.Controle.DirTipoMovimento; + cmbTipoControleDirecional.Items.AddRange( + Enum.GetNames(typeof(Enums.TiposControladorDirecional)) + ); + cmbTipoControleDirecional.SelectedIndex = + (int)(Variaveis.OperacaoEmAndamento.Parametros.Controle?.DirTipoMovimento ?? Enums.TiposControladorDirecional.MPC); cmbStatusCorredor.Items.Clear(); - cmbStatusCorredor.Items.AddRange(Enum.GetNames(typeof(Enums.StatusCarroMapa))); + 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(); + int intervaloInicial = LimitarInteiro( + (int)Math.Round(tempoGps), + IntervaloMinimoGpsMs, + IntervaloMaximoGpsMs + ); - txtTempoGPS.Text = tempoGps.ToString(); + txtTempoGPS.Text = intervaloInicial.ToString(CultureInfo.CurrentCulture); + Interlocked.Exchange(ref _tempoGpsConfiguradoMs, intervaloInicial); + + MapasModel mapaAnterior = Variaveis.OperacaoEmAndamento.Mapa; + if (mapaAnterior != null) + { + try + { + mapaAnterior.RuasSelecionadasAlteradas -= + Mapa_RuasSelecionadasAlteradas; + } + catch + { + } + } Variaveis.OperacaoEmAndamento.Mapa = new MapasModel() { pnlMapa = pnlMapa, }; - Variaveis.OperacaoEmAndamento.Mapa.RuasSelecionadasAlteradas += Mapa_RuasSelecionadasAlteradas; + Variaveis.OperacaoEmAndamento.Mapa.RuasSelecionadasAlteradas += + Mapa_RuasSelecionadasAlteradas; + + // Durante a simulação, somente o timer visual deste Form publica + // no WebView. Isso impede publicação duplicada no GPSService e, + // principalmente, acesso ao WebView pela thread da física. + _publicacaoMapaGpsAnterior = + GPSService.PublicacaoMapaAutomaticaHabilitada; + GPSService.PublicacaoMapaAutomaticaHabilitada = false; + Interlocked.Exchange(ref _publicacaoMapaGpsConfigurada, 1); + + AtualizarConfiguracaoSimulacaoDaTela(); + + // Sem uiControl: o callback nunca é encaminhado para a UI. + tmrLeitura = new AsyncTaskTimerModel( + "tmrLeituraSimulacao_" + GetHashCode(), + tmrLeitura_Tick, + intervaloInicial, + uiControl: null, + timeout: Math.Max(5_000, intervaloInicial * 10), + scheduleMode: AsyncTaskTimerScheduleMode.FixedRateSkipMissed, + runImmediately: false + ); + tmrLeitura.Start(); + + // Timer exclusivamente visual. Se a UI atrasar, ele próprio atrasa, + // mas nunca interfere no relógio da simulação. + tmrTela = new System.Windows.Forms.Timer(); + tmrTela.Interval = IntervaloTelaMs; + tmrTela.Tick += tmrTela_Tick; + tmrTela.Start(); + + AtualizarDadosTela(); } - private void Mapa_RuasSelecionadasAlteradas(object sender, EventArgs e) + private void frmSimulacaoMapaGPS_FormClosing( + object sender, + FormClosingEventArgs e) { - //Variaveis.OperacaoEmAndamento.Parametros.RuasPercorrer = mapa.RuasPercorrer.ToList(); + EncerrarRecursos(); + + var operacao = Variaveis.OperacaoEmAndamento; + if (operacao?.Sensoriamento?.Operacao != null) + operacao.Sensoriamento.Operacao.OperacaoIniciada = false; } - private void frmSimulacaoMapaGPS_FormClosing(object sender, FormClosingEventArgs e) + private void EncerrarRecursos() { - tmrLeitura?.Dispose(); - MapaDinamico?.Dispose(); - Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.OperacaoIniciada = false; - } - - private async Task tmrLeitura_Tick() - { - if (Interlocked.Exchange(ref _tickEmExecucao, 1) == 1) + if (Interlocked.Exchange(ref _encerrando, 1) == 1) return; + if (Interlocked.Exchange( + ref _publicacaoMapaGpsConfigurada, + 0) == 1) + { + GPSService.PublicacaoMapaAutomaticaHabilitada = + _publicacaoMapaGpsAnterior; + } + try { - int tempo = 0; - bool automatico = false; - - // Esses componentes pertencem à UI. - if (!IsDisposed && IsHandleCreated) + if (tmrTela != null) { - Invoke(new Action(() => - { - int.TryParse(txtTempoGPS.Text, out tempo); - automatico = chbAutomatico.Checked; - })); + tmrTela.Stop(); + tmrTela.Tick -= tmrTela_Tick; + tmrTela.Dispose(); + tmrTela = null; + } + } + catch + { + } + + try + { + tmrLeitura?.Dispose(); + tmrLeitura = null; + } + catch + { + } + + try + { + var mapa = Variaveis.OperacaoEmAndamento?.Mapa; + if (mapa != null) + { + mapa.RuasSelecionadasAlteradas -= + Mapa_RuasSelecionadasAlteradas; + mapa.Dispose(); + } + } + catch + { + } + + try + { + MapaDinamico?.Dispose(); + MapaDinamico = null; + } + catch + { + } + } + + #endregion + + #region Relógio da simulação + + private async Task tmrLeitura_Tick(CancellationToken cancellationToken) + { + if (cancellationToken.IsCancellationRequested || + Volatile.Read(ref _encerrando) == 1) + { + return; + } + + if (Volatile.Read(ref _automatico) == 0) + return; + + await ExecutarPassoSimulacaoAsync( + cancellationToken, + aguardarGate: false + ).ConfigureAwait(false); + } + + private async Task ExecutarPassoSimulacaoAsync( + CancellationToken cancellationToken, + bool aguardarGate) + { + bool entrou = false; + + try + { + if (aguardarGate) + { + await _passoSimulacaoGate + .WaitAsync(cancellationToken) + .ConfigureAwait(false); + entrou = true; + } + else + { + entrou = await _passoSimulacaoGate + .WaitAsync(0, cancellationToken) + .ConfigureAwait(false); + + if (!entrou) + return false; } - if (tempo > 0) - tempoGps = tempo; + cancellationToken.ThrowIfCancellationRequested(); - if (automatico) - { - // Temporariamente mantém o passo na UI porque o método ainda - // acessa vários TextBox diretamente. - Invoke(new Action(() => - { - btnAcrescentarGPS_Click( - btnAcrescentarGPS, - EventArgs.Empty - ); - })); - } - - if (_cronometroTela.ElapsedMilliseconds >= IntervaloTelaMs) - { - SolicitarAtualizacaoTela(); - _cronometroTela.Restart(); - } - - // Não reduza para 1 ms quando estiver atrasado. - tmrLeitura.SetInterval(Math.Max((int)tempoGps, 10)); + ExecutarPassoSimulacaoCore(cancellationToken); + return true; + } + catch (OperationCanceledException) + { + return false; + } + catch (Exception ex) + { + Variaveis.MostrarLog( + "[Simulacao] Falha no passo de simulação: " + ex + ); + return false; } finally { - Interlocked.Exchange(ref _tickEmExecucao, 0); + if (entrou) + _passoSimulacaoGate.Release(); + } + } + + private void ExecutarPassoSimulacaoCore( + CancellationToken cancellationToken) + { + var operacao = Variaveis.OperacaoEmAndamento; + + if (operacao == null || + operacao.Controle == null || + operacao.Sensoriamento == null) + { + return; } - await Task.CompletedTask; - } - private void SolicitarAtualizacaoTela() - { - if (IsDisposed || !IsHandleCreated) - return; + cancellationToken.ThrowIfCancellationRequested(); - if (Interlocked.Exchange(ref _atualizacaoTelaPendente, 1) == 1) - return; + int intervaloMs = LimitarInteiro( + Volatile.Read(ref _tempoGpsConfiguradoMs), + IntervaloMinimoGpsMs, + IntervaloMaximoGpsMs + ); - BeginInvoke(new Action(() => + // Passo nominal e limitado. Um atraso de UI nunca causa teleporte. + double tempoPassadoSegundos = intervaloMs / 1000.0; + + double velocidadeCalculada = + FuncoesMatematicas.CalculaVelocidadeMsPercentual( + operacao.SimulacaoRpmControle + ); + + var controleMovimento = operacao.Controle.TiposControle? + .FirstOrDefault(x => x.Tipo == T_Code.Mov); + + if (controleMovimento != null && + controleMovimento.DirecaoAtual == Direcao.Tras) { - try + velocidadeCalculada *= -1.0; + } + + if (operacao.DispMvd?.Dados != null) + operacao.DispMvd.Dados.VelocidadeMedia = velocidadeCalculada; + + double velocidadeFisica = + operacao.Sensoriamento.Operacao.StatusOperacaoAtual == + StatusOperacao.EmAndamento + ? velocidadeCalculada + : 0.0; + + GPSModel novaPosicao = ObterPassoOperacaoGravado(operacao); + double anguloControle = ObterUltimoAnguloControle(); + + if (novaPosicao == null) + { + bool atualizarAnguloControle = + ticksControle >= ticksControleMax; + + if (atualizarAnguloControle) + ticksControle = 0; + else + ticksControle++; + + if (atualizarAnguloControle) { - AtualizarDadosTela(); - } - catch (Exception ex) - { - Variaveis.MostrarLog( - "[Simulacao] Erro ao atualizar tela: " + ex + anguloControle = CalcularAnguloControleAtrasado( + Volatile.Read(ref _delayComandoPassos) ); } - finally + + GPSModel gps = GPSService.GetSnapshot(); + + double anguloAtual = + ValorFinito(gps?.AnguloCarroDefinido ?? double.NaN) + ? gps.AnguloCarroDefinido + : ValorFinito(gps?.OrientacaoReal ?? double.NaN) + ? gps.OrientacaoReal + : 0.0; + + TipoMovimentoDirecional tipoMovimento = + operacao.Controle.TipoMovimento; + + novaPosicao = VariaveisEquipamento.SimularNovaPosicao( + tipoMovimento, + anguloControle, + velocidadeFisica, + tempoPassadoSegundos, + anguloAtual, + gps + ); + } + + if (novaPosicao == null) + return; + + PrepararLeituraSimulada( + novaPosicao, + velocidadeFisica, + intervaloMs + ); + + cancellationToken.ThrowIfCancellationRequested(); + + AtualizarPosicaoGPS( + novaPosicao, + Volatile.Read(ref _delayPosicaoPassos) + ); + + GPSModel posicaoCorrigida = GPSService.GetSnapshot(); + + lock (_estadoSimulacaoLock) + { + posicaoAtual = novaPosicao.Clone(); + velocidadeCarroMs = velocidadeCalculada; + _ultimoAnguloControleAplicado = anguloControle; + + if (posicaoCorrigida != null) { - Interlocked.Exchange(ref _atualizacaoTelaPendente, 0); + _trilhaVisual.Enqueue(posicaoCorrigida.Clone()); + + while (_trilhaVisual.Count > MaxPontosTrilhaVisual) + _trilhaVisual.Dequeue(); } - })); + } + + // Sensoriamento.AtualizarDados() não é chamado aqui. + // O tmrLeituras da OperacaoModel continua sendo o único proprietário + // do snapshot/contexto Redis, igual ao fluxo real do produto. } + + private GPSModel ObterPassoOperacaoGravado(OperacaoModel operacao) + { + if (operacao?.Simulando != true) + return null; + + lock (_estadoSimulacaoLock) + { + if (passosOperacao == null || passosOperacao.Count == 0) + return null; + + int indice = Math.Min( + Math.Max(0, idxPassoOperacao), + passosOperacao.Count - 1 + ); + + GPSModel leitura = passosOperacao[indice]?.Clone(); + + if (idxPassoOperacao < passosOperacao.Count - 1) + idxPassoOperacao++; + + return leitura; + } + } + + private void PrepararLeituraSimulada( + GPSModel leitura, + double velocidade, + int intervaloMs) + { + if (leitura == null) + return; + + DateTime agora = DateTime.Now; + double timestampMonotonico = + Stopwatch.GetTimestamp() / (double)Stopwatch.Frequency; + + leitura.Momento = agora; + leitura.DataHora = agora; + leitura.UltimoComandoRespondido = DateTime.UtcNow; + leitura.Velocidade = velocidade; + + if (!CoordenadaGeograficaValida( + leitura.LatitudeAnt, + leitura.LongitudeAnt) && + CoordenadaGeograficaValida( + leitura.Latitude, + leitura.Longitude)) + { + leitura.LatitudeAnt = leitura.Latitude; + leitura.LongitudeAnt = leitura.Longitude; + } + + if (leitura.TimestampPos != null) + leitura.TimestampPos.valor = timestampMonotonico; + + if (leitura.TimestampOri != null) + leitura.TimestampOri.valor = timestampMonotonico; + + // O campo Distancia normalmente já vem preenchido por + // SimularNovaPosicao. Para trajetórias importadas, preservamos o valor. + if (!ValorFinito(leitura.Distancia) || leitura.Distancia < 0) + leitura.Distancia = Math.Abs(velocidade) * intervaloMs / 1000.0; + } + + private void AtualizarPosicaoGPS( + GPSModel leitura, + int delayPosicaoPassos) + { + if (leitura == null) + return; + + // A escrita e o processamento completo ocorrem sob o mesmo lock + // interno usado pelas sentenças NMEA reais. O Form não altera + // mais os campos estáticos do GPSService de forma fragmentada. + GPSService.AtualizarLeituraSimulada( + leitura, + Math.Max(0, delayPosicaoPassos) + ); + } + + #endregion + + #region Atualização da interface + + private void tmrTela_Tick(object sender, EventArgs e) + { + if (Volatile.Read(ref _encerrando) == 1) + return; + + AtualizarConfiguracaoSimulacaoDaTela(); + AtualizarDadosTela(); + } + + private void AtualizarConfiguracaoSimulacaoDaTela() + { + if (IsDisposed || Disposing) + return; + + int intervaloAtual = Volatile.Read(ref _tempoGpsConfiguradoMs); + int novoIntervalo; + + if (!int.TryParse( + txtTempoGPS.Text, + NumberStyles.Integer, + CultureInfo.CurrentCulture, + out novoIntervalo)) + { + novoIntervalo = intervaloAtual > 0 + ? intervaloAtual + : (int)Math.Round(tempoGps); + } + + novoIntervalo = LimitarInteiro( + novoIntervalo, + IntervaloMinimoGpsMs, + IntervaloMaximoGpsMs + ); + + if (novoIntervalo != intervaloAtual) + { + Interlocked.Exchange( + ref _tempoGpsConfiguradoMs, + novoIntervalo + ); + tempoGps = novoIntervalo; + + try + { + tmrLeitura?.SetInterval(novoIntervalo); + tmrLeitura?.SetTimeout( + Math.Max(5_000, novoIntervalo * 10) + ); + } + catch (ObjectDisposedException) + { + } + } + + Interlocked.Exchange( + ref _automatico, + chbAutomatico.Checked ? 1 : 0 + ); + + int delayComando; + if (!int.TryParse(txtDelayComando.Text, out delayComando)) + delayComando = 0; + + int delayPosicao; + if (!int.TryParse(txtDelayPosicao.Text, out delayPosicao)) + delayPosicao = 0; + + Interlocked.Exchange( + ref _delayComandoPassos, + LimitarInteiro(delayComando, 0, 100) + ); + Interlocked.Exchange( + ref _delayPosicaoPassos, + LimitarInteiro(delayPosicao, 0, 100) + ); + } + + private void SolicitarAtualizacaoTela() + { + if (Volatile.Read(ref _encerrando) == 1 || + IsDisposed || + !IsHandleCreated) + { + return; + } + + if (Interlocked.Exchange( + ref _atualizacaoTelaPendente, + 1) == 1) + { + return; + } + + try + { + BeginInvoke(new Action(delegate + { + try + { + if (Volatile.Read(ref _encerrando) == 0) + { + AtualizarConfiguracaoSimulacaoDaTela(); + AtualizarDadosTela(); + } + } + catch (Exception ex) + { + Variaveis.MostrarLog( + "[Simulacao] Erro ao atualizar tela: " + ex + ); + } + finally + { + Interlocked.Exchange( + ref _atualizacaoTelaPendente, + 0 + ); + } + })); + } + catch (ObjectDisposedException) + { + Interlocked.Exchange(ref _atualizacaoTelaPendente, 0); + } + catch (InvalidOperationException) + { + Interlocked.Exchange(ref _atualizacaoTelaPendente, 0); + } + } + private void AtualizarDadosTela() { - var _Controle = Variaveis.OperacaoEmAndamento.Controle; - var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento; - var _Trajetoria = Variaveis.OperacaoEmAndamento.Trajetoria; + var operacao = Variaveis.OperacaoEmAndamento; + var controle = operacao?.Controle; + var sensoriamento = operacao?.Sensoriamento; + var trajetoria = operacao?.Trajetoria; - if (_Trajetoria == null) + if (operacao == null || + controle == null || + sensoriamento == null || + sensoriamento.Operacao == null || + trajetoria == null || + sensoriamento.Trajetoria == null) { return; } - lblStatusOperacao.Text = $"Operação: {_Sensoriamento.Operacao.StatusOperacaoAtual}"; - if (_Sensoriamento.Operacao.StatusOperacaoAtual == StatusOperacao.Aguardando) lblStatusOperacao.Text += $" ({_Sensoriamento.Operacao.TempoAguardandoSeg:0})"; - lblTipoMovimento.Text = (_Controle?.TipoMovimento ?? TipoMovimentoDirecional.Diagnostico).ToString(); + // Antes de a operação começar, latitude, longitude e ângulo + // pertencem ao operador e nunca são sobrescritos pelo timer visual. + // Durante a operação, eles voltam a funcionar como telemetria. + if (sensoriamento.Operacao.OperacaoIniciada) + AtualizarCamposPosicaoSimuladaNaTela(); - if (_Trajetoria == null) + var dadosTrajetoria = sensoriamento.Trajetoria; + var corredor = dadosTrajetoria.CorredorAtual; + var proximoPonto = dadosTrajetoria.ProximoPonto; + var pontoMaisProximo = dadosTrajetoria.PontoMaisProximo; + + lblStatusOperacao.Text = + "Operação: " + sensoriamento.Operacao.StatusOperacaoAtual; + + if (sensoriamento.Operacao.StatusOperacaoAtual == + StatusOperacao.Aguardando) { - //Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas(); - return; + lblStatusOperacao.Text += + $" ({sensoriamento.Operacao.TempoAguardandoSeg:0})"; } - if (_Sensoriamento.Trajetoria == null) + lblTipoMovimento.Text = + (controle.TipoMovimento).ToString(); + + string statusVisual = "-"; + try + { + statusVisual = sensoriamento + .OperadorVisual + .Analises + .segmentacao + .status_corredor + .ToString(); + } + catch { - return; } - var h = GPSUtils.DistanciaDoTrecho(simulacao.Select(x => new GPSModel() { Latitude = x.latitude, Longitude = x.longitude, OrientacaoReal = x.orientacao }).ToList()); + lblStatus.Text = + $"Carro: {dadosTrajetoria.StatusCarro} ({statusVisual})"; + lblDirecao.Text = + $"Direção: {dadosTrajetoria.DirecaoCaminho}"; + lblDistanciaOperacao.Text = + $"Distância Operação: {dadosTrajetoria.DistanciaPercorrida:F2} m de {dadosTrajetoria.DistanciaTotal:F2} m"; + lblDistanciaFinal.Text = + $"Distância Restante: {dadosTrajetoria.DistanciaRestante:F2} m"; - lblStatus.Text = $"Carro: {_Sensoriamento.Trajetoria.StatusCarro} ({_Sensoriamento.OperadorVisual.Analises.segmentacao.status_corredor})"; - lblDirecao.Text = $"Direção: {_Sensoriamento.Trajetoria.DirecaoCaminho}"; - lblDistanciaOperacao.Text = $"Distância Operação: {_Sensoriamento.Trajetoria.DistanciaPercorrida:F2} m de {_Sensoriamento.Trajetoria.DistanciaTotal:F2} m"; - lblDistanciaFinal.Text = $"Distância Restante: {_Sensoriamento.Trajetoria.DistanciaRestante:F2} m"; - //lblDistanciaLateral.Text = $"Esq: {_Sensoriamento.Trajetoria.DistanciaEsquerda:F2} m - Dir: {_Sensoriamento.Trajetoria.DistanciaDireita:F2} m {Variaveis.OperacaoEmAndamento.Controle.ErroLateral:F2} m"; - lblDistanciaLateral.Text = $"h: {h:F2} m, p: {_Controle.SimulacaoMPC.Count}, s: {_Controle.DebugCustoMpc.Count}"; - 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:F2} m"; - lblDistAnt.Text = $"Distância Ponto Anterior: {_Sensoriamento.Trajetoria.PontoMaisProximo.DistanciaTrajeto:F2} m"; - lblTempoRestante.Text = $"Tempo Restante: {_Sensoriamento.Trajetoria.TempoEstimadoRestante}"; - lblTempoOperacao.Text = $"Tempo: " + _Sensoriamento.Operacao.TempoDecorrido.ToString("HH:mm:ss") + $" de {_Sensoriamento.Trajetoria.TempoEstimadoOperacao}"; - lblPercentualRua.Text = $"Rua: {_Sensoriamento.Trajetoria.CorredorAtual.Progresso:F2}%"; - lblPercentualOperacao.Text = $"Operação: {_Sensoriamento.Trajetoria.ProgressoTrajeto:F2}%"; + List simulacaoAtual = + CopiarListaSegura(controle.SimulacaoMPC); + simulacao = simulacaoAtual; + double distanciaHorizonte = + CalcularDistanciaSimulacao(simulacaoAtual); + + int qtdDebug = controle.DebugCustoMpc?.Count ?? 0; + AsyncTaskTimerMetrics metricas = tmrLeitura?.GetMetrics(); + + lblDistanciaLateral.Text = + $"h: {distanciaHorizonte:F2} m, " + + $"p: {simulacaoAtual.Count}, s: {qtdDebug}, " + + $"tick: {(metricas?.LastExecutionDurationMs ?? 0):F0} ms, " + + $"atrasos: {metricas?.Overruns ?? 0}"; + + lblRua.Text = + "Corredor: " + + ((corredor?.Dentro ?? false) ? "Dentro" : "Fora"); + lblMargem.Text = + "Margem: " + + (dadosTrajetoria.NaMargemDoCorredor ? "Sim" : "Não"); + lblProximoPonto.Text = + $"Próximo Ponto: {proximoPonto?.idxPonto ?? 0}"; + lblAproximando.Text = + (proximoPonto?.Aproximando ?? false) + ? "Aproximando" + : "Afastando"; + lblDistProx.Text = + $"Distância Próximo Ponto: {(proximoPonto?.DistanciaTrajeto ?? 0):F2} m"; + lblDistAnt.Text = + $"Distância Ponto Anterior: {(pontoMaisProximo?.DistanciaTrajeto ?? 0):F2} m"; + lblTempoRestante.Text = + $"Tempo Restante: {dadosTrajetoria.TempoEstimadoRestante}"; + lblTempoOperacao.Text = + "Tempo: " + + sensoriamento.Operacao.TempoDecorrido.ToString("HH:mm:ss") + + $" de {dadosTrajetoria.TempoEstimadoOperacao}"; + lblPercentualRua.Text = + $"Rua: {(corredor?.Progresso ?? 0):F2}%"; + lblPercentualOperacao.Text = + $"Operação: {dadosTrajetoria.ProgressoTrajeto:F2}%"; pnlOrientacaoTrajeto.Invalidate(); pnlAnguloControle.Invalidate(); pnlOrientacaoCarro.Invalidate(); - if (_Controle.SimulacaoMPC != null) + AtualizarMapasVisuais( + operacao, + sensoriamento, + trajetoria, + simulacaoAtual + ); + + if (!sensoriamento.Operacao.OperacaoIniciada && + btnIniciarSimulacao.Text.Contains("Parar")) { - simulacao = _Controle.SimulacaoMPC; + parar = true; + btnIniciarSimulacao.Text = "Iniciar Simulação"; + parar = false; } - else + } + + private void AtualizarCamposPosicaoSimuladaNaTela() + { + GPSModel posicaoExibicao; + double velocidadeExibicao; + double anguloControleExibicao; + + lock (_estadoSimulacaoLock) { - simulacao = new List(); + posicaoExibicao = posicaoAtual?.Clone(); + velocidadeExibicao = velocidadeCarroMs; + anguloControleExibicao = _ultimoAnguloControleAplicado; } - if (_Trajetoria?.CorredorAtual != null || (_Trajetoria?.RetornandoBase ?? false)) + if (posicaoExibicao != null) + { + txtAnguloGPS.Text = + posicaoExibicao.OrientacaoReal.ToString("0.00"); + txtVelocidade.Text = + velocidadeExibicao.ToString("0.00"); + txtDistanciaGPS.Text = + posicaoExibicao.Distancia.ToString("0.0000"); + + double latitudeExibicao = + CoordenadaGeograficaValida( + posicaoExibicao.LatitudeAnt, + posicaoExibicao.LongitudeAnt) + ? posicaoExibicao.LatitudeAnt + : posicaoExibicao.Latitude; + + double longitudeExibicao = + CoordenadaGeograficaValida( + posicaoExibicao.LatitudeAnt, + posicaoExibicao.LongitudeAnt) + ? posicaoExibicao.LongitudeAnt + : posicaoExibicao.Longitude; + + txtLatitude.Text = latitudeExibicao.ToString(); + txtLongitude.Text = longitudeExibicao.ToString(); + txtAnguloControle.Text = + anguloControleExibicao.ToString("0.00"); + } + } + + private void AtualizarMapasVisuais( + OperacaoModel operacao, + OperacaoSensoriamentoConjuntoModel sensoriamento, + TrajetoriaMapaOperacaoModel trajetoria, + List simulacaoAtual) + { + if (MapaDinamico == null || sensoriamento?.Gps == null) + return; + + List trajetoriaVisual = + CriarTrajetoriaVisual( + trajetoria._TrajetoriaDinamica, + MaxPontosTrajetoriaVisual + ); + + List corredorVisual = + trajetoria.RetornandoBase + ? new List() + : CriarTrajetoriaVisual( + trajetoria.CorredorAtual?.Pontos, + MaxPontosCorredorVisual + ); + + List trilhaVisual = ObterTrilhaVisual(); + List> ruasVisual = + ObterRuasVisuais(trajetoria.RuasPlantacao); + + if (trajetoria.CorredorAtual != null || + trajetoria.RetornandoBase) { MapaDinamico.AtualizarDados( - KinectService.Iniciado && KinectService.Leitura.StatusDetecao ? KinectService.Leitura.ObstaculoCritico : null, - (float)_Sensoriamento.Trajetoria.AnguloCaminho, - (float)_Sensoriamento.Gps.AnguloCarroDefinido, - _Sensoriamento.Gps, - _Trajetoria._TrajetoriaDinamica, - _Trajetoria.RetornandoBase ? new List() : _Trajetoria.CorredorAtual.Pontos, - Variaveis.OperacaoEmAndamento.GPSTrajetoria, - _Trajetoria.RuasPlantacao, - simulacao + KinectService.Iniciado && + KinectService.Leitura.StatusDetecao + ? KinectService.Leitura.ObstaculoCritico + : null, + (float)sensoriamento.Trajetoria.AnguloCaminho, + (float)sensoriamento.Gps.AnguloCarroDefinido, + sensoriamento.Gps, + trajetoriaVisual, + corredorVisual, + trilhaVisual, + ruasVisual, + simulacaoAtual ); } + bool publicarTrajetoria = + Interlocked.Exchange(ref _forcarTrajetoriaWeb, 0) == 1 || + _cronometroTrajetoriaWeb.ElapsedMilliseconds >= + IntervaloTrajetoriaWebMs; - if (!Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.OperacaoIniciada) + List rotaWeb = null; + + if (publicarTrajetoria) { - if (btnIniciarSimulacao.Text.Contains("Parar")) - { - parar = true; - btnIniciarSimulacao_Click(new object(), new EventArgs()); - parar = false; - } + _cronometroTrajetoriaWeb.Restart(); + rotaWeb = ConverterTrajetoriaParaGps(trajetoriaVisual); + } - //Variaveis.OperacaoEmAndamento.Mapa.AtualizarRuasSelecionadas(); + AtualizarMapaWebSemBloquear( + operacao.Mapa, + sensoriamento.Gps, + sensoriamento.Operacao.OperacaoIniciada, + publicarTrajetoria, + rotaWeb + ); + } + + private async void AtualizarMapaWebSemBloquear( + MapasModel mapa, + GPSModel gps, + bool emFoco, + bool publicarTrajetoria, + List rotaWeb) + { + if (mapa == null || gps == null || + Volatile.Read(ref _encerrando) == 1) + { + return; + } + + if (Interlocked.Exchange(ref _mapaWebEmExecucao, 1) == 1) + { + if (publicarTrajetoria) + Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1); + + return; + } + + try + { + // Este método é iniciado pela thread da UI. O await preserva + // o contexto necessário pelo WebView2. + await mapa.AtualizarPosicaoMapaAsync( + MapasVariaveisModel.IDMarcadorRover, + gps.Latitude, + gps.Longitude, + gps.AnguloCarroDefinido, + emFoco + ); + + if (publicarTrajetoria && rotaWeb != null) + await mapa.DefinirTrajetoriaPlanejadaAsync(rotaWeb); + } + catch (ObjectDisposedException) + { + } + catch (InvalidOperationException ex) + { + Variaveis.MostrarLog( + "[Simulacao] Atualização do mapa Web ignorada: " + + ex.Message + ); + } + catch (Exception ex) + { + Variaveis.MostrarLog( + "[Simulacao] Erro atualizando mapa Web: " + ex + ); + } + finally + { + Interlocked.Exchange(ref _mapaWebEmExecucao, 0); } } + #endregion - + #region Snapshots visuais limitados + + private static List CriarTrajetoriaVisual( + IList origem, + int limite) + { + if (origem == null || origem.Count == 0) + return new List(); + + limite = Math.Max(16, limite); + + for (int tentativa = 0; tentativa < 3; tentativa++) + { + try + { + int count = origem.Count; + int passo = Math.Max( + 1, + (int)Math.Ceiling(count / (double)limite) + ); + + var resultado = + new List( + Math.Min(count, limite + 64) + ); + + for (int i = 0; i < count; i++) + { + PontoTrajetoriaModel ponto = origem[i]; + if (ponto == null || ponto.Posicao == null) + continue; + + bool estrutural = + ponto.PontoBorda || + ponto.PontoLigacao || + ponto.Tipo == TipoPontoRua.BordaEntrada || + ponto.Tipo == TipoPontoRua.BordaSaida || + ponto.Tipo == TipoPontoRua.LigacaoEntrada || + ponto.Tipo == TipoPontoRua.LigacaoSaida; + + if (i == 0 || + i == count - 1 || + estrutural || + i % passo == 0) + { + if (resultado.Count == 0 || + !ReferenceEquals(resultado.Last(), ponto)) + { + resultado.Add(ponto); + } + } + } + + return resultado; + } + catch (ArgumentOutOfRangeException) + { + Thread.Yield(); + } + catch (InvalidOperationException) + { + Thread.Yield(); + } + } + + return new List(); + } + + private List ObterTrilhaVisual() + { + lock (_estadoSimulacaoLock) + return new List(_trilhaVisual); + } + + private List> ObterRuasVisuais( + IList> ruas) + { + if (ruas == null || ruas.Count == 0) + return new List>(); + + // As ruas da trajetória são imutáveis durante uma operação. + // Cachear por instância + quantidade evita percorrer milhares de + // pontos em todo frame apenas para validar o cache visual. + if (ReferenceEquals(_ruasFonteCache, ruas) && + ruas.Count == _ruasFonteQuantidadeRuas) + { + return _ruasVisualCache; + } + + var resultado = new List>(); + + for (int i = 0; i < ruas.Count; i++) + { + resultado.Add( + DecimarPontosGps( + ruas[i], + MaxPontosPorRuaVisual + ) + ); + } + + _ruasFonteCache = ruas; + _ruasFonteQuantidadeRuas = ruas.Count; + _ruasVisualCache = resultado; + + return resultado; + } + + private static List DecimarPontosGps( + IList origem, + int limite) + { + if (origem == null || origem.Count == 0) + return new List(); + + int count = origem.Count; + int passo = Math.Max( + 1, + (int)Math.Ceiling(count / (double)Math.Max(2, limite)) + ); + + var resultado = new List( + Math.Min(count, limite + 1) + ); + + for (int i = 0; i < count; i += passo) + { + GPSModel ponto = origem[i]; + if (ponto != null) + resultado.Add(ponto); + } + + GPSModel ultimo = origem[count - 1]; + if (ultimo != null && + (resultado.Count == 0 || + !ReferenceEquals(resultado[resultado.Count - 1], ultimo))) + { + resultado.Add(ultimo); + } + + return resultado; + } + + private static List ConverterTrajetoriaParaGps( + IList pontos) + { + var resultado = new List(pontos?.Count ?? 0); + + if (pontos == null) + return resultado; + + for (int i = 0; i < pontos.Count; i++) + { + GPSModel posicao = pontos[i]?.Posicao; + if (posicao == null) + continue; + + resultado.Add(new GPSModel() + { + Latitude = posicao.Latitude, + Longitude = posicao.Longitude + }); + } + + return resultado; + } + + private static List CopiarListaSegura(IList origem) + { + if (origem == null || origem.Count == 0) + return new List(); + + for (int tentativa = 0; tentativa < 3; tentativa++) + { + try + { + int count = origem.Count; + var resultado = new List(count); + + for (int i = 0; i < count; i++) + resultado.Add(origem[i]); + + return resultado; + } + catch (ArgumentOutOfRangeException) + { + Thread.Yield(); + } + catch (InvalidOperationException) + { + Thread.Yield(); + } + } + + return new List(); + } + + private static double CalcularDistanciaSimulacao( + IList pontos) + { + if (pontos == null || pontos.Count < 2) + return 0.0; + + double total = 0.0; + var anterior = new GPSModel() + { + Latitude = pontos[0].latitude, + Longitude = pontos[0].longitude + }; + + for (int i = 1; i < pontos.Count; i++) + { + var atual = new GPSModel() + { + Latitude = pontos[i].latitude, + Longitude = pontos[i].longitude + }; + + double distancia = + GPSUtils.DistanciaEntrePontos(anterior, atual); + + if (ValorFinito(distancia) && distancia >= 0) + total += distancia; + + anterior = atual; + } + + return total; + } + + #endregion + + #region Controles e comandos + + private void Mapa_RuasSelecionadasAlteradas( + object sender, + EventArgs e) + { + _ruasFonteCache = null; + _ruasFonteQuantidadeRuas = -1; + _ruasVisualCache = new List>(); + Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1); + } private void btnCarregar_Click(object sender, EventArgs e) { - Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click((Control) sender, e); + Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click( + (Control)sender, + e + ); } - private void btnIniciarGPS_Click(object sender, EventArgs e) + private bool TentarCriarPosicaoInicialDaTela( + out GPSModel posicao) { - posicaoAtual = new GPSModel() + posicao = null; + + double latitude; + double longitude; + double angulo; + + if (!TentarLerDouble(txtLatitude.Text, out latitude) || + !TentarLerDouble(txtLongitude.Text, out longitude) || + !TentarLerDouble(txtAnguloGPS.Text, out angulo) || + !CoordenadaGeograficaValida(latitude, longitude)) { - DataHora = DateTime.Now, - Lon0 = double.Parse(txtLongitude.Text.Replace(".", ",")), - LongitudeAnt = double.Parse(txtLongitude.Text.Replace(".", ",")), - Longitude = double.Parse(txtLongitude.Text.Replace(".", ",")), - LatitudeAnt = double.Parse(txtLatitude.Text.Replace(".", ",")), - Lat0 = double.Parse(txtLatitude.Text.Replace(".", ",")), - Latitude = double.Parse(txtLatitude.Text.Replace(".", ",")), - Heartbeat = GPSService.UltimaLeitura.Heartbeat, - OrientacaoReal = double.Parse(txtAnguloGPS.Text.Replace(".", ",")), - }; - AtualizarPosicaoGPS(); - btnAcrescentarGPS.Enabled = true; - txtAnguloGPS.Enabled = true; - txtDistanciaGPS.Enabled = true; - //btnIniciarGPS.Enabled = false; - //txtLatitude.ReadOnly = true; - //txtLongitude.ReadOnly = true; + return false; + } + + GPSModel atual = GPSService.GetSnapshot() ?? new GPSModel(); + + atual.DataHora = DateTime.Now; + atual.Momento = DateTime.Now; + atual.Lon0 = longitude; + atual.LongitudeAnt = longitude; + atual.Longitude = longitude; + atual.LatitudeAnt = latitude; + atual.Lat0 = latitude; + atual.Latitude = latitude; + atual.OrientacaoReal = GPSUtils.NormalizarAngulo(angulo); + atual.AnguloCarroDefinido = atual.OrientacaoReal; + atual.OrientacaoMovimento = atual.OrientacaoReal; + atual.TipoOrientacao = "A"; + atual.Distancia = 0.0; + atual.Velocidade = 0.0; + + posicao = atual; + return true; } - private void AtualizarPosicaoGPS() + private async void btnIniciarGPS_Click(object sender, EventArgs e) { - if (!GPSService.Iniciado || (txtLongitude.Text != "" && txtLatitude.Text != "")) + GPSModel nova; + + if (!TentarCriarPosicaoInicialDaTela(out nova)) { - double orientacaoReal = GPSUtils.NormalizarAngulo(posicaoAtual.OrientacaoReal); - - GPSService.UltimaLeitura.Momento = posicaoAtual.Momento; - GPSService.UltimaLeitura.DataHora = posicaoAtual.DataHora; - GPSService.UltimaLeitura.UltimoComandoRespondido = posicaoAtual.UltimoComandoRespondido; - - GPSService.UltimaLeitura.Latitude = posicaoAtual.Latitude; - GPSService.UltimaLeitura.Longitude = posicaoAtual.Longitude; - GPSService.UltimaLeitura.LatitudeAnt = posicaoAtual.LatitudeAnt; - GPSService.UltimaLeitura.LongitudeAnt = posicaoAtual.LongitudeAnt; - - GPSService.UltimaLeitura.OrientacaoReal = orientacaoReal; - GPSService.UltimaLeitura.AnguloCarroDefinido = orientacaoReal; - GPSService.UltimaLeitura.TipoOrientacao = "A"; - - GPSService.UltimaLeitura.OrientacaoMovimento = - double.IsNaN(posicaoAtual.OrientacaoMovimento) || double.IsInfinity(posicaoAtual.OrientacaoMovimento) - ? orientacaoReal - : GPSUtils.NormalizarAngulo(posicaoAtual.OrientacaoMovimento); - - GPSService.UltimaLeitura.Distancia = posicaoAtual.Distancia; - GPSService.UltimaLeitura.Velocidade = posicaoAtual.Velocidade; - - GPSService.UltimaLeitura.TimestampOri = posicaoAtual.TimestampOri.Clone(); - GPSService.UltimaLeitura.TimestampPos = posicaoAtual.TimestampPos.Clone(); - - GPSService.UltimaLeitura.Heartbeat = posicaoAtual.Heartbeat; - GPSService.UltimaLeitura.Inicializado = true; + MessageBox.Show( + "Latitude, longitude ou orientação inválida.", + "Simulação", + MessageBoxButtons.OK, + MessageBoxIcon.Warning + ); + return; } - GPSService.AtualizarAtrasoPosicoes(Convert.ToInt32(txtDelayPosicao.Text)); - GPSService.AtualizarCoordenadasGPS(); - } + AtualizarConfiguracaoSimulacaoDaTela(); - private async void btnAcrescentarGPS_Click(object sender, EventArgs e) - { - GPSModel novaPosicao = null; - velocidadeCarroMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(Variaveis.OperacaoEmAndamento.SimulacaoRpmControle); - if (Variaveis.OperacaoEmAndamento.Controle.TiposControle.FirstOrDefault(x => x.Tipo == T_Code.Mov).DirecaoAtual == Direcao.Tras) - velocidadeCarroMs *= -1; - //Console.WriteLine($"RPM SP: {Variaveis.OperacaoEmAndamento.SimulacaoRpmControle} ({velocidadeCarroMs:F2} ms)"); + PrepararLeituraSimulada( + nova, + 0.0, + Volatile.Read(ref _tempoGpsConfiguradoMs) + ); - Variaveis.OperacaoEmAndamento.DispMvd.Dados.VelocidadeMedia = velocidadeCarroMs; + btnIniciarGPS.Enabled = false; - 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 = GPSService.UltimaLeitura; - - double anguloControle = 0.0; - TipoMovimentoDirecional tipoMovimento = Variaveis.OperacaoEmAndamento.Controle.TipoMovimento; - double veloccidade = Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.StatusOperacaoAtual != StatusOperacao.EmAndamento ? 0 : velocidadeCarroMs; - - var tipoControle = Variaveis.OperacaoEmAndamento.Parametros.Controle.DirTipoMovimento; - - if (passosOperacao.Count > 0 && Variaveis.OperacaoEmAndamento.Simulando) + try { - novaPosicao = passosOperacao[Math.Min(passosOperacao.Count - 1, idxPassoOperacao)]; - idxPassoOperacao++; - } - else - { - anguloControle = double.Parse(txtAnguloControle.Text); - - bool atualizarAnguloControle = (ticksControle >= ticksControleMax); - if (atualizarAnguloControle) + await Task.Run(delegate { - ticksControle = 0; - } - else - { - ticksControle++; - } + AtualizarPosicaoGPS( + nova, + Volatile.Read(ref _delayPosicaoPassos) + ); + }); - if (atualizarAnguloControle) - { - btnCalcularAngulo_Click(new object(), new EventArgs()); - anguloControle = double.Parse(txtAnguloControle.Text); - } - } + lock (_estadoSimulacaoLock) + posicaoAtual = nova.Clone(); - // Calcula a nova posicao do robo - //GPSModel pos = NovaPosicaoSimulacao(velocidadeMs, tempoPassado); - if (novaPosicao == null) - { - novaPosicao = VariaveisEquipamento.SimularNovaPosicao(tipoMovimento, anguloControle, veloccidade, tempoPassado, anguloAtual, _Gps); - } + btnAcrescentarGPS.Enabled = true; + txtAnguloGPS.Enabled = true; + txtDistanciaGPS.Enabled = true; - posicaoAtual = novaPosicao; - - - //Console.WriteLine($"C#: {DadosPosicao(novaPosicao)}"); - //Console.WriteLine($"Py: {DadosPosicao(pos)}"); - - // Atualiza a interface gráfica - txtAnguloGPS.Text = posicaoAtual.OrientacaoReal.ToString("0.00"); - txtVelocidade.Text = veloccidade.ToString("0.00"); - txtDistanciaGPS.Text = posicaoAtual.Distancia.ToString("0.0000"); - txtLatitude.Text = posicaoAtual.LatitudeAnt.ToString(); - txtLongitude.Text = posicaoAtual.LongitudeAnt.ToString(); - - UltimaAtualizacaoGps = DateTime.Now; - - AtualizarPosicaoGPS(); - - Variaveis.OperacaoEmAndamento.Sensoriamento.AtualizarDados(); - - if (!chbAutomatico.Checked) - { + Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1); + AtualizarCamposPosicaoSimuladaNaTela(); AtualizarDadosTela(); } - } - - private GPSModel NovaPosicaoSimulacao(double velocidadeMs, double tempo) - { - if (Variaveis.OperacaoEmAndamento.Controle.SimulacaoMPC.Count() > 1) + catch (Exception ex) { - 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; + Variaveis.MostrarLog( + "[Simulacao] Erro iniciando GPS simulado: " + ex + ); + MessageBox.Show( + "Não foi possível iniciar o GPS simulado.\n\n" + + ex.Message, + "Simulação", + MessageBoxButtons.OK, + MessageBoxIcon.Error + ); + } + finally + { + if (!IsDisposed) + btnIniciarGPS.Enabled = true; } - return null; } - private string DadosPosicao(GPSModel pos) + private async void btnAcrescentarGPS_Click( + object sender, + EventArgs e) { - if (pos == null) return ""; - return $"Lat: {pos.Latitude}, Long: {pos.Longitude}, Theta: {pos.OrientacaoReal}, Distancia: {pos.Distancia}"; + if (Interlocked.Exchange( + ref _passoManualEmExecucao, + 1) == 1) + { + return; + } + + AtualizarConfiguracaoSimulacaoDaTela(); + btnAcrescentarGPS.Enabled = false; + + try + { + await Task.Run(() => + ExecutarPassoSimulacaoAsync( + CancellationToken.None, + aguardarGate: true + ) + ); + + AtualizarCamposPosicaoSimuladaNaTela(); + AtualizarDadosTela(); + } + catch (Exception ex) + { + Variaveis.MostrarLog( + "[Simulacao] Erro no passo manual: " + ex + ); + } + finally + { + Interlocked.Exchange(ref _passoManualEmExecucao, 0); + + if (!IsDisposed) + btnAcrescentarGPS.Enabled = true; + } } - private void btnIniciarSimulacao_Click(object sender, EventArgs e) + private async void btnIniciarSimulacao_Click( + object sender, + EventArgs e) { - if (!Variaveis.OperacaoEmAndamento.Mapa.RuasPercorrer.Any()) + var operacao = Variaveis.OperacaoEmAndamento; + + if (operacao?.Mapa?.RuasPercorrer == null || + !operacao.Mapa.RuasPercorrer.Any()) { MessageBox.Show("Selecione as ruas para realizar a operação!"); return; } - - if (!Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.OperacaoIniciada && !parar) + + try { - Variaveis.OperacaoEmAndamento.IniciarSimulacao(); + if (!operacao.Sensoriamento.Operacao.OperacaoIniciada && + !parar) + { + // O operador pode alterar o ângulo depois de posicionar o + // carro. Sincroniza a edição final imediatamente antes de + // iniciar para que o primeiro passo use esse heading. + GPSModel posicaoInicial; + + if (!TentarCriarPosicaoInicialDaTela( + out posicaoInicial)) + { + MessageBox.Show( + "Latitude, longitude ou orientação inválida.", + "Simulação", + MessageBoxButtons.OK, + MessageBoxIcon.Warning + ); + return; + } + + PrepararLeituraSimulada( + posicaoInicial, + 0.0, + Volatile.Read(ref _tempoGpsConfiguradoMs) + ); + + await Task.Run(delegate + { + AtualizarPosicaoGPS( + posicaoInicial, + Volatile.Read(ref _delayPosicaoPassos) + ); + }); + + lock (_estadoSimulacaoLock) + posicaoAtual = posicaoInicial.Clone(); + + await operacao.IniciarSimulacao(); + } + + bool iniciada = + operacao.Sensoriamento.Operacao.OperacaoIniciada; + + btnIniciarSimulacao.Text = iniciada + ? "Parar Simulação" + : "Iniciar Simulação"; + + if (iniciada) + { + GPSService.PenultimaLeitura.Latitude = + GPSService.UltimaLeitura.Latitude; + GPSService.PenultimaLeitura.Longitude = + GPSService.UltimaLeitura.Longitude; + + lock (_estadoSimulacaoLock) + _trilhaVisual.Clear(); + + Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1); + AtualizarDadosTela(); + } } - - btnIniciarSimulacao.Text = Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.OperacaoIniciada ? "Parar Simulação" : "Iniciar Simulação"; - if (Variaveis.OperacaoEmAndamento.Sensoriamento.Operacao.OperacaoIniciada) + catch (Exception ex) { - GPSService.PenultimaLeitura.Latitude = GPSService.UltimaLeitura.Latitude; - GPSService.PenultimaLeitura.Longitude = GPSService.UltimaLeitura.Longitude; - //Variaveis.OperacaoEmAndamento.Mapa.GerarTrajetoriaDinamica(); - - AtualizarDadosTela(); + Variaveis.MostrarLog( + "[Simulacao] Erro iniciando simulação: " + ex + ); + MessageBox.Show( + "Não foi possível iniciar a simulação.\n\n" + + ex.Message, + "Simulação", + MessageBoxButtons.OK, + MessageBoxIcon.Error + ); } } private void btnCarregarMapa_Click(object sender, EventArgs e) { - Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click((Control)sender, e); + Variaveis.OperacaoEmAndamento.Mapa.btnCarregar_Click( + (Control)sender, + e + ); } private void btnCarregarOperacao_Click(object sender, EventArgs e) { - passosOperacao = new List(); - 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) + using (var ofd = new OpenFileDialog()) { - OperacaoDadosModel data = JsonConvert.DeserializeObject(File.ReadAllText(ofd.FileName)); + ofd.Filter = "Arquivos de Operação|*.lgop"; + ofd.InitialDirectory = + Application.StartupPath + "\\" + + Variaveis.CaminhoOperacoes; - passosOperacao = JsonConvert.DeserializeObject(OperacaoModel.DeserializarDadosOperacao(ofd.FileName, T_Code.Gps.ToString())).ToList(); + if (ofd.ShowDialog() != DialogResult.OK) + return; - txtTempoGPS.Text = "1000"; - lblCarregado.Text = "Operação"; + try + { + // Mantém a validação do contêiner da operação. + JsonConvert.DeserializeObject( + File.ReadAllText(ofd.FileName) + ); + + GPSModel[] dados = + JsonConvert.DeserializeObject( + OperacaoModel.DeserializarDadosOperacao( + ofd.FileName, + T_Code.Gps.ToString() + ) + ) ?? Array.Empty(); + + DefinirPassosOperacao(dados.ToList()); + + txtTempoGPS.Text = "1000"; + lblCarregado.Text = "Operação"; + AtualizarConfiguracaoSimulacaoDaTela(); + } + catch (Exception ex) + { + MessageBox.Show( + "Não foi possível carregar a operação.\n\n" + + ex.Message, + "Simulação", + MessageBoxButtons.OK, + MessageBoxIcon.Error + ); + } } } private void btnCarregarTrajetoria_Click(object sender, EventArgs e) { - passosOperacao = new List(); - 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) + using (var ofd = new OpenFileDialog()) { - var DadosMapa = JsonConvert.DeserializeObject(File.ReadAllText(ofd.FileName)); + ofd.Filter = "Arquivos de Trajetória|*.json"; + ofd.InitialDirectory = + Application.StartupPath + "\\" + + Variaveis.CaminhoMapasConvertidos; - passosOperacao = DadosMapa.features[0].geometry.coordinates - .Select(x => new GPSModel() - { - Latitude = x[1], - Longitude = x[0] - }) - .ToList(); + if (ofd.ShowDialog() != DialogResult.OK) + return; - txtTempoGPS.Text = "200"; - lblCarregado.Text = "Trajetória"; + try + { + var dadosMapa = + JsonConvert.DeserializeObject( + File.ReadAllText(ofd.FileName) + ); + + var coordenadas = dadosMapa? + .features? + .FirstOrDefault()? + .geometry? + .coordinates; + + if (coordenadas == null || coordenadas.Count == 0) + throw new InvalidDataException( + "O arquivo não contém uma trajetória válida." + ); + + var passos = coordenadas + .Where(x => x != null && x.Count >= 2) + .Select(x => new GPSModel() + { + Latitude = x[1], + Longitude = x[0], + LatitudeAnt = x[1], + LongitudeAnt = x[0] + }) + .ToList(); + + DefinirPassosOperacao(passos); + + txtTempoGPS.Text = "200"; + lblCarregado.Text = "Trajetória"; + AtualizarConfiguracaoSimulacaoDaTela(); + } + catch (Exception ex) + { + MessageBox.Show( + "Não foi possível carregar a trajetória.\n\n" + + ex.Message, + "Simulação", + MessageBoxButtons.OK, + MessageBoxIcon.Error + ); + } } } - private void pnlOrientacaoTrajeto_Paint(object sender, PaintEventArgs e) + private void DefinirPassosOperacao(List passos) { - try + lock (_estadoSimulacaoLock) { - //double angulo = Variaveis.OperacaoEmAndamento.Trajetoria.AnguloCaminho; - //double anguloCorredor = Variaveis.OperacaoEmAndamento.Trajetoria.AnguloMedioCorredor; - double angulo = Variaveis.OperacaoEmAndamento.Trajetoria?.ProximoPonto?.Orientacao ?? 0; - double anguloCorredor = Variaveis.OperacaoEmAndamento.Trajetoria?.AnguloCaminho ?? 0; - FuncoesGlobais.DesenharInclinacao((Panel)sender, e, (float)angulo, 90, (float)anguloCorredor); + passosOperacao = passos ?? new List(); + idxPassoOperacao = 0; + _trilhaVisual.Clear(); + historicoAngulosControle.Clear(); } - 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 ?? 0; - FuncoesGlobais.DesenharInclinacao((Panel)sender, e, (float)angulo, 90, (float)dif); - } - catch { } + Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1); } private void btnAtualizarLeitura_Click(object sender, EventArgs e) { + AtualizarConfiguracaoSimulacaoDaTela(); AtualizarDadosTela(); } private void btnCalcularAngulo_Click(object sender, EventArgs e) { - historicoAngulosControle.Enqueue(Variaveis.OperacaoEmAndamento.SimulacaoAnguloControle); - int errosConsiderar = int.Parse(txtDelayComando.Text); - while (historicoAngulosControle.Count > 1 && (historicoAngulosControle.Count > errosConsiderar || errosConsiderar == 0)) + AtualizarConfiguracaoSimulacaoDaTela(); + + double angulo = CalcularAnguloControleAtrasado( + Volatile.Read(ref _delayComandoPassos) + ); + + lock (_estadoSimulacaoLock) + _ultimoAnguloControleAplicado = angulo; + + txtAnguloControle.Text = angulo.ToString("0.00"); + } + + private double CalcularAnguloControleAtrasado(int errosConsiderar) + { + double novoAngulo = + Variaveis.OperacaoEmAndamento?.SimulacaoAnguloControle ?? 0.0; + + if (!ValorFinito(novoAngulo)) + novoAngulo = 0.0; + + lock (_estadoSimulacaoLock) { - historicoAngulosControle.Dequeue(); + historicoAngulosControle.Enqueue(novoAngulo); + + while (historicoAngulosControle.Count > 1 && + (historicoAngulosControle.Count > errosConsiderar || + errosConsiderar == 0)) + { + historicoAngulosControle.Dequeue(); + } + + _ultimoAnguloControleAplicado = + historicoAngulosControle.Peek(); + + return _ultimoAnguloControleAplicado; } - - txtAnguloControle.Text = historicoAngulosControle.Peek().ToString("0.00"); } - private void chbSonarVirando_CheckedChanged(object sender, EventArgs e) + private double ObterUltimoAnguloControle() { - + lock (_estadoSimulacaoLock) + return _ultimoAnguloControleAplicado; } - private void chbAutomatico_CheckedChanged(object sender, EventArgs e) + private void chbSonarVirando_CheckedChanged( + object sender, + EventArgs e) { - UltimaAtualizacaoGps = DateTime.MinValue; + } + + private void chbAutomatico_CheckedChanged( + object sender, + EventArgs e) + { + Interlocked.Exchange( + ref _automatico, + chbAutomatico.Checked ? 1 : 0 + ); } private void btnReiniciar_Click(object sender, EventArgs e) { + EncerrarRecursos(); Variaveis.OperacaoEmAndamento.ReiniciarSimulacao(); - frmSimulacaoMapaGPS frm = new frmSimulacaoMapaGPS(); + + var frm = new frmSimulacaoMapaGPS(); frm.Show(); - - this.Close(); + Close(); } - private void cmbTipoControleDirecional_SelectedIndexChanged(object sender, EventArgs e) + private void cmbTipoControleDirecional_SelectedIndexChanged( + object sender, + EventArgs e) { - Variaveis.OperacaoEmAndamento.Parametros.Controle.DirTipoMovimento = (TiposControladorDirecional)cmbTipoControleDirecional.SelectedIndex; - RedisService.AtualizarCampos(CtxKey.DadosOperacao, ("configurado", false)); + if (cmbTipoControleDirecional.SelectedIndex < 0 || + Variaveis.OperacaoEmAndamento?.Parametros?.Controle == null) + { + return; + } + + Variaveis.OperacaoEmAndamento.Parametros.Controle + .DirTipoMovimento = + (TiposControladorDirecional) + cmbTipoControleDirecional.SelectedIndex; + + RedisService.AtualizarCampos( + CtxKey.DadosOperacao, + ("configurado", false) + ); } - private void cmbStatusCorredor_SelectedIndexChanged(object sender, EventArgs e) + private void cmbStatusCorredor_SelectedIndexChanged( + object sender, + EventArgs e) { var json = RedisService.Get(CtxKey.DadosVisualWorker); - if (string.IsNullOrWhiteSpace(json)) return; + if (string.IsNullOrWhiteSpace(json)) + return; + JObject root; try { @@ -601,52 +1706,285 @@ namespace AgroBase.Forms { return; } + long tsUnix = root["ts_analise"]?.Value() ?? 0; + RedisService.AtualizarCampos( CtxKey.DadosVisualWorker, ("ts_analise", tsUnix + 1), - ("segmentacao.status_corredor", cmbStatusCorredor.SelectedIndex) + ( + "segmentacao.status_corredor", + cmbStatusCorredor.SelectedIndex + ) ); } - private void chbAcompanharCarro_CheckedChanged(object sender, EventArgs e) + private void chbAcompanharCarro_CheckedChanged( + object sender, + EventArgs e) { - MapaDinamico.AlterarTravaRover(chbAcompanharCarro.Checked); + MapaDinamico?.AlterarTravaRover( + chbAcompanharCarro.Checked + ); } - - - private void btnRetornarBase_Click(object sender, EventArgs e) { - //OperacaoModel.CarregarParametrosOperacaoPadrao(ModoOperacao.RetornoBase, true); - Variaveis.OperacaoEmAndamento.Trajetoria.IniciarRetornoBase(VariaveisOperacao.PosicaoBase); - Variaveis.OperacaoEmAndamento.Simulando = true; + var operacao = Variaveis.OperacaoEmAndamento; + if (operacao?.Trajetoria == null) + return; + + operacao.Trajetoria.IniciarRetornoBase( + VariaveisOperacao.PosicaoBase + ); + operacao.Simulando = true; + + Interlocked.Exchange(ref _forcarTrajetoriaWeb, 1); } private void btnFixarBase_Click(object sender, EventArgs e) { + double latitude; + double longitude; + double angulo; + + if (!TentarLerDouble(txtLatitude.Text, out latitude) || + !TentarLerDouble(txtLongitude.Text, out longitude) || + !TentarLerDouble(txtAnguloGPS.Text, out angulo)) + { + MessageBox.Show( + "Latitude, longitude ou orientação inválida.", + "Simulação", + MessageBoxButtons.OK, + MessageBoxIcon.Warning + ); + return; + } + VariaveisOperacao.PosicaoBase = new GPSModel() { DataHora = DateTime.Now, - Lon0 = double.Parse(txtLongitude.Text.Replace(".", ",")), - LongitudeAnt = double.Parse(txtLongitude.Text.Replace(".", ",")), - Longitude = double.Parse(txtLongitude.Text.Replace(".", ",")), - LatitudeAnt = double.Parse(txtLatitude.Text.Replace(".", ",")), - Lat0 = double.Parse(txtLatitude.Text.Replace(".", ",")), - Latitude = double.Parse(txtLatitude.Text.Replace(".", ",")), + Momento = DateTime.Now, + Lon0 = longitude, + LongitudeAnt = longitude, + Longitude = longitude, + LatitudeAnt = latitude, + Lat0 = latitude, + Latitude = latitude, Heartbeat = GPSService.UltimaLeitura.Heartbeat, - OrientacaoReal = double.Parse(txtAnguloGPS.Text.Replace(".", ",")), + OrientacaoReal = angulo, + AnguloCarroDefinido = angulo }; - GPSService.EnviarCoordenadasParaMapa(VariaveisOperacao.PosicaoBase.Latitude, VariaveisOperacao.PosicaoBase.Longitude, VariaveisOperacao.PosicaoBase.AnguloCarroDefinido, false, rover: false); + + GPSService.EnviarCoordenadasParaMapa( + VariaveisOperacao.PosicaoBase.Latitude, + VariaveisOperacao.PosicaoBase.Longitude, + VariaveisOperacao.PosicaoBase.AnguloCarroDefinido, + false, + rover: false + ); } private void btnLiberarCorredor_Click(object sender, EventArgs e) { - int idx_corredor = (Variaveis.OperacaoEmAndamento.Trajetoria.CorredorAtual?.Idx ?? 0) + (Variaveis.OperacaoEmAndamento.Trajetoria.CorredorAtual?.Concluido ?? false ? 1 : 0); - Variaveis.OperacaoEmAndamento.Trajetoria.AtualizarDadosAutonomiaCorredor(idxCorredor: idx_corredor, bat_liberada: true, herb_liberado: true); - } - } + var trajetoria = Variaveis.OperacaoEmAndamento?.Trajetoria; + if (trajetoria == null) + return; - + int idxCorredor = + (trajetoria.CorredorAtual?.Idx ?? 0) + + ((trajetoria.CorredorAtual?.Concluido ?? false) ? 1 : 0); + + trajetoria.AtualizarDadosAutonomiaCorredor( + idxCorredor: idxCorredor, + bat_liberada: true, + herb_liberado: true + ); + } + + #endregion + + #region Paints auxiliares + + private void pnlOrientacaoTrajeto_Paint( + object sender, + PaintEventArgs e) + { + try + { + double angulo = + Variaveis.OperacaoEmAndamento + .Trajetoria? + .ProximoPonto? + .Orientacao ?? 0; + + double anguloCorredor = + Variaveis.OperacaoEmAndamento + .Trajetoria? + .AnguloCaminho ?? 0; + + 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 diferenca = + Variaveis.OperacaoEmAndamento + .Sensoriamento? + .Trajetoria? + .ErroOrientacaoAngularProximoPonto ?? 0; + + FuncoesGlobais.DesenharInclinacao( + (Panel)sender, + e, + (float)angulo, + 90, + (float)diferenca + ); + } + catch + { + } + } + + #endregion + + #region Utilidades + + private static bool TentarLerDouble( + string texto, + out double valor) + { + if (double.TryParse( + texto, + NumberStyles.Float, + CultureInfo.CurrentCulture, + out valor)) + { + return ValorFinito(valor); + } + + string normalizado = + (texto ?? string.Empty).Trim().Replace(',', '.'); + + if (double.TryParse( + normalizado, + NumberStyles.Float, + CultureInfo.InvariantCulture, + out valor)) + { + return ValorFinito(valor); + } + + valor = 0.0; + return false; + } + + private static bool CoordenadaGeograficaValida( + double latitude, + double longitude) + { + return ValorFinito(latitude) && + ValorFinito(longitude) && + latitude >= -90.0 && latitude <= 90.0 && + longitude >= -180.0 && longitude <= 180.0 && + !(Math.Abs(latitude) < 0.0000001 && + Math.Abs(longitude) < 0.0000001); + } + + private static bool ValorFinito(double valor) + { + return !double.IsNaN(valor) && !double.IsInfinity(valor); + } + + private static int LimitarInteiro( + int valor, + int minimo, + int maximo) + { + return Math.Max(minimo, Math.Min(maximo, valor)); + } + + // Mantidos porque podem ser usados durante depuração manual do form. + private GPSModel NovaPosicaoSimulacao( + double velocidadeMs, + double tempo) + { + List pontos = + CopiarListaSegura( + Variaveis.OperacaoEmAndamento? + .Controle? + .SimulacaoMPC + ); + + if (pontos.Count <= 1) + return null; + + MPCSimulacaoModel posicao = pontos[1]; + + return new GPSModel() + { + Latitude = posicao.latitude, + Longitude = posicao.longitude, + LatitudeAnt = posicao.latitude, + LongitudeAnt = posicao.longitude, + OrientacaoReal = posicao.orientacao, + Distancia = velocidadeMs * tempo, + }; + } + + private string DadosPosicao(GPSModel pos) + { + if (pos == null) + return string.Empty; + + return + $"Lat: {pos.Latitude}, Long: {pos.Longitude}, " + + $"Theta: {pos.OrientacaoReal}, Distancia: {pos.Distancia}"; + } + + #endregion + } } diff --git a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs index 8686b9426..37425f1d3 100644 --- a/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs +++ b/AgroBase/AgroBase/Models/Operacoes/OperacaoModel.cs @@ -1959,7 +1959,7 @@ namespace AgroBase.Models var dispMvd = DispMvd; if (dispMvd?.Dados?.Modulos != null) // && value != Controle.TipoMovimento - { + { Controle.DefinirTipoMovimento(value, dispMvd.Dados.Modulos); } } diff --git a/AgroBase/AgroBase/Services/GPSService.cs b/AgroBase/AgroBase/Services/GPSService.cs index cb50a9e72..01984a431 100644 --- a/AgroBase/AgroBase/Services/GPSService.cs +++ b/AgroBase/AgroBase/Services/GPSService.cs @@ -12,6 +12,7 @@ using System.Net.Sockets; using System.Text; using System.Threading; using System.Threading.Tasks; +using System.Windows.Forms; using static AgroBase.Models.Enums; namespace AgroBase.Services @@ -171,6 +172,94 @@ namespace AgroBase.Services private static string _lastError; private static readonly object _metricsTextLock = new object(); + + + private const int IntervaloTrajetoriaMapaMs = 1_000; + private const int MaxPontosTrajetoriaMapa = 800; + + private static int _publicacaoMapaAutomaticaHabilitada = 1; + private static long _ultimaTrajetoriaMapaMono; + + /// + /// Permite que uma tela seja dona da publicação visual sem interromper o + /// processamento GNSS, Redis, trajetória ou histórico da operação. + /// O valor padrão preserva o comportamento legado. + /// + public static bool PublicacaoMapaAutomaticaHabilitada + { + get + { + return Volatile.Read( + ref _publicacaoMapaAutomaticaHabilitada + ) == 1; + } + set + { + Volatile.Write( + ref _publicacaoMapaAutomaticaHabilitada, + value ? 1 : 0 + ); + } + } + + public static void AtualizarLeituraSimulada(GPSModel leitura, int atrasoPosicaoPassos = 0) + { + if (leitura == null) + throw new ArgumentNullException(nameof(leitura)); + + lock (_stateLock) + { + double orientacaoReal = + GPSUtils.NormalizarAngulo(leitura.OrientacaoReal); + + UltimaLeitura.Momento = leitura.Momento; + UltimaLeitura.DataHora = leitura.DataHora; + UltimaLeitura.UltimoComandoRespondido = + leitura.UltimoComandoRespondido; + + UltimaLeitura.Latitude = leitura.Latitude; + UltimaLeitura.Longitude = leitura.Longitude; + UltimaLeitura.LatitudeAnt = leitura.LatitudeAnt; + UltimaLeitura.LongitudeAnt = leitura.LongitudeAnt; + + UltimaLeitura.OrientacaoReal = orientacaoReal; + UltimaLeitura.AnguloCarroDefinido = orientacaoReal; + UltimaLeitura.TipoOrientacao = "A"; + + UltimaLeitura.OrientacaoMovimento = + double.IsNaN(leitura.OrientacaoMovimento) || + double.IsInfinity(leitura.OrientacaoMovimento) + ? orientacaoReal + : GPSUtils.NormalizarAngulo( + leitura.OrientacaoMovimento + ); + + UltimaLeitura.Distancia = leitura.Distancia; + UltimaLeitura.Velocidade = leitura.Velocidade; + + if (leitura.TimestampOri != null) + UltimaLeitura.TimestampOri = + leitura.TimestampOri.Clone(); + + if (leitura.TimestampPos != null) + UltimaLeitura.TimestampPos = + leitura.TimestampPos.Clone(); + + UltimaLeitura.Heartbeat = leitura.Heartbeat; + UltimaLeitura.Inicializado = true; + + // Monitor/lock do C# é reentrante; os dois métodos abaixo usam o + // mesmo lock e permanecem em uma única transação de leitura GNSS. + AtualizarAtrasoPosicoes( + Math.Max(0, atrasoPosicaoPassos) + ); + AtualizarCoordenadasGPS(); + } + } + + + + // ============================================================ // INICIALIZAÇÃO DA PORTA // ============================================================ @@ -2198,14 +2287,30 @@ namespace AgroBase.Services op.Trajetoria.LoopAtualizaDados(); } - AtualizarTrajetoriaDinamica(); - - EnviarCoordenadasParaMapa(UltimaLeitura.Latitude, UltimaLeitura.Longitude, UltimaLeitura.AnguloCarroDefinido, op.Sensoriamento.Operacao.OperacaoIniciada, rover: true); - - GPSModel posicaoBase = VariaveisOperacao.ObterPosicaoBaseSnapshot(); - if ((posicaoBase?.UltimoComandoRespondido ?? DateTime.MinValue) > DateTime.UtcNow.AddSeconds(-60)) + if (PublicacaoMapaAutomaticaHabilitada) { - EnviarCoordenadasParaMapa(posicaoBase.Latitude, posicaoBase.Longitude, posicaoBase.OrientacaoReal, false, rover: false); + AtualizarTrajetoriaDinamica(); + + EnviarCoordenadasParaMapa( + UltimaLeitura.Latitude, + UltimaLeitura.Longitude, + UltimaLeitura.AnguloCarroDefinido, + op.Sensoriamento.Operacao.OperacaoIniciada, + rover: true + ); + + GPSModel posicaoBase = VariaveisOperacao.ObterPosicaoBaseSnapshot(); + + if ((posicaoBase?.UltimoComandoRespondido ?? DateTime.MinValue) > DateTime.UtcNow.AddSeconds(-60)) + { + EnviarCoordenadasParaMapa( + posicaoBase.Latitude, + posicaoBase.Longitude, + posicaoBase.OrientacaoReal, + false, + rover: false + ); + } } PenultimaLeitura.Inicializado = UltimaLeitura.Inicializado; @@ -2221,29 +2326,172 @@ namespace AgroBase.Services // PUBLICAÇÃO LOCAL LATEST-ONLY // ============================================================ - public static void EnviarCoordenadasParaMapa(double Latitude, double Longitude, double Orientacao, bool EmFoco, bool rover) + public static void EnviarCoordenadasParaMapa(double latitude, double longitude, double orientacao, bool emFoco, bool rover) { - int ID = rover ? MapasVariaveisModel.IDMarcadorRover : MapasVariaveisModel.IDMarcadorBase; - Variaveis.OperacaoEmAndamento?.Mapa?.AtualizarPosicaoMapaAsync(ID, Latitude, Longitude, Orientacao, EmFoco); + MapasModel mapa = Variaveis.OperacaoEmAndamento?.Mapa; + Panel painel = mapa?.pnlMapa; + + // Produto headless: retorna antes de tocar no WebView. + if (painel == null || painel.IsDisposed || !painel.IsHandleCreated) + { + return; + } + + int id = rover + ? MapasVariaveisModel.IDMarcadorRover + : MapasVariaveisModel.IDMarcadorBase; + + ExecutarMapaNaUi( + mapa, + delegate + { + Task tarefa = mapa.AtualizarPosicaoMapaAsync( + id, + latitude, + longitude, + orientacao, + emFoco + ); + + ObservarFalhaMapa( + tarefa, + "GPSService.EnviarCoordenadasParaMapa" + ); + } + ); } public static void AtualizarTrajetoriaDinamica() { var op = Variaveis.OperacaoEmAndamento; + var trajetoria = op?.Trajetoria; + MapasModel mapa = op?.Mapa; + Panel painel = mapa?.pnlMapa; - var dinamica = op.Trajetoria?._TrajetoriaDinamica; + // No produto sem UI, não cria cópia nem serializa a trajetória. + if (trajetoria == null || painel == null || painel.IsDisposed || !painel.IsHandleCreated) + { + return; + } - if (!(dinamica?.Any() ?? false)) + long anterior = Interlocked.Read(ref _ultimaTrajetoriaMapaMono); + + if (GetAgeMs(anterior) < IntervaloTrajetoriaMapaMs) return; - var trajetoria = op.Trajetoria.TrajetoriaDinamica.Select(x => new - { - latitude = x.Latitude, - longitude = x.Longitude - }) - .ToArray(); + long agora = Stopwatch.GetTimestamp(); - op?.Mapa?.DefinirTrajetoriaPlanejadaAsync(op.Trajetoria.TrajetoriaDinamica); + // Caso duas leituras cheguem juntas, somente uma publica. + if (Interlocked.CompareExchange(ref _ultimaTrajetoriaMapaMono, agora, anterior) != anterior) + { + return; + } + + List pontos = CriarTrajetoriaMapaVisual(trajetoria._TrajetoriaDinamica, MaxPontosTrajetoriaMapa); + + if (pontos.Count == 0) + return; + + ExecutarMapaNaUi( + mapa, + delegate + { + Task tarefa = mapa.DefinirTrajetoriaPlanejadaAsync(pontos); + + ObservarFalhaMapa( + tarefa, + "GPSService.AtualizarTrajetoriaDinamica" + ); + } + ); + } + + private static List CriarTrajetoriaMapaVisual(IList origem, int limite) + { + var resultado = new List(); + + if (origem == null || origem.Count == 0) + return resultado; + + int count = origem.Count; + int passo = Math.Max(1, (int)Math.Ceiling(count / (double)Math.Max(2, limite))); + + for (int i = 0; i < count; i++) + { + PontoTrajetoriaModel ponto = origem[i]; + + if (ponto?.Posicao == null) + continue; + + bool estrutural = + ponto.PontoBorda || + ponto.PontoLigacao || + ponto.Tipo == TipoPontoRua.BordaEntrada || + ponto.Tipo == TipoPontoRua.BordaSaida || + ponto.Tipo == TipoPontoRua.LigacaoEntrada || + ponto.Tipo == TipoPontoRua.LigacaoSaida; + + if (i != 0 && i != count - 1 && !estrutural && i % passo != 0) + { + continue; + } + + resultado.Add(new GPSModel() + { + Latitude = ponto.Posicao.Latitude, + Longitude = ponto.Posicao.Longitude + }); + } + + return resultado; + } + + private static void ExecutarMapaNaUi(MapasModel mapa, Action acao) + { + Panel painel = mapa?.pnlMapa; + + if (acao == null || painel == null || painel.IsDisposed || !painel.IsHandleCreated) + { + return; + } + + try + { + if (painel.InvokeRequired) + { + painel.BeginInvoke(acao); + return; + } + + acao(); + } + catch (ObjectDisposedException) + { + } + catch (InvalidOperationException) + { + // A janela pode estar fechando entre as validações. + } + } + + private static void ObservarFalhaMapa(Task tarefa, string origem) + { + if (tarefa == null) + return; + + _ = tarefa.ContinueWith( + t => + { + Exception erro = t.Exception?.GetBaseException(); + RecordError( + "[" + origem + "] " + + (erro?.Message ?? "Falha desconhecida no mapa.") + ); + }, + CancellationToken.None, + TaskContinuationOptions.OnlyOnFaulted, + TaskScheduler.Default + ); } private static async Task LocalPublisherLoopAsync(CancellationToken cancellationToken)