RobotApp/RobotApp/Services/Robot/RobotOrderController.cs

using RobotApp.Interfaces;
using RobotApp.Services.Exceptions;
using RobotApp.VDA5050.Order;
using RobotApp.VDA5050.State;
using Action = RobotApp.VDA5050.InstantAction.Action;

namespace RobotApp.Services.Robot;

public class RobotOrderController(INavigation NavigationManager, IInstantActions ActionManager, IError ErrorManager, 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 { get; private set; } = string.Empty;
    public int LastNodeSequenceId { get; private set; }

    private readonly Dictionary<string, Action[]> OrderActions = [];
    private readonly Mutex OrderMutex = new();

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

    private int BaseSequenceId = 0;

    public void StartOrder(string orderId, Node[] nodes, Edge[] edges)
    {
        if (OrderMutex.WaitOne(2000))
        {
            try
            {
                if (!string.IsNullOrEmpty(OrderId) && orderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, orderId));
                if (nodes.Length < 2) throw new OrderException(RobotErrors.Error1002(nodes.Length));
                if (edges.Length < 1) throw new OrderException(RobotErrors.Error1003(edges.Length));
                if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(nodes.Length, edges.Length));
                if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State));

                for (int i = 0; i < nodes.Length; i++)
                {
                    if (nodes[i].Actions is not null && nodes[i].Actions.Length > 0)
                    {
                        foreach (var item in nodes[i].Actions)
                        {
                            item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}";
                        }
                        OrderActions.Add(nodes[i].NodeId, nodes[i].Actions);
                    }
                    if (i < nodes.Length - 1 && edges[i] is not null && edges[i].Length > 0)
                    {
                        foreach (var item in edges[i].Actions)
                        {
                            item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}";
                        }
                        OrderActions.TryAdd(nodes[i].NodeId, edges[i].Actions);
                    }
                    if (nodes[i].SequenceId != i) throw new OrderException(RobotErrors.Error1010(nodes[i].NodeId, nodes[i].SequenceId, i));
                    if (i < nodes.Length - 1 && edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(edges[i].EdgeId, edges[i].SequenceId, i));
                    if (nodes[i].Released) BaseSequenceId = nodes[i].SequenceId;
                }

                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
                })];

                OrderId = orderId;
                ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
                NavigationManager.Move(nodes, edges);
                HandleNavigationStart();
            }
            catch (OrderException orEx)
            {
                if (orEx.Error is not null)
                {
                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
                    Logger.Warning($"Lỗi khi khởi tạo Order: {orEx.Error.ErrorDescription}");
                }
            }
            catch (Exception ex)
            {
                Logger.Warning($"Lỗi khi khởi tạo Order: {ex.Message}");
            }
            finally
            {
                OrderMutex.ReleaseMutex();
            }
        }
        Logger.Warning($"Lỗi khi khởi tạo Order: có order đang được khởi tạo chưa xong");
    }

    public void UpdateOrder(int orderUpdateId,Node[] nodes, Edge[] edges)
    {
        if (OrderMutex.WaitOne(2000))
        {
            try
            {
                if (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.Error1005());
                if (orderUpdateId > OrderUpdateId)
                {
                    OrderUpdateId = orderUpdateId;
                    // Check Order Update hợp lệ
                    // Check Order update Hoziron hay không
                }
            }
            catch (OrderException orEx)
            {
                if (orEx.Error is not null)
                {
                    ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
                    Logger.Warning($"Lỗi khi cập nhật Order: {orEx.Error.ErrorDescription}");
                }
            }
            catch (Exception ex)
            {
                Logger.Warning($"Lỗi khi cập nhật Order: {ex.Message}");
            }
            finally
            {
                OrderMutex.ReleaseMutex();
            }
        }
        Logger.Warning($"Lỗi khi cập nhật Order: có order đang được cập nhật chưa xong");
    }

    public void StopOrder()
    {
        HandleNavigationStop();
        NavigationManager.CancelMovement();

        OrderId = string.Empty;
        OrderActions.Clear();
        OrderUpdateId = 0;
        BaseSequenceId = 0;
        LastNodeSequenceId = 0;
        NodeStates = [];
        EdgeStates = [];
    }

    public void PauseOrder()
    {
        throw new NotImplementedException();
    }

    public void ResumeOrder()
    {
        throw new NotImplementedException();
    }

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

    private void HandleNavigationStop()
    {
        OrderTimer?.Dispose();
    }

    private void OrderHandler()
    {
        if(!string.IsNullOrEmpty(OrderId) && BaseSequenceId > 0)
        {
            if(NavigationManager.State == NavigationState.Initializing)
            {
                // khởi tạo Order
                
            }
            else
            {
                // xử lí khi có Order
            }
        }
    }
}