agrobot_base/AgroBase/AgroBase/Models/MPCController.cs

using AgroBase.Models;
using AgroBase.Services;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Threading.Tasks;
using static AgroBase.Models.Enums;

public class MPCController
{
    private string TopicoRota = "mpc/rota";
    private string TopicoPosicao = "mpc/posicao";
    private string TopicoComando = "mpc/comando";
    private int Horizonte = 20;
    private double _dt = 0;
    private DateTime _ultimaAtualizacao = DateTime.MinValue;
    public Process pythonProcess;
    public readonly object processLock = new object();
    public bool Iniciado = false;

    public MPCComandoModel ComandoAtual = new MPCComandoModel();

    public MPCController()
    {
        
    }

    public async Task<bool> Inicializar()
    {
        await Variaveis.MqttService.AdicionarNovoTopico(TopicoRota);
        await Variaveis.MqttService.AdicionarNovoTopico(TopicoPosicao);
        await Variaveis.MqttService.AdicionarNovoTopico(TopicoComando, true, 2, async (mensagem) =>
        {
            if (!Iniciado && mensagem.Mensagem == "OK")
            {
                Iniciado = true;
            }
            else
            {
                try
                {
                    ComandoAtual = JsonConvert.DeserializeObject<MPCComandoModel>(mensagem.Mensagem);
                }
                catch (Exception ex)
                {
                    Console.WriteLine("Erro ao deserializar resposta do MPC: " + ex.Message.ToString());
                }
                _ultimaAtualizacao = DateTime.Now;
            }
        });

        lock (processLock)
        {
            pythonProcess = PythonService.RunScript(PythonService.ScriptMPCController, new string[] {});
        }

        bool ScriptIniciado() => Iniciado;
        await FuncoesGlobais.AguardarCondicaoAsync(ScriptIniciado);

        return Iniciado;
    }

    public async Task EnviarDadosTrajetoria(List<PontoTrajetoriaModel> _trajetoria)
    {
        var _Controle = Variaveis.OperacaoEmAndamento.Controle;

        var trajetoria = new
        {
            pontos = _trajetoria
                    .Select(p => new
                    {
                        lat = p.Posicao.Latitude,
                        lon = p.Posicao.Longitude,
                        tipo = (int)p.Tipo,
                        distanciaMargem = p.LarguraCorredor * 0.8
                    })
                    .ToList(),
            horizonte = Horizonte,
            angulo_max_graus = _Controle.Angulo_Max,
            distancia_entre_eixos = (VariaveisEquipamento.DistanciaEntreEixos / 100.0),
            velocidade_min = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMin),
            velocidade_max = FuncoesMatematicas.CalculaVelocidadeMsPercentual(_Controle.PercentualVelocidadeMax)
        };

        await Variaveis.MqttService.PublishAsync(
            Variaveis.MqttService.Topicos.First(x => x.Topico == TopicoRota),
            JsonConvert.SerializeObject(trajetoria)
        );
    }

    public async Task<MPCComandoModel> Compute(double _lat, double _long, double _theta, double _velocidadeMs)
    {
        DateTime EnviadoEm = DateTime.Now;

        double taxaHz = GPSService.TaxaAmostragemHz; // ex: 10 Hz
        double tempoIdeal = 1.0 / taxaHz; // = 0.1 s
        double tempoMinimo = tempoIdeal * 0.5; // 50% abaixo
        double tempoMaximo = tempoIdeal * 2.0; // até 2x acima
        _dt = (EnviadoEm - _ultimaAtualizacao).TotalSeconds;
        double dtCalc = Math.Max(tempoMinimo, Math.Min(tempoMaximo, _dt));

        var _Sensoriamento = Variaveis.OperacaoEmAndamento.Sensoriamento;

        var dadosEnvio = new
        {
            lat = _lat,
            lon = _long,
            theta = _theta,
            velocidade = _velocidadeMs,
            dt = dtCalc,
            contexto = new
            {
                StatusCarro = (int)_Sensoriamento.Trajetoria.StatusCarro, 
                DentroCorredor = _Sensoriamento.Trajetoria.CorredorAtual.Dentro, 
                ManobrandoEntreRuas = _Sensoriamento.Trajetoria.ManobrandoEntreRuas
            }
        };

        await Variaveis.MqttService.PublishAsync(
            Variaveis.MqttService.Topicos.First(x => x.Topico == TopicoPosicao),
            JsonConvert.SerializeObject(dadosEnvio)
        );

        bool Respondido() => _ultimaAtualizacao > EnviadoEm;
        await FuncoesGlobais.AguardarCondicaoAsync(Respondido, 200, 10);

        return ComandoAtual;
    }

    public MPCController Clone()
    {
        return new MPCController()
        {
            ComandoAtual = ComandoAtual?.Clone() ?? new MPCComandoModel(),
            Horizonte = Horizonte, 
            Iniciado = Iniciado,
            _ultimaAtualizacao = _ultimaAtualizacao,
            _dt = _dt
        }
;    }

    public void Dispose()
    {
        lock (processLock)
        {
            pythonProcess?.Kill();
            pythonProcess?.Dispose();
            pythonProcess = null;
        }

        Iniciado = false;
        ComandoAtual = new MPCComandoModel();
        Task.Run(async () => {
            Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoRota));
            Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoPosicao));
            await Variaveis.MqttService.UnsubscribeAsync(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoComando));
            Variaveis.MqttService.Topicos.Remove(Variaveis.MqttService.Topicos.FirstOrDefault(x => x.Topico == TopicoComando));
        });
    }

}

public class MPCComandoModel
{
    public TipoMovimentoDirecional tipo { get; set; } = TipoMovimentoDirecional.RodasDianteiras;
    public double angulo { get; set; } = 0;
    public List<MPCSimulacaoModel> simulacao { get; set; } = new List<MPCSimulacaoModel>();

    public MPCComandoModel Clone()
    {
        return new MPCComandoModel()
        {
            tipo = tipo, 
            angulo = angulo,
            simulacao = new List<MPCSimulacaoModel>(simulacao)
        };
    }
}

public class MPCSimulacaoModel
{
    public double latitude { get; set; }
    public double longitude { get; set; }
}