Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/RobotOrderController.cs

@@ -0,0 +1,969 @@
+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))
+        {
+            if (safetySpeed == Interfaces.SafetySpeed.Very_Fast)
+            {
+                SafetySpeed = 0.0; // 0 = không giới hạn từ safety; edge/config quyết định 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 = 1.0;
+                return true;
+            case Interfaces.SafetySpeed.Slow:
+                speed = 0.3;
+                return true;
+            case Interfaces.SafetySpeed.Normal:
+                speed = 0.6;
+                return true;
+            case Interfaces.SafetySpeed.Medium:
+                speed = 0.9;
+                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()
+    {
+        if (Nodes.Length <= 0 || IsNavigationFinished)
+            return;
+
+        CachedNavConfig ??= NavigationConfig.GetNavigationConfig();
+        double maxConfigSpeed = CachedNavConfig.MaxLinearVelocity;
+        if (maxConfigSpeed <= 0)
+            return;
+
+        // Bắt đầu từ max config; EdgeSpeed/SafetySpeed = 0 nghĩa là không giới hạn từ nguồn đó
+        double targetSpeed = maxConfigSpeed;
+
+        if (EdgeSpeed > 0)
+            targetSpeed = Math.Min(targetSpeed, EdgeSpeed);
+
+        if (SafetySpeed > 0)
+            targetSpeed = Math.Min(targetSpeed, SafetySpeed);
+
+        targetSpeed = Math.Max(targetSpeed, CachedNavConfig.MinLinearVelocity);
+
+        if (Math.Abs(CurrentSpeed - targetSpeed) < 0.01)
+            return;
+
+        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);
+    }
+
+    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);
+        }
+    }
+
+}