agrobot_base/AgroBase/AgroBase/Models/MPCControllerAprimorado.cs

using AgroBase.Models;
using AgroBase.Services;
using System;
using System.Collections.Generic;
using System.Linq;

public class MPCControllerAprimorado
{
    private double distanciaPrevisao = 2.0;
    private int horizonte;
    private double maxAnguloDirecao;
    private double maxAngulo4Rodas;
    private double velocidadeMin;
    private double velocidadeMax;

    private double pesoOrientacao = 2.5;
    private double pesoLateral = 1.5;
    private double pesoDistancia = 0.8;
    private double pesoSuavidade = 0.5;
    private double limiteErroOrientacao = 45.0;
    private double limiteErroLateral = 2.5; // metros

    private DateTime ultimaAtualizacao = DateTime.MinValue;

    public MPCControllerAprimorado()
    {
        var op = Variaveis.OperacaoEmAndamento;

        this.maxAnguloDirecao = op.Parametros?.Controle?.DirAnguloMaximo ?? 30.0;
        this.maxAngulo4Rodas = this.maxAnguloDirecao * 0.8;
        this.velocidadeMin = op.Parametros?.Controle?.MovVelocidadeCErvasPercent ?? 20.0;
        this.velocidadeMax = op.Parametros?.Controle?.MovVelocidadeSErvasPercent ?? 100.0;
    }

    public (double anguloControle, Enums.TipoMovimentoDirecional modo) CalcularControle(GPSModel posicalAtual, double velocidadeAtual, List<GPSModel> proximaTrajetoria, Enums.TipoMovimentoDirecional[] modosPermitidos)
    {
        if (proximaTrajetoria == null || proximaTrajetoria.Count < 2)
            return (0, Enums.TipoMovimentoDirecional.RodasDianteiras);

        if (proximaTrajetoria.Count > 3)
        {
            proximaTrajetoria = new List<GPSModel>(proximaTrajetoria.Take(3));
        }

        if (ultimaAtualizacao == DateTime.MinValue)
        {
            ultimaAtualizacao = DateTime.Now.AddMilliseconds(-(1.0 / GPSService.TaxaAmostragemHz));
        }

        double dt = (DateTime.Now - ultimaAtualizacao).TotalSeconds;
        double tempoMax = (1.0 / GPSService.TaxaAmostragemHz) * 1.5;
        dt = Math.Max(0.5, Math.Min(dt, tempoMax));

        double velMs = FuncoesMatematicas.CalculaVelocidadeMsPercentual(velocidadeAtual);
        double passoDist = velMs * dt;

        horizonte = Math.Min(Convert.ToInt32(distanciaPrevisao / passoDist), proximaTrajetoria.Count);

        var candidatos = GerarCandidatos(modosPermitidos);
        double melhorCusto = double.MaxValue;
        (double angulo, Enums.TipoMovimentoDirecional modo) melhorControle = (0, Enums.TipoMovimentoDirecional.RodasDianteiras);

        foreach (var candidato in candidatos)
        {
            double custo = SimularTrajetoriaComReavaliacao(posicalAtual, proximaTrajetoria, posicalAtual.AnguloCarroDefinido, candidato.angulo, candidato.modo, velMs, dt);
            //Console.WriteLine($"Candidato: {candidato.angulo}, Custo: {custo.ToString()}");
            if (custo < melhorCusto)
            {
                melhorCusto = custo;
                melhorControle = candidato;
            }
        }

        ultimaAtualizacao = DateTime.Now;
        return melhorControle;
    }

    private List<(double angulo, Enums.TipoMovimentoDirecional modo)> GerarCandidatos(Enums.TipoMovimentoDirecional[] modos)
    {
        var candidatos = new List<(double, Enums.TipoMovimentoDirecional)>();

        foreach (var modo in modos)
        {
            double maxAng = (modo == Enums.TipoMovimentoDirecional.MovimentoArco || modo == Enums.TipoMovimentoDirecional.MovimentoDiagonal)
                ? maxAngulo4Rodas : maxAnguloDirecao;

            for (double ang = -maxAng; ang <= maxAng; ang += 5.0)
            {
                candidatos.Add((ang, modo));
            }
        }

        return candidatos;
    }

    private double SimularTrajetoriaComReavaliacao(GPSModel posicaoInicial, List<GPSModel> trajeto, double anguloInicial, double anguloControleInicial, Enums.TipoMovimentoDirecional modoInicial, double velocidadeMs, double dt)
    {
        var op = Variaveis.OperacaoEmAndamento;

        double custoTotal = 0;
        GPSModel posAtual = posicaoInicial;
        double anguloAtual = anguloInicial;
        double anguloControle = anguloControleInicial;
        Enums.TipoMovimentoDirecional modo = modoInicial;

        posAtual = VariaveisEquipamento.SimularNovaPosicao(modoInicial, anguloControleInicial, velocidadeMs, dt, anguloAtual, posAtual);
        anguloAtual = posAtual.OrientacaoReal;

        for (int i = 0; i < horizonte; i++)
        {
            double melhorCustoIteracao = double.MaxValue;

            foreach (var candidato in GerarCandidatos(new[] { modo }))
            {
                var novaPos = VariaveisEquipamento.SimularNovaPosicao(candidato.modo, candidato.angulo, velocidadeMs, dt, anguloAtual, posAtual);
                var pontoAlvo = trajeto[Math.Min(i + 1, trajeto.Count - 1)];

                double orientacaoAlvo = GPSUtils.CalcularOrientacao(posAtual, pontoAlvo);

                double d0 = GPSUtils.DistanciaEntrePontos(posAtual, novaPos);
                double a0 = GPSUtils.CalcularOrientacao(posAtual, novaPos);

                double d1 = GPSUtils.DistanciaEntrePontos(posAtual, pontoAlvo);
                double a1 = GPSUtils.CalcularOrientacao(posAtual, pontoAlvo);

                double d2 = GPSUtils.DistanciaEntrePontos(novaPos, pontoAlvo);
                double a2 = GPSUtils.CalcularOrientacao(novaPos, pontoAlvo);

                double erroDist = d2 - d1;
                double erroOri = Math.Abs(GPSUtils.CalcularDiferencaAngulo(novaPos.OrientacaoReal, orientacaoAlvo));

                (GPSModel ponto, int idx) = GPSUtils.PontoMaisProximoTrechoComIndice(posAtual, trajeto);
                int idxNovoPonto = op.Trajetoria.PontoAtual.idxPonto + idx;
                PontoTrajetoriaModel pontoAtual = op.Trajetoria._TrajetoriaFixa[idxNovoPonto].Clone();

                int idxRuaEsquerda = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaEsquerda;
                double distEsq = op.Trajetoria.CalcularDistanciaLateral(true, posAtual, idxRuaEsquerda, pontoAtual.Posicao);

                int idxRuaDireita = op.Trajetoria._Corredores[pontoAtual.idxCorredor].idxRuaDireita;
                double distDir = op.Trajetoria.CalcularDistanciaLateral(false, posAtual, idxRuaDireita, pontoAtual.Posicao);

                double erroLat = Math.Abs(distEsq - distDir);

                double deltaAngulo = Math.Abs(GPSUtils.CalcularDiferencaAngulo(novaPos.OrientacaoReal, anguloAtual));

                //Console.WriteLine($"Angulo Inicial: {anguloControleInicial}, Passo: {i}, Candidato: {candidato.angulo}, Erro lateral: {erroLat}, Erro orientacao: {erroOri}");

                if (false && (erroOri > limiteErroOrientacao || erroLat > limiteErroLateral))
                {
                    continue; // Ignora este candidato
                }

                double custo = (erroOri * pesoOrientacao) + (erroLat * pesoLateral) + (erroDist * pesoDistancia) + (deltaAngulo * pesoSuavidade);

                if (custo < melhorCustoIteracao)
                {
                    melhorCustoIteracao = custo;
                    posAtual = novaPos;
                    anguloAtual = novaPos.OrientacaoReal;
                    anguloControle = candidato.angulo;
                    modo = candidato.modo;

                    //Console.WriteLine($"Angulo Inicial: {anguloControleInicial}, Passo: {i}, Candidato: {candidato.angulo}, Custo: {custo}, Erro lateral: {erroLat}, Erro orientacao: {erroOri}, Erro distancia: {erroDist}");
                }
            }

            custoTotal += melhorCustoIteracao;

            if (melhorCustoIteracao == double.MaxValue)
            {
                break;
            }
        }

        return custoTotal;
    }

}