using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Shared.Enums;

namespace RobotNet10.RobotApp.Services.Simulation.Navigation;

public class DifferentialNavigation : SimulationNavigation
{
    private readonly Logger<DifferentialNavigation> Logger;

    public DifferentialNavigation(IServiceProvider ServiceProvider) : base(ServiceProvider)
    {
        using var scope = ServiceProvider.CreateScope();
        Logger = scope.ServiceProvider.GetRequiredService<Logger<DifferentialNavigation>>();
    }

    private bool IsBackToPath = false;
    private double? BackToAngle;

    protected override void NavigationHandler()
    {
        try
        {
            if (NavState == NavigationState.Rotating)
            {
                if (RotatePID is not null)
                {
                    double Error = Visualization.Theta - TargetAngle;
                    if (Error > 180) Error -= 360;
                    else if (Error < -180) Error += 360;
                    if (Math.Abs(Error) < 1)
                    {
                        if(IsBackToPath && BackToAngle.HasValue)
                        {
                            TargetAngle = BackToAngle.Value;
                            BackToAngle = null;
                            IsBackToPath = false;
                        }
                        else if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit.Waypoints_Value.Count > 2) NavState = NavigationState.Moving;
                        else
                        {
                            NavState = NavigationState.Completed;
                            Dispose();
                        }
                    }
                    else
                    {
                        var SpeedCal = RotatePID.PID_step(Error * Math.PI / 180, AngularVelocity, -AngularVelocity, CycleHandlerMilliseconds / 1000.0);
                        VelocityController.SetSpeed(SpeedCal, SpeedCal, CycleHandlerMilliseconds / 1000.0);
                    }
                }
            }
            else if (NavState == NavigationState.Moving)
            {
                if (MovePurePursuit is not null && MovePurePursuit.Waypoints_Value is not null && MovePurePursuit?.OrderNodes is not null && MovePurePursuit.OrderNodes.Length > 1 && GoalRotate is not null)
                {
                    if (MovePID is not null && MoveFuzzy is not null && MovePurePursuit is not null)
                    {
                        var DistanceToGoal = Math.Sqrt(Math.Pow(Visualization.X - MovePurePursuit.OrderNodes[^1].X, 2) + Math.Pow(Visualization.Y - MovePurePursuit.OrderNodes[^1].Y, 2));
                        var DistanceToCheckingNode = Math.Sqrt(Math.Pow(Visualization.X - GoalRotate.X, 2) + Math.Pow(Visualization.Y - GoalRotate.Y, 2));
                        var deviation = GoalRotate.NodeId == MovePurePursuit.OrderNodes[^1].NodeId ? 0.02 : 0.05;
                        if (DistanceToCheckingNode > deviation)
                        {
                            double SpeedTarget = MovePID.PID_step(DistanceToCheckingNode, MaxVelocity, 0, CycleHandlerMilliseconds / 1000.0);
                            double AngularVel = MovePurePursuit.PurePursuit_step(Visualization.X, Visualization.Y, Visualization.Theta * Math.PI / 180);
                            AngularVel *= MovePurePursuit.Waypoints_Value[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD ? 1 : -1;
                            (double AngularVelocityLeft, double AngularVelocityRight) = MoveFuzzy.Fuzzy_step(SpeedTarget, AngularVel, CycleHandlerMilliseconds / 1000.0);

                            if (MovePurePursuit.Waypoints_Value[MovePurePursuit.OnNodeIndex].Direction == RobotDirection.FORWARD)
                            {
                                AngularVelocityLeft /= PhysicalCog.WheelRadius;
                                AngularVelocityRight = AngularVelocityRight / PhysicalCog.WheelRadius * -1;
                            }
                            else
                            {
                                AngularVelocityLeft = AngularVelocityLeft / PhysicalCog.WheelRadius * -1;
                                AngularVelocityRight /= PhysicalCog.WheelRadius;
                            }
                            VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight, CycleHandlerMilliseconds / 1000.0);
                        }
                        else if (DistanceToGoal < 0.02)
                        {
                            if (MovePurePursuit.OrderNodes[^1].Theta is { } theta)
                            {
                                TargetAngle = theta * 180 / Math.PI;
                                NavState = NavigationState.Rotating;
                                MovePurePursuit = null;
                            }
                            else
                            {
                                NavState = NavigationState.Completed;
                                Dispose();
                            }
                        }
                        else
                        {
                            double? targetAngle = null;
                            if (GoalRotate.Theta is { } theta)
                            {
                                targetAngle = theta;
                                ProcessedRotations.Add(GoalRotate.NodeId);
                                BackToAngle = GoalRotate?.ContinueTheta * 180 / Math.PI;
                                IsBackToPath = true;
                            }

                            var newGoalRotate = FindNextRotateGoal();
                            if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
                            {
                                GoalRotate = newGoalRotate;
                                UpdateGoal(newGoalRotate.NodeId);
                            }

                            if(targetAngle.HasValue)
                            {
                                TargetAngle = targetAngle.Value * 180 / Math.PI;
                                NavState = NavigationState.Rotating;
                            }
                        }
                    }
                }
            }
            else if (NavState == NavigationState.Paused) VelocityController.Stop();
        }
        catch (Exception ex)
        {
            Logger.Error($"Error in DifferentialNavigation: {ex.Message}");
        }
    }
}