agrobot_base/AgroBase/AgroBase/Models/MPCController.cs

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

public class MPCController
{
    private int horizon;
    private double maxSteeringAngle;
    private double maxSteeringAngleFourWheels;
    private double minVelocity;
    private double maxVelocity;
    private double accelerationLimit;

    // Pesos da função de custo ajustados
    private double distancePredict = 3.0;
    private double weightPosition = 1.0;
    private double weightAlignment = 0.8;
    private double weightSteering = 0.5;
    private double weightVelocity = 0.3;
    private double weightAcceleration = 1.0;  // Penaliza mudanças bruscas de velocidade
    private double weightObstacle = 2.5; // Maior penalização para obstáculos
    private double weightLaneCentering = 1.2; // Mantém o robô no centro do corredor

    private DateTime _ultimaAtualizacao = DateTime.MinValue;

    public MPCController()
    {
        // Definições baseadas em variáveis globais
        this.horizon = Convert.ToInt32(distancePredict / TrajetoriaMapaOperacaoModel.DistanciaEntrePontos);
        this.maxSteeringAngle = Variaveis.OperacaoEmAndamento?.Controle?.Angulo_Max ?? 30.0;
        this.maxSteeringAngleFourWheels = this.maxSteeringAngle * 1.0;
        this.minVelocity = Variaveis.OperacaoEmAndamento?.Controle?.PercentualVelocidadeMin ?? 20.0;
        this.maxVelocity = Variaveis.OperacaoEmAndamento?.Controle?.PercentualVelocidadeMax ?? 100.0;
        this.accelerationLimit = 1.5; // Limita aceleração a valores realistas
    }

    public (double steeringAngle, Enums.TipoMovimentoDirecional mode) ComputeControl(
        double currentX, double currentY, double currentTheta,
        double targetX, double targetY, double targetTheta,
        double currentVelocity, double leftEdge, double rightEdge, List<Obstaculo> obstacles)
    {
        double dt = (DateTime.Now - _ultimaAtualizacao).TotalSeconds;
        dt = Math.Max(0.01, Math.Min(dt, 0.2));

        // Define o número total de passos e passos por grupo
        int totalSteps = 10;
        int stepsPerGroup = 5;

        // Calcula o número de grupos
        int numGroups = totalSteps / stepsPerGroup;

        // Pré-calcula todas as combinações de controle
        List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions = PreGenerateControlOptions();

        // Variáveis para armazenar o melhor custo e a melhor sequência
        double bestCost = double.MaxValue;
        List<(double steering, Enums.TipoMovimentoDirecional mode)> bestSequence = null;

        // Função de avaliação (passada como parâmetro)
        Func<List<(double steering, Enums.TipoMovimentoDirecional mode)>, double> evaluateSequence = sequence =>
        {
            return SimulateTrajectory(
                currentX, currentY, currentTheta,
                targetX, targetY, targetTheta,
                currentVelocity, sequence,
                leftEdge, rightEdge, obstacles, dt, Variaveis.OperacaoEmAndamento.Sensoriamento.Trajetoria.CorredorAtual.Dentro,
                stepsPerGroup // Passa o número de passos por grupo
            );
        };

        // Gera e avalia todas as sequências dinamicamente
        GenerateAndEvaluateSequences(
            new List<(double steering, Enums.TipoMovimentoDirecional mode)>(),
            numGroups, // Número de grupos
            controlOptions,
            ref bestCost,
            ref bestSequence,
            evaluateSequence
        );

        _ultimaAtualizacao = DateTime.Now;

        // Retorna o primeiro comando da melhor sequência
        return bestSequence[0];
    }


    private List<(double steering, Enums.TipoMovimentoDirecional mode)> PreGenerateControlOptions()
    {
        List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions = new List<(double steering, Enums.TipoMovimentoDirecional mode)>();

        // Gera todas as opções de steering (ângulos) e modos
        List<double> steeringOptions = new List<double>();
        for (double s = -maxSteeringAngle; s <= maxSteeringAngle; s += 5.0) // Passo de 5 graus
        {
            steeringOptions.Add(s);
        }

        List<Enums.TipoMovimentoDirecional> modeOptions = new List<Enums.TipoMovimentoDirecional>()
        {
            Enums.TipoMovimentoDirecional.RodasDianteiras,
            Enums.TipoMovimentoDirecional.MovimentoArco
        };

        // Cria todas as combinações de steering e mode
        foreach (var steering in steeringOptions)
        {
            foreach (var mode in modeOptions)
            {
                controlOptions.Add((steering, mode));
            }
        }

        return controlOptions;
    }

    private void GenerateControlSequences(List<(double steering, Enums.TipoMovimentoDirecional mode)> currentSequence, int remainingSteps, List<List<(double steering, Enums.TipoMovimentoDirecional mode)>> allSequences, List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions) // Pré-geradas
    {
        // Base da recursão: se não houver mais passos no horizonte, salva a sequência
        if (remainingSteps == 0)
        {
            allSequences.Add(new List<(double steering, Enums.TipoMovimentoDirecional mode)>(currentSequence));
            return;
        }

        // Itera sobre as combinações pré-calculadas
        foreach (var control in controlOptions)
        {
            // Adiciona o controle atual à sequência
            currentSequence.Add(control);

            // Chama recursivamente para o próximo passo, diminuindo o horizonte
            GenerateControlSequences(currentSequence, remainingSteps - 1, allSequences, controlOptions);

            // Remove o controle para explorar a próxima combinação
            currentSequence.RemoveAt(currentSequence.Count - 1);
        }
    }

    private void GenerateAndEvaluateSequences(
        List<(double steering, Enums.TipoMovimentoDirecional mode)> currentSequence,
        int remainingSteps,
        List<(double steering, Enums.TipoMovimentoDirecional mode)> controlOptions,
        ref double bestCost,
        ref List<(double steering, Enums.TipoMovimentoDirecional mode)> bestSequence,
        Func<List<(double steering, Enums.TipoMovimentoDirecional mode)>, double> evaluateSequence)
    {
        // Base da recursão: se não houver mais passos, avalia a sequência
        if (remainingSteps == 0)
        {
            double cost = evaluateSequence(currentSequence);

            // Verifica se a sequência atual tem o menor custo
            if (cost < bestCost)
            {
                bestCost = cost;
                bestSequence = new List<(double steering, Enums.TipoMovimentoDirecional mode)>(currentSequence);
            }

            return;
        }

        // Itera sobre as combinações de controle
        foreach (var control in controlOptions)
        {
            // Adiciona o controle atual à sequência
            currentSequence.Add(control);

            // Chama recursivamente para o próximo passo, diminuindo o horizonte
            GenerateAndEvaluateSequences(currentSequence, remainingSteps - 1, controlOptions, ref bestCost, ref bestSequence, evaluateSequence);

            // Remove o controle para explorar a próxima combinação
            currentSequence.RemoveAt(currentSequence.Count - 1);
        }
    }




    private double SimulateTrajectory(
        double lat, double lon, double theta,
        double targetLat, double targetLon, double targetTheta,
        double currentVelocity,
        List<(double steering, Enums.TipoMovimentoDirecional mode)> sequence,
        double leftEdge, double rightEdge, List<Obstaculo> obstacles, double dt, bool insideStreet,
        int stepsPerGroup)
    {
        double cost = 0;

        var posicaoAtual = new GPSModel { Latitude = lat, Longitude = lon, OrientacaoReal = theta };
        var posicaoAlvo = new GPSModel { Latitude = targetLat, Longitude = targetLon };
        (double targetX, double targetY) = GPSUtils.ConverterLatLongParaMetros(posicaoAlvo, posicaoAtual);

        foreach (var control in sequence)
        {
            double steering = control.steering;
            Enums.TipoMovimentoDirecional mode = control.mode;

            // Simula "stepsPerGroup" passos para o mesmo comando
            for (int step = 0; step < stepsPerGroup; step++)
            {
                var novaPosicao = VariaveisEquipamento.SimularNovaPosicao(mode, steering, currentVelocity, dt, theta, posicaoAtual);

                posicaoAtual = novaPosicao;
                theta = novaPosicao.OrientacaoReal;

                (double novaX, double novaY) = GPSUtils.ConverterLatLongParaMetros(novaPosicao, posicaoAlvo);

                double distanceError = Math.Sqrt(Math.Pow(targetX - novaX, 2) + Math.Pow(targetY - novaY, 2));
                double laneError = insideStreet ? Math.Abs(leftEdge - rightEdge) : 0.0;

                // Acumula o custo
                cost += (distanceError * 10.0) + (laneError * 20.0);
            }
        }

        return cost;
    }




}