RobotApp/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs

using RobotApp.Common.Shares;
using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;
using RobotApp.Services.Exceptions;
using RobotApp.Services.Robot.Simulation.Navigation.Algorithm;
using RobotApp.VDA5050.Order;

namespace RobotApp.Services.Robot.Simulation.Navigation;

public class SimulationNavigation : INavigation, IDisposable
{
    public NavigationState State => NavState;
    public bool Driving => NavDriving;
    public bool IsReady => true;
    public double VelocityX => Visualization.Vx;
    public double VelocityY => Visualization.Vy;
    public double Omega => Visualization.Omega;

    public event Action? OnNavigationFinished;

    protected NavigationState NavState = NavigationState.Idle;
    protected bool NavDriving = false;

    protected const int CycleHandlerMilliseconds = 50;
    private const double Scale = 1;
    private WatchTimer<SimulationNavigation>? NavigationTimer;
    //private HighPrecisionTimer<SimulationNavigation>? NavigationTimer;

    protected double TargetAngle = 0;
    protected PID? RotatePID;
    protected readonly double AngularVelocity;

    protected PID? MovePID;
    protected FuzzyLogic? MoveFuzzy;
    protected PurePursuit? MovePurePursuit;

    protected Node[] OrderNodes = [];
    protected Edge[] OrderEdges = [];
    protected NavigationNode[] SimulationOrderNodes = [];

    protected readonly SimulationVisualization Visualization;
    protected readonly SimulationVelocity VelocityController;
    protected readonly RobotConfiguration RobotConfiguration;
    protected readonly RobotPathPlanner PathPlanner;

    protected NavigationNode[] NavigationPath = [];
    protected NavigationNode? CurrentBaseNode;
    protected NavigationState ResumeState = NavigationState.Idle;

    private readonly Logger<SimulationNavigation> Logger;

    private bool IsIdle => NavState == NavigationState.Idle || NavState == NavigationState.Canceled;

    public SimulationNavigation(IServiceProvider ServiceProvider)
    {
        using var scope = ServiceProvider.CreateScope();
        Logger = scope.ServiceProvider.GetRequiredService<Logger<SimulationNavigation>>();
        Visualization = scope.ServiceProvider.GetRequiredService<SimulationVisualization>();
        RobotConfiguration = scope.ServiceProvider.GetRequiredService<RobotConfiguration>();
        PathPlanner = scope.ServiceProvider.GetRequiredService<RobotPathPlanner>();
        VelocityController = new(Visualization, RobotConfiguration.SimulationModel);
        AngularVelocity = RobotConfiguration.SimulationModel.MaxAngularVelocity * RobotConfiguration.Width / 2 / 2 / RobotConfiguration.RadiusWheel;
    }

    protected void HandleNavigationStart()
    {
        NavigationTimer = new((int)(CycleHandlerMilliseconds / Scale), NavigationHandler, Logger);
        NavigationTimer.Start();
    }

    protected void HandleNavigationStop()
    {
        NavigationTimer?.Dispose();
        NavigationTimer = null;
    }

    protected virtual void NavigationHandler() { }

    public void CancelMovement()
    {
        Dispose();
    }

    public void Move(Node[] nodes, Edge[] edges)
    {
        if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState));
        NavState = NavigationState.Initializing;
        if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
        if (edges.Length < 1) throw new SimulationException(RobotErrors.Error1003(edges.Length));
        if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));

        var pathDirection = PathPlanner.GetNavigationNode(Visualization.Theta, nodes, edges);
        NavigationPath = PathPlanner.PathSplit(pathDirection, edges);

        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
        MoveFuzzy = new FuzzyLogic(); 
        MovePurePursuit = new PurePursuit()
            .WithLookheadDistance(0.35) 
            .WithPath([.. NavigationPath]);

        (NavigationNode node, int index) = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y);
        if(index >= NavigationPath.Length - 1) return;

        double angleFoward = Math.Atan2(NavigationPath[index + 1].Y - node.Y, NavigationPath[index + 1].X - node.X) * 180 / Math.PI;
        double angleBacward = Math.Atan2(node.Y - NavigationPath[index + 1].Y, node.X - NavigationPath[index + 1].X) * 180 / Math.PI;
        OrderNodes = nodes;
        OrderEdges = edges;
        SimulationOrderNodes = pathDirection;
        Rotate(node.Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
    }

    public void MoveStraight(double x, double y)
    {
        if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState));
        var headRobotNode = new NavigationNode()
        {
            X = Visualization.X * Math.Acos(Visualization.Theta * Math.PI / 180),
            Y = Visualization.Y * Math.Asin(Visualization.Theta * Math.PI / 180),
        };
        var goalNode = new NavigationNode()
        {
            NodeId = "RobotGoal",
            X = x,
            Y = y,
        };
        var currentRobotNode = new NavigationNode()
        {
            NodeId = "RobotCurrentNode",
            X = Visualization.X,
            Y = Visualization.Y,
        }; 
        goalNode.Theta = MathExtensions.GetVectorAngle(currentRobotNode.X, currentRobotNode.Y, headRobotNode.X, headRobotNode.Y, goalNode.X, goalNode.Y) > 90 ?
                            Math.Atan2(currentRobotNode.Y - goalNode.Y, currentRobotNode.X - goalNode.X) :
                            Math.Atan2(goalNode.Y - currentRobotNode.Y, goalNode.X - currentRobotNode.X);
        currentRobotNode.Theta = goalNode.Theta;
        NavigationPath = PathPlanner.PathSplit([currentRobotNode, goalNode], [new Edge()
        {
            EdgeId = "Straight edge",
            Trajectory = new Trajectory()
            {
                Degree = 1,
                ControlPoints = []
            },
            StartNodeId = currentRobotNode.NodeId,
            EndNodeId = goalNode.NodeId,
        }]);
        
        MovePID = new PID().WithKp(1.5).WithKi(0.0001).WithKd(0.8);
        MoveFuzzy = new FuzzyLogic();  
        MovePurePursuit = new PurePursuit()
            .WithLookheadDistance(0.25) 
            .WithPath(NavigationPath);
        
        double Angle = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X);
        Rotate(Angle * 180 / Math.PI);
        UpdateOrder(goalNode.NodeId);
    }

    public void Pause()
    {
        ResumeState = State;
        NavState = NavigationState.Paused;
    }

    public void Resume()
    {
        NavState = ResumeState;
    }

    public void Rotate(double angle)
    {
        if (!IsIdle && NavState!= NavigationState.Initializing) throw new SimulationException(RobotErrors.Error1012(NavState));
        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
        TargetAngle = MathExtensions.NormalizeAngle(angle);
        NavState = NavigationState.Rotating;
        HandleNavigationStart();
    }

    public void UpdateOrder(string lastBaseNodeId)
    {
        var lastBaseNode = SimulationOrderNodes.FirstOrDefault(n => n.NodeId == lastBaseNodeId);
        if (lastBaseNode is not null && lastBaseNode.NodeId != CurrentBaseNode?.NodeId)
        {
            CurrentBaseNode = lastBaseNode;
            MovePurePursuit?.UpdateGoal(lastBaseNode.NodeId);
        }
    }

    public void RefreshOrder(Node[] nodes, Edge[] edges)
    {
        throw new NotImplementedException();
    }

    public void SetSpeed(double speed)
    {
        throw new NotImplementedException();
    }

    public void Dispose()
    {
        HandleNavigationStop();
        VelocityController.Stop();
        OrderNodes = [];
        OrderEdges = [];
        CurrentBaseNode = null;
        NavigationPath = [];
        SimulationOrderNodes = [];
        NavDriving = false;
        NavState = NavigationState.Idle;
        OnNavigationFinished?.Invoke();
        GC.SuppressFinalize(this);
    }

    public void Refresh()
    {
        NavState = NavigationState.Idle;
    }
}