using RobotApp.Common.Shares.Dtos;
using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;
using RobotApp.Services.Exceptions;
using RobotApp.Services.State;
using RobotApp.VDA5050.InstantAction;
using RobotApp.VDA5050.Order;
using RobotApp.VDA5050.State;
using System.Collections.Concurrent;
using System.Data;
using Action = RobotApp.VDA5050.InstantAction.Action;

namespace RobotApp.Services.Robot;

public class RobotOrderController(INavigation NavigationManager,
                                  ILocalization Localization,
                                  IInstantActions ActionManager,
                                  IError ErrorManager,
                                  ISafety SafetyManager,
                                  RobotStateMachine StateManager,
                                  RobotConfiguration RobotConfiguration,
                                  Logger<RobotOrderController> Logger) : IOrder
{
    public string OrderId { get; private set; } = string.Empty;
    public int OrderUpdateId { get; private set; }
    public NodeState[] NodeStates { get; private set; } = [];
    public EdgeState[] EdgeStates { get; private set; } = [];
    public string LastNodeId => LastNode is null ? "" : LastNode.NodeId;
    public int LastNodeSequenceId => LastNode is null ? 0 : LastNode.SequenceId;
    public (NodeState[], EdgeStateDto[]) CurrentPath => GetCurrentPath();

    private const int CycleHandlerMilliseconds = 200;
    private WatchTimer<RobotOrderController>? OrderTimer;

    private readonly Dictionary<string, Action[]> OrderActions = [];
    private readonly ConcurrentQueue<Action> ActionWaitingRunning = [];
    private List<Action> FinalAction = [];

    private OrderMsg? NewOrder;
    private Node[] Nodes = [];
    private VDA5050.Order.Edge[] Edges = [];
    private Node? CurrentBaseNode;
    private Node? LastNode;
    private Node[] NewOrderNodes = [];
    private VDA5050.Order.Edge[] NewOrderEdges = [];

    private readonly Lock LockObject = new();

    private bool IsCancelOrder = false;
    private bool IsActionRunning = false;
    private bool IsWaitingPaused = false;
    private bool IsNavigationFinished = false;
    private bool IsOneceNode = false;
    private Action? ActionHard = null;

    public void UpdateOrder(OrderMsg order)
    {
        lock (LockObject)
        {
            NewOrder = order;
        }
        if (OrderTimer is null) HandleOrderStart();
    }

    public void StopOrder()
    {
        if (NodeStates.Length > 0 || OrderTimer is not null) IsCancelOrder = true;
    }

    public void PauseOrder()
    {
        NavigationManager.Pause();
    }

    public void ResumeOrder()
    {
        NavigationManager.Resume();
    }

    private void HandleOrderStart()
    {
        OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
        OrderTimer.Start();
    }

    private void HandleOrderStop()
    {
        OrderTimer?.Dispose();
        OrderTimer = null;
        OrderActions.Clear();
        ActionWaitingRunning.Clear();
        CurrentBaseNode = null;
        Nodes = [];
        Edges = [];
        UpdateState();
        SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
        NavigationManager.OnNavigationFinished -= NavigationFinished;
        StateManager.TransitionTo(AutoStateType.Idle);
    }

    private Node? GetCurrentNode()
    {
        Node? inNode = null;
        double minDistance = double.MaxValue;
        foreach (var node in Nodes)
        {
            var distance = Localization.DistanceTo(node.NodePosition.X, node.NodePosition.Y);
            var nodeMin = node.NodePosition.AllowedDeviationXY == 0.0 ? 0.5 : node.NodePosition.AllowedDeviationXY;
            if (distance <= nodeMin)
            {
                if (distance < minDistance && node.NodeId != Nodes[^1].NodeId)
                {
                    minDistance = distance;
                    inNode = node;
                }
            }
        }
        return inNode;
    }

    private void NavigationFinished()
    {
        IsNavigationFinished = true;
    }

    private void OnSafetySpeedChanged(SafetySpeed safetySpeed)
    {
        double speed = 0.15;
        if (RobotConfiguration.SafetySpeedMap.TryGetValue(safetySpeed, out double safeSpeed)) speed = safeSpeed;
        NavigationManager.SetSpeed(speed);
    }

    private void UpdateState()
    {
        NodeStates = [.. Nodes.Select(n => new NodeState
                    {
                        NodeId = n.NodeId,
                        Released = n.Released,
                        SequenceId = n.SequenceId,
                        NodeDescription = n.NodeDescription,
                        NodePosition = new()
                        {
                            X = n.NodePosition.X,
                            Y = n.NodePosition.Y,
                            Theta = n.NodePosition.Theta,
                            MapId = n.NodePosition.MapId
                        }
                    })];
        EdgeStates = [.. Edges.Select(e => new EdgeState
                    {
                        EdgeId = e.EdgeId,
                        Released = e.Released,
                        EdgeDescription = e.EdgeDescription,
                        SequenceId = e.SequenceId,
                        Trajectory = e.Trajectory
                    })];
    }

    private void HandleNewOrder(OrderMsg order)
    {
        if (order.OrderId == OrderId)
        {
            if (order.OrderUpdateId < OrderUpdateId) throw new OrderException(RobotErrors.Error1018(OrderUpdateId, order.OrderUpdateId));
            if (order.OrderUpdateId == OrderUpdateId) return;
            if (order.Nodes[0].NodeId != LastNodeId)
            {
                throw new OrderException(RobotErrors.Error1016(LastNodeId, order.Nodes[0].NodeId));
            }
            if (order.Nodes[0].SequenceId != LastNodeSequenceId)
            {
                throw new OrderException(RobotErrors.Error1017(LastNodeSequenceId, order.Nodes[0].SequenceId));
            }
        }

        OrderActions.Clear();
        ActionManager.ClearInstantActions();
        LastNode = null;
        FinalAction = [];
        IsNavigationFinished = false;
        IsCancelOrder = false;
        IsActionRunning = false;
        IsWaitingPaused = false;
        ActionHard = null;

        for (int i = 0; i < order.Edges.Length; i++)
        {
            if (order.Edges[i].Trajectory is null)
            {
                order.Edges[i].Trajectory = new Trajectory()
                {
                    Degree = 1,
                    ControlPoints = [new ControlPoint() { X = 0, Y = 0 }]
                };
            }
        }

        for (int i = 0; i < order.Nodes.Length; i++)
        {
            if (order.Nodes[i].Actions is not null && order.Nodes[i].Actions.Length > 0)
            {
                foreach (var item in order.Nodes[i].Actions)
                {
                    item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}";
                }
                if (OrderActions.TryGetValue(order.Nodes[i].NodeId, out Action[]? actions) && actions is not null)
                {
                    actions = [.. actions, .. order.Nodes[i].Actions];
                }
                else OrderActions.Add(order.Nodes[i].NodeId, order.Nodes[i].Actions);
            }
            if (i < order.Nodes.Length - 1 && order.Edges[i].Actions is not null && order.Edges[i].Actions.Length > 0)
            {
                foreach (var item in order.Edges[i].Actions)
                {
                    item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}";
                }

                if (OrderActions.TryGetValue(order.Nodes[i].NodeId, out Action[]? actions) && actions is not null)
                {
                    actions = [.. actions, .. order.Edges[i].Actions];
                }
                else OrderActions.Add(order.Nodes[i].NodeId, order.Edges[i].Actions);
            }
            if (order.Nodes[i].Released) CurrentBaseNode = order.Nodes[i];
        }
        SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;

        if (OrderActions.TryGetValue(order.Nodes[^1].NodeId, out Action[]? finalactions) && finalactions is not null && finalactions.Length > 0) FinalAction = [.. finalactions];

        if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);

        if (order.Nodes.Length > 1 && order.Edges.Length >= 0)
        {
            NavigationManager.Move(order.Nodes, order.Edges);
            NavigationManager.OnNavigationFinished += NavigationFinished;
        }
        else IsOneceNode = true;
        OrderId = order.OrderId;
        OrderUpdateId = order.OrderUpdateId;
        Nodes = order.Nodes;
        Edges = order.Edges;
        NewOrderNodes = order.Nodes;
        NewOrderEdges = order.Edges;
        if (CurrentBaseNode is not null && CurrentBaseNode.NodeId != Nodes[0].NodeId && Nodes.Length > 1) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
        if (StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing);
        UpdateState();
    }

    private void ClearLastNode()
    {
        if (LastNode is null) return;
        var currentLastNodeIndex = Array.FindIndex(Nodes, n => n.NodeId == LastNode.NodeId.ToString());
        if (currentLastNodeIndex != -1 && currentLastNodeIndex < Nodes.Length - 1)
        {
            Nodes = [.. Nodes.Skip(currentLastNodeIndex + 1)];
            Edges = [.. Edges.Skip(currentLastNodeIndex + 1)];
            UpdateState();
        }
    }

    public void HandleUpdateOrder(OrderMsg order)
    {
        if (order.OrderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, order.OrderId));
        if (order.OrderUpdateId <= OrderUpdateId) return;

        if (CurrentBaseNode is not null && order.Nodes[0].NodeId != CurrentBaseNode.NodeId)
        {
            throw new OrderException(RobotErrors.Error1016(LastNodeId, order.Nodes[0].NodeId));
        }
        if (CurrentBaseNode is not null && order.Nodes[0].SequenceId != CurrentBaseNode.SequenceId)
        {
            throw new OrderException(RobotErrors.Error1017(LastNodeSequenceId, order.Nodes[0].SequenceId));
        }

        var lastBastNode = order.Nodes.Last(n => n.Released);
        if (lastBastNode is not null && lastBastNode.NodeId != CurrentBaseNode?.NodeId)
        {
            CurrentBaseNode = lastBastNode;
            NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
        }
        Nodes = order.Nodes;
        Edges = order.Edges;
        OrderUpdateId = order.OrderUpdateId;
        ClearLastNode();
    }

    private void HandleOrder()
    {
        if (Nodes.Length <= 0) return;
        if (IsCancelOrder)
        {
            NavigationManager.CancelMovement();
        }

        if (IsNavigationFinished || IsOneceNode)
        {
            if (IsCancelOrder && !ActionManager.HasActionRunning)
            {
                HandleOrderStop();
                return;
            }

            if (FinalAction.Count > 0)
            {
                var action = FinalAction[0];
                var robotAction = ActionManager[action.ActionId];
                if (robotAction is null)
                {
                    FinalAction.Remove(action);
                    return;
                }
                if (robotAction.IsCompleted)
                    if (robotAction.Status == ActionStatus.WAITING) ActionManager.StartOrderAction(action.ActionId);
            }
            else
            {
                LastNode = Nodes[^1]; // ddoanj nay phai sua lai kien tra xem co toi node cuoi that khong
                HandleOrderStop();
                return;
            }
        }

        var currentNode = GetCurrentNode();
        if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId)
        {
            LastNode = currentNode;
            ClearLastNode();

            if (OrderActions.TryGetValue(currentNode.NodeId, out Action[]? actions) && actions is not null && actions.Length > 0)
            {
                if (actions.Any(a => a.BlockingType == BlockingType.SOFT.ToString() || a.BlockingType == BlockingType.HARD.ToString()))
                {
                    NavigationManager.Pause();
                    IsWaitingPaused = true;
                }
                foreach (var action in actions)
                {
                    ActionWaitingRunning.Enqueue(action);
                }
            }
        }

        if (ActionHard is not null)
        {
            var robotAction = ActionManager[ActionHard.ActionId];
            if (robotAction is null) return;
            if (robotAction is not null && robotAction.IsCompleted) ActionHard = null;
        }
        else
        {
            if (!ActionWaitingRunning.IsEmpty)
            {
                IsActionRunning = !IsWaitingPaused || (IsWaitingPaused && NavigationManager.State == NavigationState.Paused);
                if (IsActionRunning)
                {
                    if (ActionWaitingRunning.TryDequeue(out Action? action) && action is not null)
                    {
                        var robotAction = ActionManager[action.ActionId];
                        if (robotAction is null)
                        {
                            ActionWaitingRunning.Enqueue(action);
                            return;
                        }
                        ActionManager.StartOrderAction(action.ActionId);
                        ActionHard = action.BlockingType == BlockingType.HARD.ToString() ? action : null;
                    }
                }
            }
            else
            {
                if (IsWaitingPaused)
                {
                    IsWaitingPaused = false;
                    NavigationManager.Resume();
                    if (CurrentBaseNode is not null && CurrentBaseNode.NodeId != Nodes[0].NodeId && Nodes.Length > 1) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
                }
            }
        }
    }

    private void OrderHandler()
    {
        try
        {
            if (NewOrder is not null)
            {
                OrderMsg NewOrderHandler;
                lock (LockObject)
                {
                    NewOrderHandler = NewOrder;
                    NewOrder = null;
                }

                if (NewOrderHandler.Nodes.Length < 1) throw new OrderException(RobotErrors.Error1002(NewOrderHandler.Nodes.Length));
                if (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length));

                if (NodeStates.Length != 0 || EdgeStates.Length != 0) HandleUpdateOrder(NewOrderHandler);
                else
                {
                    Node startNode = NewOrderHandler.Nodes[0];
                    var nodeDeviation = startNode.NodePosition.AllowedDeviationXY == 0.0 ? NewOrderHandler.Nodes.Length == 1 ? 0.3 : 0.5 : startNode.NodePosition.AllowedDeviationXY;
                    var distance = Localization.DistanceTo(startNode.NodePosition.X, startNode.NodePosition.Y);
                    if (distance > nodeDeviation) throw new OrderException(RobotErrors.Error1019());

                    if (NewOrderHandler.Nodes.Length > 1)
                    {
                        Node endNode = NewOrderHandler.Nodes[^1];
                        nodeDeviation = endNode.NodePosition.AllowedDeviationXY == 0.0 ? 0.2 : endNode.NodePosition.AllowedDeviationXY;
                        distance = Localization.DistanceTo(endNode.NodePosition.X, endNode.NodePosition.Y);
                        if (distance < nodeDeviation) throw new OrderException(RobotErrors.Error1020());
                    }
                    HandleNewOrder(NewOrderHandler);
                }
            }
            HandleOrder();
        }
        catch (RobotExeption orEx)
        {
            if (orEx.Error is not null)
            {
                ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
                Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}");
                if (Nodes.Length == 0) HandleOrderStop();
            }
            else Logger.Warning($"Lỗi khi xử lí Order: {orEx.Message}");
        }
        catch (Exception ex)
        {
            Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}");
        }
    }

    private EdgeStateDto[] SplitChecking(Node lastNode, Node nearLastNode, VDA5050.Order.Edge edge)
    {
        List<EdgeStateDto> pathEdges = [];
        var splitStartPath = RobotPathPlanner.PathSplit([lastNode, nearLastNode], [edge], 0.5);
        if (splitStartPath is not null && splitStartPath.Length > 0)
        {
            int index = 0;
            double minDistance = double.MaxValue;
            for (int i = 0; i < splitStartPath.Length; i++)
            {
                var distance = Math.Sqrt(Math.Pow(Localization.X - splitStartPath[i].NodePosition.X, 2) +
                                         Math.Pow(Localization.Y - splitStartPath[i].NodePosition.Y, 2));
                if (distance < minDistance)
                {
                    minDistance = distance;
                    index = i;
                }
            }
            if (edge.Trajectory is null || edge.Trajectory.Degree == 1)
            {
                pathEdges.Add(new()
                {
                    StartX = splitStartPath[index].NodePosition.X,
                    StartY = splitStartPath[index].NodePosition.Y,
                    EndX = nearLastNode.NodePosition.X,
                    EndY = nearLastNode.NodePosition.Y,
                    Degree = 1,
                });
            }
            else
            {
                for (int i = index; i < splitStartPath.Length - 1; i++)
                {
                    pathEdges.Add(new()
                    {
                        StartX = splitStartPath[i].NodePosition.X,
                        StartY = splitStartPath[i].NodePosition.Y,
                        EndX = splitStartPath[i + 1].NodePosition.X,
                        EndY = splitStartPath[i + 1].NodePosition.Y,
                        Degree = 1,
                    });
                }
            }
        }
        return [.. pathEdges];
    }

    private (NodeState[], EdgeStateDto[]) GetCurrentPath()
    {
        if (NodeStates.Length == 0 && EdgeStates.Length == 0) return ([], []);

        List<EdgeStateDto> pathEdges = [];
        var lastNodeIndex = Array.FindIndex(NewOrderNodes, n => n.NodeId == LastNodeId);
        lastNodeIndex = lastNodeIndex != -1 ? lastNodeIndex : 0;
        if (lastNodeIndex < NewOrderNodes.Length - 1) pathEdges = [.. SplitChecking(NewOrderNodes[lastNodeIndex], NewOrderNodes[lastNodeIndex + 1], NewOrderEdges[lastNodeIndex])];
        if (lastNodeIndex < NewOrderNodes.Length - 2)
        {
            var nodes = NewOrderNodes.ToList().GetRange(lastNodeIndex + 1, NewOrderNodes.Length - lastNodeIndex - 1);
            var edges = NewOrderEdges.ToList().GetRange(lastNodeIndex + 1, nodes.Count - 1);
            for (int i = 0; i < nodes.Count - 1; i++)
            {
                if (edges[i] is null) return (NodeStates, [.. pathEdges]);

                var trajectory = edges[i].Trajectory;
                var controlPoints = trajectory?.ControlPoints;


                pathEdges.Add(new()
                {
                    StartX = nodes[i].NodePosition.X,
                    StartY = nodes[i].NodePosition.Y,
                    EndX = nodes[i + 1].NodePosition.X,
                    EndY = nodes[i + 1].NodePosition.Y,
                    ControlPoint1X = controlPoints is { Length: > 2 } ? controlPoints[1].X : 0,
                    ControlPoint1Y = controlPoints is { Length: > 2 } ? controlPoints[1].Y : 0,
                    ControlPoint2X = controlPoints is { Length: > 3 } ? controlPoints[2].X : 0,
                    ControlPoint2Y = controlPoints is { Length: > 3 } ? controlPoints[2].Y : 0,
                    Degree = trajectory is null ? 1 : trajectory.Degree,
                });
            }
        }
        return (NodeStates, [.. pathEdges]);
    }

}