using RobotNet.VDA5050.Order;
using RobotNet10.Common;
using RobotNet10.RobotApp.Detection;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.ConfigManager;
using RobotNet10.RobotApp.Services.Robot;
using RobotNet10.RobotApp.Services.Robot.Models;
using RobotNet10.RobotApp.Services.Simulation.Algorithm;
using RobotNet10.RobotApp.Shared.Enums;

namespace RobotNet10.RobotApp.Services.Simulation;

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<NavigationState>? OnNavigationFinished;

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

    protected const int CycleHandlerMilliseconds = 50;
    private WatchThread<SimulationNavigation>? NavigationTimer;

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

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

    protected readonly SimulationVisualization Visualization;
    protected readonly SimulationVelocity VelocityController;
    protected readonly IRobotConfiguration RobotConfiguration;

    protected OrderNode? GoalRotate;
    protected OrderNode? CurrentBaseNode;
    protected HashSet<string> ProcessedRotations = [];
    protected NavigationState ResumeState = NavigationState.Idle;

    protected SimulationConfig SimCog;
    protected RobotPhysicalConfig PhysicalCog;
    protected double MaxVelocity;

    private readonly ILogger<SimulationNavigation> Logger;

    public SimulationNavigation(IServiceProvider ServiceProvider)
    {
        using var scope = ServiceProvider.CreateScope();
        Logger = scope.ServiceProvider.GetRequiredService<ILogger<SimulationNavigation>>();
        Visualization = scope.ServiceProvider.GetRequiredService<SimulationVisualization>();
        RobotConfiguration = scope.ServiceProvider.GetRequiredService<IRobotConfiguration>();
        SimCog = RobotConfiguration.GetSimulationConfig();
        MaxVelocity = SimCog.MaxVelocity;
        PhysicalCog = RobotConfiguration.GetRobotPhysicalConfig();
        VelocityController = new(Visualization, SimCog);
        Visualization.SetPhysical(PhysicalCog.WheelRadius, PhysicalCog.Width);
        AngularVelocity = SimCog.MaxAngularVelocity * PhysicalCog.Width / 2 / 2 / PhysicalCog.WheelRadius;
    }

    protected void HandleNavigationStart()
    {
        SimCog = RobotConfiguration.GetSimulationConfig();
        PhysicalCog = RobotConfiguration.GetRobotPhysicalConfig();
        Visualization.SetPhysical(PhysicalCog.WheelRadius, PhysicalCog.Width);
        NavigationTimer = new(CycleHandlerMilliseconds, NavigationHandler, Logger);
        NavigationTimer.Start();
    }

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

    protected virtual void NavigationHandler() { }

    public void SafetyStop()
    {
        NavState = NavigationState.SafetyStop;
    }

    public void CancelMovement()
    {
        NavState = NavigationState.Canceled;
        Dispose();
    }

    public void Move(RobotNet.VDA5050.Order.OrderMsg order, bool hasLoad = false)
    {
        var nodes = order.Nodes;
        var edges = order.Edges;
        NavState = NavigationState.Initializing;

        MovePID = new PID().WithKp(1).WithKi(0.0001).WithKd(0.6);
        MoveFuzzy = new FuzzyLogic();
        MovePurePursuit = new PurePursuit()
            .WithLookheadDistance(0.35)
            .WithPath(nodes, edges, Visualization.Theta * Math.PI / 180);

        (NavigationNode node, int index) = MovePurePursuit.GetOnNavNode(Visualization.X, Visualization.Y);
        if (index >= MovePurePursuit.Waypoints_Value.Count - 1) return;

        double angleFoward = Math.Atan2(MovePurePursuit.Waypoints_Value[index + 1].Y - node.Y, MovePurePursuit.Waypoints_Value[index + 1].X - node.X) * 180 / Math.PI;
        double angleBacward = Math.Atan2(node.Y - MovePurePursuit.Waypoints_Value[index + 1].Y, node.X - MovePurePursuit.Waypoints_Value[index + 1].X) * 180 / Math.PI;
        Rotate(node.Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
    }

    public void MoveStraight(double x, double y, bool hasLoad = false, RobotDirection? direction = null)
    {
        //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 = SpaceCompute.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;

        //MovePID = new PID().WithKp(1.5).WithKi(0.0001).WithKd(0.8);
        //MoveFuzzy = new FuzzyLogic();
        //MovePurePursuit = new PurePursuit()
        //    .WithLookheadDistance(0.25)
        //    .WithPath([currentRobotNode, goalNode], [new Edge()
        //{
        //    EdgeId = "Straight edge",
        //    Trajectory = new Trajectory()
        //    {
        //        Degree = 1,
        //        ControlPoints = []
        //    },
        //    StartNodeId = currentRobotNode.NodeId,
        //    EndNodeId = goalNode.NodeId,
        //}], Visualization.Theta);

        //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 DockTo(IDetectSession session, bool hasLoad = false, RobotDirection? direction = null)
    {
    }

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

    public void Resume()
    {
        NavState = ResumeState;
    }

    public void Rotate(double angle)
    {
        RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1);
        TargetAngle = SpaceCompute.NormalizeDegreeAngle(angle);
        NavState = NavigationState.Rotating;
        HandleNavigationStart();
    }

    protected void UpdateGoal(string goalId)
    {
        MovePurePursuit?.UpdateGoal(goalId);
    }

    public void UpdateOrder(string newBaseNodeId)
    {
        var newBaseNode = MovePurePursuit?.OrderNodes.FirstOrDefault(n => n.NodeId == newBaseNodeId);
        if (newBaseNode is not null && newBaseNode.NodeId != CurrentBaseNode?.NodeId)
        {
            CurrentBaseNode = newBaseNode;
            var newGoalRotate = FindNextRotateGoal();
            if (newGoalRotate is not null && newGoalRotate.NodeId != GoalRotate?.NodeId)
            {
                GoalRotate = newGoalRotate;
                UpdateGoal(newGoalRotate.NodeId);
            }
        }
    }

    public void RefreshOrder(Node[] nodes, Edge[] edges)
    {
        return;
    }

    public void SetSpeed(double speed)
    {
        MaxVelocity = speed;
    }

    public void Dispose()
    {
        HandleNavigationStop();
        VelocityController.Stop();
        CurrentBaseNode = null;
        MovePurePursuit = null;
        MovePID = null;
        RotatePID = null;
        NavDriving = false;
        OnNavigationFinished?.Invoke(NavState);
        NavState = NavigationState.Idle;
        GC.SuppressFinalize(this);
    }

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

    public void Start()
    {
        return;
    }

    public void Stop()
    {
        Dispose();
    }

    /// <summary>
    /// Tìm node có IsWaitingRotate đầu tiên trong path từ currentNode đến currentGoal
    /// </summary>
    protected OrderNode? FindNextRotateGoal()
    {
        if (CurrentBaseNode == null || MovePurePursuit is null || MovePurePursuit.OrderNodes.Length == 0) return null;

        int goalIndex = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == CurrentBaseNode.NodeId);
        if (goalIndex == -1) return null;

        int lastNodeIdx = Array.FindIndex(MovePurePursuit.OrderNodes, n => n.NodeId == GoalRotate?.NodeId);
        lastNodeIdx = lastNodeIdx == -1 ? 0 : lastNodeIdx + 1;

        // Tìm từ node hiện tại đến goal
        for (int i = lastNodeIdx; i <= goalIndex; i++)
        {
            var node = MovePurePursuit.OrderNodes[i];

            // Tìm node có IsWaitingRotate và chưa xử lý
            if (node.IsWaitRotating && !ProcessedRotations.Contains(node.NodeId))
            {
                return node;
            }
        }
        return CurrentBaseNode;
    }
}