using RobotNet.VDA5050.Order;
using RobotNet.VDA5050.State;
using RobotNet.VDA5050.Type;
using RobotNet10.Common;
using RobotNet10.RobotApp.Client.Pages;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.RobotApp.Services.ConfigManager;
using RobotNet10.RobotApp.Services.Exceptions;
using RobotNet10.RobotApp.Services.State;
using System.Collections.Concurrent;
using System.Data;
using Action = RobotNet.VDA5050.InstantAction.Action;

namespace RobotNet10.RobotApp.Services.Robot;

public class RobotOrderController(INavigation NavigationManager,
                                  ILocalization Localization,
                                  IAction ActionManager,
                                  IError ErrorManager,
                                  IPlcController SafetyManager,
                                  RobotStateMachine StateManager,
                                  INavigationConfig NavigationConfig,
                                  ILogger<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 bool NewBaseRequest { get; private set; }
    public double DistanceSinceLastNode { get; private set; }
    public bool IsPaused { get; private set; } = false;

    private const int CycleHandlerMilliseconds = 100;
    private WatchThread<RobotOrderController>? OrderTimer;

    private readonly Dictionary<string, Action[]> OrderActions = []; // Node actions keyed by NodeId
    private readonly ConcurrentQueue<Action> ActionWaitingRunning = [];

    private OrderMsg? NewOrder;
    private OrderMsg? _currentActiveOrder;
    private Node[] Nodes = [];
    private Edge[] Edges = [];
    private Node? CurrentBaseNode;
    private Node? LastNode;
    private Edge? CurrentEdge; // Track current edge for EDGE action lifecycle
    private readonly ConcurrentBag<string> RunningEdgeActionIds = []; // Track running EDGE action IDs (thread-safe)

    private readonly Lock LockObject = new();

    private bool IsCancelOrder = false;
    private bool IsCancelSentToNavigation = false;
    private bool IsActionRunning = false;
    private bool IsWaitingPaused = false;
    private bool IsNavigationFinished = false;
    private bool HasNewOrder = false;
    private Action? ActionHard = null;
    private NavigationState NavState = NavigationState.None;

    private double SafetySpeed = 0.0;
    private double EdgeSpeed = 0.0;
    private double CurrentSpeed = 0.0;
    private Navigation.NavigationConfig? CachedNavConfig = null;

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

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

    public void PauseOrder()
    {
        IsPaused = true;
        NavigationManager.Pause();
        ActionManager.PauseActions();
    }

    public void ResumeOrder()
    {
        IsPaused = false;
        NavigationManager.Resume();
        ActionManager.ResumeActions();
    }

    private void HandleOrderStart()
    {
        // Console.WriteLine("HandleOrderStart called");
        OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
        OrderTimer.Start();
    }

    private void HandleOrderStop()
    {
        OrderTimer?.Dispose();
        OrderTimer = null;
        OrderActions.Clear();
        ActionWaitingRunning.Clear();
        ActionManager.StopOrderAction(); // Stop all running order actions
        // Reset state flags
        IsCancelOrder = false;
        IsCancelSentToNavigation = false;
        IsNavigationFinished = false;
        IsActionRunning = false;
        IsWaitingPaused = false;
        IsPaused = false;
        ActionHard = null;
        CurrentBaseNode = null;
        Nodes = [];
        Edges = [];
        _currentActiveOrder = null;
        // Reset EDGE action tracking
        CurrentEdge = null;
        RunningEdgeActionIds.Clear();
        // Reset speed tracking
        SafetySpeed = 0.0;
        EdgeSpeed = 0.0;
        CurrentSpeed = 0.0;
        CachedNavConfig = null;
        UpdateState();
        SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
        NavigationManager.OnNavigationFinished -= NavigationFinished;
        StateManager.Fire(RobotEventType.CompleteExecution);
    }

    /// <summary>
    /// VDA5050 Compliance: Normalize angle to range [-π, π]
    /// </summary>
    private static double NormalizeAngle(double angle)
    {
        while (angle > Math.PI) angle -= 2 * Math.PI;
        while (angle < -Math.PI) angle += 2 * Math.PI;
        return angle;
    }

    private Node? GetCurrentNode()
    {
        Node? inNode = null;
        double minDistance = double.MaxValue;
        foreach (var node in Nodes)
        {
            var distance = Localization.DistanceTo(node.NodePosition?.X ?? 0, node.NodePosition?.Y ?? 0);
            var nodeMin = node.NodePosition?.AllowedDeviationXY == 0.0 ? 0.5 : node.NodePosition?.AllowedDeviationXY ?? 0.3;

            bool positionMatch = distance <= nodeMin;
            bool orientationMatch = true;

            // VDA5050 Compliance: Check theta if specified
            if (node.NodePosition?.Theta is not null)
            {
                var currentTheta = Localization.Theta;
                var targetTheta = node.NodePosition.Theta.Value;
                var allowedThetaDev = node.NodePosition.AllowedDeviationTheta ?? Math.PI; // Default: any orientation

                var thetaDiff = Math.Abs(NormalizeAngle(currentTheta - targetTheta));
                orientationMatch = thetaDiff <= allowedThetaDev;
            }

            if (positionMatch && orientationMatch)
            {
                // Exclude last node - it's handled separately in HandleOrder() after navigation completes (lines 392-399)
                // This ensures intermediate node actions are processed during navigation
                if (distance < minDistance && node.NodeId != Nodes[^1].NodeId)
                {
                    minDistance = distance;
                    inNode = node;
                }
            }
        }
        return inNode;
    }

    private void NavigationFinished(NavigationState state)
    {
        NavState = state;
        IsNavigationFinished = true;
    }

    /// <summary>
    /// Xử lý sự kiện thay đổi safety speed từ PLC
    /// </summary>
    private void OnSafetySpeedChanged(SafetySpeed safetySpeed)
    {
        if (TryGetSafetySpeedFromPlcSignal(safetySpeed, out double safeSpeed))
        {
            // Nếu safetySpeed là Very_Fast (giải phóng hoàn toàn), trả về logic speed theo edge/config.
            if (safetySpeed == Interfaces.SafetySpeed.Very_Fast)
            {
                SafetySpeed = 1.5; // Đặt một giá trị cao để không giới hạn tốc độ, sẽ được điều chỉnh bởi logic edge/config trong UpdateNavigationSpeed
                UpdateNavigationSpeed();
                Logger.LogInformation("SafetySpeed released: robot speed now follows edge/config limit");
            }
            else
            {
                SafetySpeed = safeSpeed;
                UpdateNavigationSpeed();
            }
        }
        else
        {
            Logger.LogWarning("Cannot map PLC SafetySpeed {SafetySpeed} to navigation speed value", safetySpeed);
        }
    }

    private bool TryGetSafetySpeedFromPlcSignal(SafetySpeed safetySpeed, out double speed)
    {
        // PLC IO tốc độ: 825/2873, 826/2874, 827/2875, 828/2876.
        // Ưu tiên map cứng theo yêu cầu vận hành để không phụ thuộc config.
        switch (safetySpeed)
        {
            case Interfaces.SafetySpeed.Very_Slow:
                speed = 0.001;
                return true;
            case Interfaces.SafetySpeed.Slow:
                speed = 0.15;
                return true;
            case Interfaces.SafetySpeed.Normal:
                speed = 0.3;
                return true;
            case Interfaces.SafetySpeed.Medium:
                speed = 0.6;
                return true;
            case Interfaces.SafetySpeed.Very_Fast:
                speed = 1.5;
                return true;
        }

        // Các mức còn lại vẫn theo cấu hình hiện tại.
        CachedNavConfig ??= NavigationConfig.GetNavigationConfig();
        if (CachedNavConfig.SafetySpeedMap.TryGetValue(safetySpeed, out double configSpeed))
        {
            speed = configSpeed;
            return true;
        }

        speed = 0.0;
        return false;
    }

    /// <summary>
    /// VDA5050 Compliance: Tính toán và áp dụng tốc độ giới hạn cho navigation
    /// Kết hợp 3 nguồn tốc độ: Edge, Safety, Config Max
    /// Sử dụng giá trị MIN để đảm bảo an toàn
    /// </summary>
    private void UpdateNavigationSpeed()
    {
        // Sử dụng cached config để tránh load lại
        // CachedNavConfig ??= NavigationConfig.GetNavigationConfig();
        // double maxConfigSpeed = CachedNavConfig.MaxLinearVelocity;

        // // Bắt đầu với tốc độ max từ config
        // double targetSpeed = maxConfigSpeed;

        // // Áp dụng giới hạn Edge (nếu có)
        // // EdgeSpeed = 0 nghĩa là không có giới hạn từ edge
        // if (EdgeSpeed > 0)
        // {
        //     targetSpeed = Math.Min(targetSpeed, EdgeSpeed);
        // }

        // // Áp dụng giới hạn Safety (nếu có)
        // // SafetySpeed = 0 nghĩa là chưa nhận được safety speed
        // if (SafetySpeed > 0)
        // {
        //     targetSpeed = Math.Min(targetSpeed, SafetySpeed);
        // }

        // // Chỉ cập nhật nếu thay đổi đáng kể (tránh update liên tục)
        // if (Math.Abs(CurrentSpeed - targetSpeed) > 0.001)
        // {
        //     CurrentSpeed = targetSpeed;
        //     NavigationManager.SetSpeed(CurrentSpeed);

        //     Logger.LogInformation(
        //         "Speed updated: {CurrentSpeed:F2} m/s [Edge: {EdgeSpeed:F2}, Safety: {SafetySpeed:F2}, Max: {MaxSpeed:F2}]",
        //         CurrentSpeed, EdgeSpeed > 0 ? EdgeSpeed : maxConfigSpeed, SafetySpeed > 0 ? SafetySpeed : maxConfigSpeed, maxConfigSpeed
        //     );
        // }

        double targetSpeed = 0;
        if (EdgeSpeed > 0 && SafetySpeed > 0)
        {
            targetSpeed = Math.Min(EdgeSpeed, SafetySpeed);
        }
        else if (EdgeSpeed > 0)
        {
            targetSpeed = EdgeSpeed;
        }
        else if (SafetySpeed > 0)
        {
            targetSpeed = SafetySpeed;
        }
        else
        {
            // Nếu cả hai đều không có giới hạn, có thể đặt về một giá trị mặc định hoặc để robot tự quyết định
            targetSpeed = 0.0; // Không giới hạn
        }
        Console.WriteLine($"Updating navigation speed: EdgeSpeed={EdgeSpeed}, SafetySpeed={SafetySpeed}, TargetSpeed={targetSpeed}");
        NavigationManager.SetSpeed(targetSpeed);
    }

    private void UpdateState()
    {
        NodeStates = [.. Nodes.Select(n => new NodeState
                    {
                        NodeId = n.NodeId,
                        Released = n.Released,
                        SequenceId = n.SequenceId,
                        NodeDescription = n.NodeDescription,
                        NodePosition = n.NodePosition is null ? null : 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 async Task ClearOldOrder()
    {
        OrderActions.Clear();
        await ActionManager.ClearActions();
        IsNavigationFinished = false;
        IsCancelOrder = false;
        IsActionRunning = false;
        IsWaitingPaused = false;
        ActionHard = null;
    }

    private void AddAction(Action[] actions, Node node)
    {
        foreach (var item in actions)
        {
            item.ActionDescription += $".On Node: {(string.IsNullOrEmpty(node.NodeDescription) ? node.NodeId : node.NodeDescription)}";
        }
        if (OrderActions.TryGetValue(node.NodeId, out Action[]? oldActions) && oldActions is not null)
        {
            OrderActions[node.NodeId] = [.. oldActions, .. actions];
        }
        else OrderActions.Add(node.NodeId, actions);
    }

    private void AddEdgeAction(Action[] actions, Edge edge)
    {
        foreach (var item in actions)
        {
            item.ActionDescription += $".On Edge: {(string.IsNullOrEmpty(edge.EdgeDescription) ? edge.EdgeId : edge.EdgeDescription)}";
        }
        if (OrderActions.TryGetValue(edge.EdgeId, out Action[]? oldActions) && oldActions is not null)
        {
            OrderActions[edge.EdgeId] = [.. oldActions, .. actions];
        }
        else OrderActions.Add(edge.EdgeId, actions);
    }

    private void ValidateNodes(Node[] nodes, int currentSequence)
    {
        for (int i = 0; i < nodes.Length; i++)
        {
            int correctSequence = i * 2 + currentSequence;
            if (nodes[i].SequenceId != correctSequence) throw new OrderException(RobotErrors.Error1012(nodes[i].NodeId, nodes[i].SequenceId, correctSequence));
            if (nodes[i].NodePosition is null) throw new OrderException(RobotErrors.Error1015(nodes[i].NodeId));
            if (i == 0)
            {
                if (nodes[i].Released)
                {
                    if (nodes[i].Actions != null && nodes[i].Actions.Length > 0) AddAction(nodes[i].Actions, nodes[i]);
                }
            }
        }
    }

    private static void ValidateTrajectory(Edge edge, Node startNode, Node endNode)
    {
        // VDA5050 Compliance: Validate NURBS trajectory structure
        if (edge.Trajectory is not null)
        {
            var traj = edge.Trajectory;

            // Validate controlPoints count (minimum 2: start and end)
            if (traj.ControlPoints is null || traj.ControlPoints.Length < 2)
            {
                throw new OrderException(RobotErrors.Error1020(edge.EdgeId));
            }

            // Validate knotVector size: must equal controlPoints.Length + degree + 1
            if (traj.KnotVector is not null)
            {
                int expectedSize = traj.ControlPoints.Length + traj.Degree + 1;
                if (traj.KnotVector.Length != expectedSize)
                {
                    throw new OrderException(RobotErrors.Error1018(edge.EdgeId));
                }

                // Validate knotVector is monotonically increasing from 0 to 1
                for (int j = 0; j < traj.KnotVector.Length; j++)
                {
                    if (traj.KnotVector[j] < 0 || traj.KnotVector[j] > 1)
                    {
                        throw new OrderException(RobotErrors.Error1019(edge.EdgeId));
                    }
                    if (j > 0 && traj.KnotVector[j] < traj.KnotVector[j - 1])
                    {
                        throw new OrderException(RobotErrors.Error1019(edge.EdgeId));
                    }
                }
            }
        }
        else
        {
            // VDA5050 Compliance: Create valid default linear trajectory
            edge.Trajectory = new Trajectory()
            {
                Degree = 1,
                ControlPoints =
                [
                    new ControlPoint()
                    {
                        X = startNode.NodePosition?.X ?? 0,
                        Y = startNode.NodePosition?.Y ?? 0,
                        Weight = 1.0
                    },
                    new ControlPoint()
                    {
                        X = endNode.NodePosition?.X ?? 0,
                        Y = endNode.NodePosition?.Y ?? 0,
                        Weight = 1.0
                    }
                ],
                KnotVector = [0, 0, 1, 1]
            };
        }
    }

    private void ValidateEdges(Edge[] edges, Node[] nodes, int currentSequence)
    {
        for (int i = 0; i < edges.Length; i++)
        {
            var startNode = nodes.FirstOrDefault(n => n.NodeId == edges[i].StartNodeId) ??
                throw new OrderException(RobotErrors.Error1008(edges[i].EdgeId, edges[i].StartNodeId));
            var endNode = nodes.FirstOrDefault(n => n.NodeId == edges[i].EndNodeId) ??
                throw new OrderException(RobotErrors.Error1009(edges[i].EdgeId, edges[i].StartNodeId));

            int correctSequence = i * 2 + 1 + currentSequence;
            if (edges[i].SequenceId != correctSequence) throw new OrderException(RobotErrors.Error1013(edges[i].EdgeId, edges[i].SequenceId, correctSequence));

            // VDA5050 Compliance: Validate or create proper trajectory
            ValidateTrajectory(edges[i], startNode, endNode);

            if (edges[i].Released)
            {
                if (endNode.Released)
                {
                    CurrentBaseNode = endNode;
                    if (endNode.Actions != null && endNode.Actions.Length > 0) AddAction(endNode.Actions, endNode);
                    if (edges[i].Actions != null && edges[i].Actions.Length > 0) AddEdgeAction(edges[i].Actions, edges[i]);
                }
            }
        }
    }

    private async Task HandleNewOrder(OrderMsg order)
    {
        if (order.OrderId == OrderId)
        {
            if (order.OrderUpdateId < OrderUpdateId) throw new OrderException(RobotErrors.Error1003(OrderUpdateId, order.OrderUpdateId));
            if (order.OrderUpdateId == OrderUpdateId) return;
            if (order.Nodes[0].NodeId != LastNodeId)
            {
                throw new OrderException(RobotErrors.Error1010(LastNodeId, order.Nodes[0].NodeId));
            }
            if (order.Nodes[0].SequenceId != LastNodeSequenceId)
            {
                throw new OrderException(RobotErrors.Error1011(LastNodeSequenceId, order.Nodes[0].SequenceId));
            }
        }

        // xử lí order mới
        // Validate Nodes, Edges
        await ClearOldOrder();
        ValidateNodes(order.Nodes, order.OrderId == OrderId ? LastNodeSequenceId : 0);
        ValidateEdges(order.Edges, order.Nodes, order.OrderId == OrderId ? LastNodeSequenceId : 0);

        // Add actions to ActionManager with correct scope
        if (OrderActions.Count > 0)
        {
            foreach (var actions in OrderActions)
            {
                ActionManager.AddOrderActions(actions.Value, order.Edges.Any(e => e.EdgeId == actions.Key) ? ActionScope.EDGE : ActionScope.NODE);
            }
        }

        if (order.Nodes.Length <= 1 || order.Edges.Length == 0)
        {
            if (order.Nodes.Length == 1 && order.Nodes[0].Actions.Length == 0) return;
            NavigationFinished(NavigationState.Completed);
        }

        OrderId = order.OrderId;
        OrderUpdateId = order.OrderUpdateId;
        Nodes = order.Nodes;
        Edges = order.Edges;
        _currentActiveOrder = order;
        ErrorManager.DeleteErrorType(ErrorType.VALIDATION_ERROR.ToString());
        ErrorManager.DeleteErrorType(ErrorType.ORDER_ERROR.ToString());
        ErrorManager.DeleteErrorType(ErrorType.ORDER_UPDATE_ERROR.ToString());
        UpdateState();
        HasNewOrder = true;
    }

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

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

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

        IsNavigationFinished = false;

        Node[] baseNodes = CurrentBaseNode is null ? [] : [.. Nodes.TakeWhile(n => n != CurrentBaseNode).Append(CurrentBaseNode)];
        Edge[] baseEdges = CurrentBaseNode is null ? [] : [.. Edges.ToList().GetRange(0, baseNodes.Length - 1)];

        ValidateNodes(order.Nodes, baseNodes.Length > 0 ? baseNodes[^1].SequenceId : 0);
        ValidateEdges(order.Edges, order.Nodes, baseNodes.Length > 0 ? baseNodes[^1].SequenceId : 0);

        if (OrderActions.Count > 0)
        {
            foreach(var actions in OrderActions)
            {
                ActionManager.AddOrderActions(actions.Value, order.Edges.Any(e => e.EdgeId == actions.Key) ? ActionScope.EDGE : ActionScope.NODE);
            }
        }

        OrderUpdateId = order.OrderUpdateId;
        Nodes = [.. baseNodes, .. order.Nodes.Skip(1)];
        Edges = [.. baseEdges, .. order.Edges];
        _currentActiveOrder = new OrderMsg
        {
            HeaderId = order.HeaderId,
            Timestamp = order.Timestamp,
            Version = order.Version,
            Manufacturer = order.Manufacturer,
            SerialNumber = order.SerialNumber,
            OrderId = order.OrderId,
            OrderUpdateId = order.OrderUpdateId,
            ZoneSetId = order.ZoneSetId,
            Nodes = Nodes,
            Edges = Edges
        };

        ErrorManager.DeleteErrorType(ErrorType.VALIDATION_ERROR.ToString());
        ErrorManager.DeleteErrorType(ErrorType.ORDER_ERROR.ToString());
        ErrorManager.DeleteErrorType(ErrorType.ORDER_UPDATE_ERROR.ToString());
        UpdateState();
    }

    private void StartActionTerminal(Node node)
    {
        var action = node.Actions[0];
        var robotAction = ActionManager[action.ActionId];
        if (robotAction is null)
        {
            if (!ActionManager.HasActionWaitting && node.Actions.Length > 0) node.Actions = [.. node.Actions.Skip(1)];
            return;
        }
        if (robotAction.IsCompleted) node.Actions = [.. node.Actions.Skip(1)];
        if (robotAction.Status == ActionStatus.WAITING) ActionManager.StartOrderAction(action.ActionId);
    }

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

        if (HasNewOrder)
        {
            if (ActionManager.HasActionWaitting) return;
            if (Nodes.Length > 1 && Edges.Length >= 0)
            {
                if (Nodes[0].Actions.Length > 0)
                {
                    // VDA5050: Check if robot is on Node[0] before triggering actions
                    var startNode = Nodes[0];
                    var nodeDeviation = startNode.NodePosition?.AllowedDeviationXY ?? 0.5;
                    if (nodeDeviation == 0.0) nodeDeviation = 0.5;
                    var distance = Localization.DistanceTo(startNode.NodePosition?.X ?? 0, startNode.NodePosition?.Y ?? 0);

                    if (distance <= nodeDeviation)
                    {
                        // VDA5050: Separate NONE from blocking actions on Node[0]
                        var noneActions = startNode.Actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
                        var blockingActions = startNode.Actions.Where(a => a.BlockingType != BlockingType.NONE).ToArray();

                        // Start NONE actions immediately - they must not delay navigation
                        foreach (var action in noneActions)
                        {
                            ActionManager.StartOrderAction(action.ActionId);
                        }

                        startNode.Actions = blockingActions;

                        if (blockingActions.Length > 0)
                        {
                            // Robot is on Node[0] - trigger blocking actions sequentially
                            StartActionTerminal(Nodes[0]);
                            return;
                        }
                        // All were NONE → fall through to start navigation
                    }
                    // else: Robot not on Node[0] - let navigation start, actions will trigger when node is traversed
                }
                else
                // Start navigation (no blocking actions on Node[0], or Node[0] has no actions)
                {
                    IsCancelSentToNavigation = false;
                    NavigationManager.OnNavigationFinished += NavigationFinished;
                    SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;

                    // Cache NavigationConfig để tránh load lại mỗi lần speed change
                    CachedNavConfig = NavigationConfig.GetNavigationConfig();

                    // VDA5050: Đọc initial safety speed khi bắt đầu navigation
                    var currentSafetySpeed = SafetyManager.SafetySpeed;
                    if (TryGetSafetySpeedFromPlcSignal(currentSafetySpeed, out double safeSpeed))
                    {
                        SafetySpeed = safeSpeed;
                        Logger.LogInformation("Initial safety speed: {SafetySpeed:F2} m/s (level: {Level})", SafetySpeed, currentSafetySpeed);
                    }

                    // VDA5050: Set initial edge speed (edge đầu tiên)
                    if (Edges.Length > 0 && Edges[0].MaxSpeed.HasValue && Edges[0].MaxSpeed is double speed)
                    {
                        EdgeSpeed = speed;
                        Logger.LogInformation("Initial edge speed: {EdgeSpeed:F2} m/s (edge: {EdgeId})", EdgeSpeed, Edges[0].EdgeId);
                    }
                    else
                    {
                        EdgeSpeed = 0.0; // Không giới hạn
                    }

                    // VDA5050: Áp dụng tốc độ ban đầu trước khi bắt đầu navigation
                    UpdateNavigationSpeed();

                    // chỗ này có thể sẽ phải sửa lại theo interface của a Hiệp
                    NavigationManager.Move(_currentActiveOrder!, SafetyManager.SetHasLoadValue);
                    if (CurrentBaseNode is not null
                        && CurrentBaseNode.NodeId != Nodes[0].NodeId
                        && CurrentBaseNode.NodeId != Nodes[^1].NodeId
                        && Nodes.Length > 1)
                    {
                        NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
                    }
                    if (StateManager.CurrentState != RobotStateType.Executing) StateManager.Fire(RobotEventType.StartExecution);
                    if(OrderActions.ContainsKey(Nodes[0].NodeId)) OrderActions.Remove(Nodes[0].NodeId);
                    HasNewOrder = false;
                }
            }
        }

        if (IsCancelOrder && !IsCancelSentToNavigation)
        {
            NavigationManager.CancelMovement();
            IsCancelSentToNavigation = true;
        }

        if (IsNavigationFinished)
        {
            if (IsCancelOrder && !ActionManager.HasActionRunning)
            {
                HandleOrderStop();
                Logger.LogInformation("Order {OrderId} is canceled", OrderId);
            }
            else if (NavState == NavigationState.Completed)
            {
                if (Nodes.Length > 0 && Nodes[^1].Actions.Length > 0) StartActionTerminal(Nodes[^1]);
                else if (ActionManager.HasActionRunning) return;
                else
                {
                    LastNode = Nodes[^1];
                    HandleOrderStop();
                    Logger.LogInformation("Order {OrderId} is finished", OrderId);
                }
            }
            else
            {
                if (NavState == NavigationState.Error) ErrorManager.AddError(RobotErrors.Error1014());
                HandleOrderStop();
                Logger.LogInformation("Order {OrderId} is error", OrderId);
            }
            return;
        }

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

            // VDA5050 Section 6.10.2: Finish EDGE actions from previous edge when leaving it
            if (CurrentEdge is not null && !RunningEdgeActionIds.IsEmpty)
            {
                Logger.LogInformation("Finishing {Count} EDGE actions from edge {EdgeId}", RunningEdgeActionIds.Count, CurrentEdge.EdgeId);
                foreach (var actionId in RunningEdgeActionIds.ToList())
                {
                    ActionManager.FinishAction(actionId);
                }
                RunningEdgeActionIds.Clear();
            }

            // VDA5050: Cập nhật edge speed và start EDGE actions khi robot vào edge tiếp theo
            // Khi đến node i, robot sẽ bắt đầu đi trên edge i (từ node i → node i+1)
            var currentNodeIndex = Array.FindIndex(Nodes, n => n.NodeId == currentNode.NodeId);
            if (currentNodeIndex >= 0 && currentNodeIndex < Edges.Length)
            {
                var nextEdge = Edges[currentNodeIndex];
                CurrentEdge = nextEdge;

                // Update edge speed
                if (nextEdge.MaxSpeed.HasValue && nextEdge.MaxSpeed.Value > 0)
                {
                    EdgeSpeed = nextEdge.MaxSpeed.Value;
                    Logger.LogInformation("Edge speed updated: {EdgeSpeed:F2} m/s (edge: {EdgeId}, node: {NodeId})",
                        EdgeSpeed, nextEdge.EdgeId, currentNode.NodeId);
                }
                else
                {
                    EdgeSpeed = 0.0; // Không giới hạn
                    Logger.LogInformation("Edge speed limit removed (edge: {EdgeId}, node: {NodeId})",
                        nextEdge.EdgeId, currentNode.NodeId);
                }

                UpdateNavigationSpeed();

                // VDA5050: Start EDGE actions for this edge
                // (Actions already added to ActionManager during HandleNewOrder)
                if (nextEdge.Actions.Length > 0)
                {
                    Logger.LogInformation("Starting {Count} EDGE actions for edge {EdgeId}", nextEdge.Actions.Length, nextEdge.EdgeId);

                    // Separate NONE from blocking EDGE actions
                    var noneActions = nextEdge.Actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
                    var blockingActions = nextEdge.Actions.Where(a => a.BlockingType == BlockingType.SOFT || a.BlockingType == BlockingType.HARD).ToArray();

                    // Start NONE actions immediately
                    foreach (var action in noneActions)
                    {
                        ActionManager.StartOrderAction(action.ActionId);
                        RunningEdgeActionIds.Add(action.ActionId);
                    }

                    // Pause navigation and enqueue blocking actions
                    if (blockingActions.Length > 0)
                    {
                        NavigationManager.Pause();
                        IsWaitingPaused = true;

                        foreach (var action in blockingActions)
                        {
                            ActionWaitingRunning.Enqueue(action);
                            RunningEdgeActionIds.Add(action.ActionId);
                        }
                    }
                }
            }
            else
            {
                // No next edge - clear current edge
                CurrentEdge = null;
            }

            if (OrderActions.TryGetValue(currentNode.NodeId, out Action[]? actions) && actions is not null && actions.Length > 0)
            {
                // VDA5050 Compliance: Separate NONE actions from blocking actions
                var noneActions = actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
                var blockingActions = actions.Where(a => a.BlockingType == BlockingType.SOFT || a.BlockingType == BlockingType.HARD).ToArray();

                // Start NONE actions immediately - they can run during movement
                foreach (var action in noneActions)
                {
                    ActionManager.StartOrderAction(action.ActionId);
                }

                // Pause navigation only if there are SOFT/HARD actions
                if (blockingActions.Length > 0)
                {
                    NavigationManager.Pause();
                    IsWaitingPaused = true;

                    // Enqueue blocking actions for sequential execution
                    foreach (var action in blockingActions)
                    {
                        ActionWaitingRunning.Enqueue(action);
                    }
                }
            }

            ClearLastNode();
        }

        UpdateNavigationSpeed();

        // VDA5050: Improved blocking logic for parallel SOFT actions
        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)
                {
                    // VDA5050: Check if there are running SOFT actions (both NODE and EDGE)
                    var runningSoftActions = ActionManager.GetRunningActions()
                        .Where(a => (a.ActionScope == ActionScope.NODE || a.ActionScope == ActionScope.EDGE) &&
                                     a.BlockingType == BlockingType.SOFT)
                        .ToList();

                    // Try to start next action(s) from queue
                    while (!ActionWaitingRunning.IsEmpty)
                    {
                        if (ActionWaitingRunning.TryPeek(out Action? action) && action is not null)
                        {
                            var robotAction = ActionManager[action.ActionId];
                            if (robotAction is null)
                            {
                                // Action not found - dequeue and skip it
                                ActionWaitingRunning.TryDequeue(out _);
                                Logger.LogWarning($"Action {action.ActionId} (type: {action.ActionType}) not found in ActionManager - skipping action");
                                continue;
                            }

                            // VDA5050: Check if action can start based on blocking type
                            if (action.BlockingType == BlockingType.HARD)
                            {
                                // HARD can only start if no actions are running
                                if (runningSoftActions.Count > 0)
                                {
                                    // Wait for SOFT actions to complete
                                    break;
                                }
                                // Start HARD action and set flag
                                ActionWaitingRunning.TryDequeue(out _);
                                ActionManager.StartOrderAction(action.ActionId);
                                ActionHard = action;
                                break; // Only one HARD action at a time
                            }
                            else if (action.BlockingType == BlockingType.SOFT)
                            {
                                // SOFT can start in parallel with other SOFT actions
                                ActionWaitingRunning.TryDequeue(out _);
                                ActionManager.StartOrderAction(action.ActionId);
                                runningSoftActions.Add(robotAction);
                                // Continue to potentially start more SOFT actions
                            }
                            else
                            {
                                // NONE should have been started already, but handle it anyway
                                ActionWaitingRunning.TryDequeue(out _);
                                ActionManager.StartOrderAction(action.ActionId);
                            }
                        }
                        else
                        {
                            break;
                        }
                    }
                }
            }
            else
            {
                if (IsWaitingPaused)
                {
                    IsWaitingPaused = false;
                    NavigationManager.Resume();
                    if (CurrentBaseNode is not null
                        && CurrentBaseNode.NodeId != Nodes[0].NodeId
                        && CurrentBaseNode.NodeId != Nodes[^1].NodeId
                        && Nodes.Length > 1)
                    {
                        NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
                    }
                }
            }
        }
    }

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

                if (NewOrderHandler.Nodes.Length == 0) 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
                {
                    if (ActionManager.HasActionRunning) return;
                    // Kiểm tra robot có nằm trên node đầu tien không
                    Node startNode = NewOrderHandler.Nodes[0];
                    var nodeDeviation = startNode.NodePosition?.AllowedDeviationXY == 0.0 ? NewOrderHandler.Nodes.Length == 1 ? 0.3 : 0.5 : startNode.NodePosition?.AllowedDeviationXY ?? 0.5;
                    var distance = Localization.DistanceTo(startNode.NodePosition?.X ?? 0, startNode.NodePosition?.Y ?? 0);
                    if (distance > nodeDeviation) throw new OrderException(RobotErrors.Error1016(startNode.NodeId, distance, nodeDeviation));

                    if (NewOrderHandler.Nodes.Length > 1)
                    {
                        Node endNode = NewOrderHandler.Nodes[^1];
                        nodeDeviation = endNode.NodePosition?.AllowedDeviationXY == 0.0 ? 0.2 : endNode.NodePosition?.AllowedDeviationXY ?? 0.2;
                        distance = Localization.DistanceTo(endNode.NodePosition?.X ?? 0, endNode.NodePosition?.Y ?? 0);
                        if (distance < nodeDeviation) throw new OrderException(RobotErrors.Error1017(endNode.NodeId, distance, nodeDeviation));
                    }
                    await HandleNewOrder(NewOrderHandler);
                }
            }
            HandleOrder();
        }
        catch (RobotException orEx)
        {
            if (orEx.Error is not null)
            {
                ErrorManager.AddError(orEx.Error);
                Logger.LogWarning("Order processing error: {orEx.Error.ErrorDescription}", orEx.Error.ErrorDescription);
            }
            else Logger.LogWarning("Order processing error: {orEx.Message}", orEx.Message);
        }
        catch (Exception ex)
        {
            Logger.LogWarning("Order processing error: {ex.Message}", ex.Message);
        }
    }

}