using Microsoft.EntityFrameworkCore;
using RobotNet.MapShares;
using RobotNet.MapShares.Dtos;
using RobotNet.MapShares.Enums;
using RobotNet.RobotManager.Data;
using RobotNet.RobotManager.Services.OpenACS;
using RobotNet.RobotManager.Services.Planner.Space;
using RobotNet.RobotManager.Services.Traffic;
using RobotNet.RobotShares.Dtos;
using RobotNet.RobotShares.Enums;
using RobotNet.RobotShares.VDA5050;
using RobotNet.RobotShares.VDA5050.Factsheet;
using RobotNet.RobotShares.VDA5050.FactsheetExtend;
using RobotNet.RobotShares.VDA5050.Order;
using RobotNet.RobotShares.VDA5050.State;
using RobotNet.RobotShares.VDA5050.Type;
using RobotNet.RobotShares.VDA5050.Visualization;
using RobotNet.Shares;
using System.Text.Json;
using Action = RobotNet.RobotShares.VDA5050.InstantAction.Action;
using NodePosition = RobotNet.RobotShares.VDA5050.Order.NodePosition;

namespace RobotNet.RobotManager.Services.Robot;

public class RobotController(string robotid,
                             string Manufacturer,
                             string Version,
                             NavigationType NavigationType,
                             IServiceProvider ServiceProvider,
                             Func<string, string, bool> FuncPub) : IRobotController, IDisposable
{
    public string SerialNumber { get; } = robotid;
    public bool IsOnline { get; set; }
    public bool IsWorking => (RobotOrder is not null && RobotOrder.IsProcessing) || StateMsg.NodeStates.Length != 0 || StateMsg.EdgeStates.Length != 0;
    public string State => GetState();
    public string[] CurrentZones => RobotOrder is null ? [] : [.. RobotOrder.CurrentZones.Select(z => z.Name)];
    public RobotOrderDto OrderState => GetOrderState();
    public RobotActionDto[] ActionStates => GetActionsState();
    public AutoResetEvent RobotUpdated { get; set; } = new(false);
    public StateMsg StateMsg { get; set; } = new();
    public VisualizationMsg VisualizationMsg { get; set; } = new();
    public FactSheetMsg FactSheetMsg { get; set; } = new();
    public FactsheetExtendMsg FactsheetExtendMsg { get; set; } = new();
    public NavigationPathEdge[] BasePath => RobotOrder is null ? [] : RobotOrder.BasePath;
    public NavigationPathEdge[] FullPath => RobotOrder is null ? [] : RobotOrder.FullPath;

    private readonly LoggerController<RobotController> Logger = ServiceProvider.GetRequiredService<LoggerController<RobotController>>();
    private readonly TimeSpan WaittingRobotFeedbackTime = TimeSpan.FromSeconds(10);
    private RobotOrder? RobotOrder;

    private CancellationTokenSource? CancelRandom;

    public void Log(string message, LogLevel level = LogLevel.Information)
    {
        switch (level)
        {
            case LogLevel.Trace: Logger.Trace($"{SerialNumber} - {message}"); break;
            case LogLevel.Error: Logger.Error($"{SerialNumber} - {message}"); break;
            case LogLevel.Warning: Logger.Warning($"{SerialNumber} - {message}"); break;
            case LogLevel.Critical: Logger.Critical($"{SerialNumber} - {message}"); break;
            case LogLevel.Information: Logger.Info($"{SerialNumber} - {message}"); break;
            case LogLevel.Debug: Logger.Debug($"{SerialNumber} - {message}"); break;
            default: Logger.Debug($"{SerialNumber} - {message}"); break;
        }
    }

    public async Task<MessageResult> CancelOrder()
    {
        try
        {
            if (StateMsg.NodeStates.Length != 0 || StateMsg.EdgeStates.Length != 0)
            {
                Action cancelOrderAction = new()
                {
                    ActionDescription = "Yêu cầu hủy nhiệm vụ hiện tại",
                    ActionParameters = [new RobotShares.VDA5050.InstantAction.ActionParameter()
                    {
                        Key = "ORDER_ID",
                        Value = StateMsg.OrderId,
                    }],
                    ActionType = ActionType.cancelOrder.ToString(),
                    BlockingType = RobotShares.VDA5050.InstantAction.BlockingType.NONE.ToString(),
                };
                var pubAction = await InstantAction(cancelOrderAction, true);
                if (!pubAction.IsSuccess) return new(false, pubAction.Message);
            }
            if (CancelRandom is not null && !CancelRandom.IsCancellationRequested) CancelRandom.Cancel();
            if (RobotOrder is not null && RobotOrder.IsProcessing)
            {
                CancellationTokenSource cancellationToken = new();
                cancellationToken.CancelAfter(TimeSpan.FromSeconds(10));
                var waitCancel = await RobotOrder.Cancel(cancellationToken.Token);
                if (!waitCancel) return new(false, "Robot không kết thúc order");
            }
            return new(true);
        }
        catch (Exception ex)
        {
            Logger.Warning($"{SerialNumber} Cancel Order logs: {ex.Message}");
            return new(false, $"{SerialNumber} Cancel Order logs: {ex.Message}");
        }
    }

    public async Task<MessageResult> CancelAction()
    {
        try
        {
            Action cancelOrderAction = new()
            {
                ActionDescription = "Yêu cầu hủy Action hiện tại",
                ActionParameters = [new RobotShares.VDA5050.InstantAction.ActionParameter()
                    {
                        Key = "ORDER_ID",
                        Value = SerialNumber,
                    }],
                ActionType = ActionType.cancelOrder.ToString(),
                BlockingType = RobotShares.VDA5050.InstantAction.BlockingType.NONE.ToString(),
            };
            var pubAction = await InstantAction(cancelOrderAction, true);
            return new(pubAction.IsSuccess, pubAction.Message);
        }
        catch (Exception ex)
        {
            Logger.Warning($"{SerialNumber} Cancel Action logs: {ex.Message}");
            return new(false, $"{SerialNumber} Cancel Action logs: {ex.Message}");
        }
    }

    public void Initialize(double x, double y, double theta)
    {
        throw new NotImplementedException();
    }

    public async Task<MessageResult<string>> InstantAction(Action action, bool waittingFisished)
    {
        CancellationTokenSource CancellationToken = new();
        CancellationToken.CancelAfter(WaittingRobotFeedbackTime);
        bool IsFeedback = false;
        int RepeatCounter = 0;
        try
        {
            action.ActionId = Guid.NewGuid().ToString();
            var instantActionsMsg = new RobotNet.RobotShares.VDA5050.InstantAction.InstantActionsMsg()
            {
                HeaderId = 1,
                Manufacturer = Manufacturer,
                Version = Version,
                Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
                SerialNumber = SerialNumber,
                Actions = [action],
            };
            var pubInstantAction = FuncPub.Invoke(VDA5050Topic.InstantActions, JsonSerializer.Serialize(instantActionsMsg, JsonOptionExtends.Write));
            if (!pubInstantAction)
            {
                string msg = $"Gửi Action xuống cho robot không thành công: {action.ActionType}";
                Log(msg, LogLevel.Warning);
                return new(false, msg);
            }
            while (!IsFeedback)
            {
                if (StateMsg is not null && StateMsg.ActionStates is not null)
                {
                    var actionstate = StateMsg.ActionStates.FirstOrDefault(ac => ac.ActionId == action.ActionId);
                    if (actionstate is not null)
                    {
                        IsFeedback = true;
                        break;
                    }
                }
                if(CancellationToken.IsCancellationRequested)
                {
                    if (RepeatCounter++ == 0)
                    {
                        await Task.Delay(1000);
                        CancellationToken = new();
                        CancellationToken.CancelAfter(WaittingRobotFeedbackTime);
                        pubInstantAction = FuncPub.Invoke(VDA5050Topic.InstantActions, JsonSerializer.Serialize(instantActionsMsg, JsonOptionExtends.Write));
                        if (!pubInstantAction)
                        {
                            string msg = $"Gửi Action xuống cho robot không thành công: {action.ActionType}";
                            Log(msg, LogLevel.Warning);
                            return new(false, msg);
                        }
                    }
                    else return new(false, $"Robot không đồng ý thực hiện action: {action.ActionType}");
                }
                await Task.Delay(500);
            }
            if (waittingFisished)
            {
                while (!CancellationToken.IsCancellationRequested)
                {
                    if (StateMsg is not null)
                    {
                        if (StateMsg.ActionStates is not null)
                        {
                            var actionCancelOrder = StateMsg.ActionStates.FirstOrDefault(a => a.ActionId == action.ActionId);
                            if (actionCancelOrder is not null)
                            {
                                if (ActionFinish(actionCancelOrder.ActionStatus)) return new(true);
                                if (ActionFailed(actionCancelOrder.ActionStatus))
                                {
                                    string msg = $"Robot trả về thực hiện action lỗi: {action.ActionType}";
                                    Log(msg, LogLevel.Warning);
                                    return new(false, msg);
                                }
                            }
                        }
                        if (GetErrorLevel() != ErrorLevel.NONE)
                        {
                            var errorStr = StateMsg.Errors.Select(error => error.ErrorType).ToArray();
                            string msg = $"Robot xảy ra lỗi sau khi gọi thực hiện action {action.ActionType}";
                            Log($"{msg} - {string.Join(",", errorStr)}", LogLevel.Warning);
                            return new(false, msg);
                        }
                    }
                    if (CancellationToken.IsCancellationRequested) return new(false, "Robot đang thực hiện Action nhưng đã quá thời gian timeout 10s");
                    await Task.Delay(500);
                }
            }
            if (CancellationToken.IsCancellationRequested) throw new Exception();
            return new(true) { Data = action.ActionId };
        }
        catch (Exception ex)
        {
            string msg = $"Có lỗi xảy ra trong quá trình gửi action : {ex.Message}";
            Log(msg, LogLevel.Warning);
            return new(false, msg);
        }
    }

    public MessageResult MoveStraight(double x, double y)
    {
        return new(false, "Chức năng này hiện không khả dụng");
    }

    public async Task<MessageResult> MoveToNode(string goalName, IDictionary<string, IEnumerable<RobotShares.VDA5050.InstantAction.Action>>? actions = null, double? lastAngle = null)
    {
        try
        {
            if (IsWorking) return new(false, "Robot đang thực hiện nhiệm vụ");

            using var scope = ServiceProvider.CreateScope();
            var PathPlanner = scope.ServiceProvider.GetRequiredService<PathPlanner>();
            var robotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
            var MapManager = scope.ServiceProvider.GetRequiredService<MapManager>();
            var TrafficManager = scope.ServiceProvider.GetRequiredService<TrafficManager>();
            var TrafficACS = scope.ServiceProvider.GetRequiredService<TrafficACS>();
            var orderLogger = scope.ServiceProvider.GetRequiredService<LoggerController<RobotOrder>>();

            var robot = await robotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == SerialNumber) ??
                        throw new Exception($"Không tìm thấy robot {SerialNumber} trong kho dữ liệu");
            var robotModel = await robotDb.RobotModels.FirstOrDefaultAsync(r => r.Id == robot.ModelId) ??
                        throw new Exception($"Không tìm thấy robot model {robot.ModelId} trong kho dữ liệu");

            var path = await PathPlanner.Planning(VisualizationMsg.AgvPosition.X, VisualizationMsg.AgvPosition.Y, VisualizationMsg.AgvPosition.Theta, NavigationType, robot.MapId, goalName);
            if (!path.IsSuccess) return new(false, path.Message);
            if(path.IsSuccess && path.Data.Nodes.Length < 2)
            {
                RobotOrder?.CreateComledted();
                return new(true, "");
            }
            if (path.Data.Nodes is null || path.Data.Edges is null || path.Data.Edges.Length == 0 || path.Data.Nodes.Length < 2)
                return new(false, $"Đường dẫn tới đích {goalName} từ [{VisualizationMsg.AgvPosition.X} - {VisualizationMsg.AgvPosition.Y} - {VisualizationMsg.AgvPosition.Theta}] không tồn tại");

            if (lastAngle != null)
            {
                path.Data.Nodes[^1].Theta = lastAngle.Value;
            }

            var mapData = await MapManager.GetMapData(robot.MapId);
            if (!mapData.IsSuccess) return new(false, mapData.Message);
            if (mapData is null || mapData.Data is null) return new(false, "Không thể tìm thấy bản đồ chứa robot.");
            var nodeInZones = await PathPlanner.GetZones(robot.MapId, path.Data.Nodes);
            if (!nodeInZones.IsSuccess) return new(false, mapData.Message);
            var order = await Order([.. path.Data.Nodes], [.. path.Data.Edges], mapData.Data, actions, TrafficManager.Enable);
            if (order.IsSuccess && order.Data is not null)
            {
                int counter = 0;
                while (counter++ < 2)
                {
                    var createAgent = TrafficManager.CreateAgent(robot.MapId, this, new()
                    {

                        NavigationType = robotModel.NavigationType,
                        Length = robotModel.Length,
                        Width = robotModel.Width,
                        NavigationPointX = robotModel.OriginX,
                        NavigationPointY = robotModel.OriginY,
                    },
                    [..path.Data.Nodes.Select(n => new TrafficNodeDto()
                    {
                        Id = n.Id,
                        Name = n.Name,
                        X = n.X,
                        Y = n.Y,
                        Direction = MapCompute.GetRobotDirection(n.Direction),
                    })], [..path.Data.Edges.Select(n => new TrafficEdgeDto()
                    {
                        Id = n.Id,
                        StartNodeId = n.StartNodeId,
                        EndNodeId = n.EndNodeId,
                        TrajectoryDegree = n.TrajectoryDegree,
                        ControlPoint1X = n.ControlPoint1X,
                        ControlPoint1Y = n.ControlPoint1Y,
                        ControlPoint2X = n.ControlPoint2X,
                        ControlPoint2Y = n.ControlPoint2Y,
                    })]);
                    if (createAgent.IsSuccess) break;
                    Logger.Warning($"{SerialNumber} - Không thể tạo traffic agent: {createAgent.Message}");
                    if (counter > 1 && !createAgent.IsSuccess)
                    {
                        var cancel = await CancelOrder();
                        if (!cancel.IsSuccess) Logger.Warning($"{SerialNumber} - Không thể hủy bỏ nhiệm vụ đã giao: {cancel.Message}");
                        return new(false, $"Không thể tạo traffic agent: {createAgent.Message} - {(cancel.IsSuccess ? "Đã hủy order" : $"Không thể hủy order: {cancel.Message}")} ");
                    }
                }
                RobotOrder = new RobotOrder([.. path.Data.Nodes], [.. path.Data.Edges], nodeInZones.Data ?? [], actions ?? new Dictionary<string, IEnumerable<Action>>(),
                                            order.Data, this, NavigationType, TrafficManager, TrafficACS, MapManager, orderLogger, Order)
                {
                    CurrentZones = RobotOrder is null ? [] : RobotOrder.CurrentZones,
                };

                return new(true);
            }
            else return new(order.IsSuccess, order.Message);
        }
        catch (Exception ex)
        {
            string msg = $"Có lỗi xảy ra trong quá trình gửi order to Node {goalName} : {ex.Message}";
            Log(msg, LogLevel.Warning);
            return new(false, msg);
        }
    }

    public MessageResult Rotate(double angle)
    {
        return new(false, "Chức năng này hiện không khả dụng");
    }

    private async Task<MessageResult> Order(OrderMsg orderMsg)
    {
        CancellationTokenSource CancellationExitToken = new();
        CancellationExitToken.CancelAfter(WaittingRobotFeedbackTime);
        try
        {
            var pubOrder = FuncPub.Invoke(VDA5050Topic.Order, JsonSerializer.Serialize(orderMsg, JsonOptionExtends.Write));
            if (!pubOrder)
            {
                string msg = $"Gửi Order xuống cho robot không thành công: {orderMsg.OrderId}";
                Log(msg, LogLevel.Warning);
                return new(false, msg);
            }
            while (!CancellationExitToken.IsCancellationRequested)
            {
                if (StateMsg is not null)
                {
                    if (StateMsg.OrderId == orderMsg.OrderId && IsWorking && StateMsg.OrderUpdateId == orderMsg.OrderUpdateId) break;
                }

                await Task.Delay(500, CancellationExitToken.Token);
            }
            if (CancellationExitToken.IsCancellationRequested) throw new Exception();
            return new(true);
        }
        catch (Exception ex)
        {
            string msg = $"Có lỗi xảy ra trong quá trình gửi order : {ex.Message}";
            if (CancellationExitToken.IsCancellationRequested) msg = $"{SerialNumber} - Robot không đồng ý thực hiện order: {orderMsg.OrderId}";

            Log(msg, LogLevel.Warning);
            return new(false, msg);
        }
    }

    private static List<Action> ConvertAction(string actions, ActionDto[] mapAction)
    {
        List<Action> Actions = [];
        var ActionIds = !string.IsNullOrEmpty(actions) ? JsonSerializer.Deserialize<Guid[]>(actions) : [];
        if (ActionIds is not null)
        {
            foreach (var actionid in ActionIds)
            {
                var actionDb = mapAction.FirstOrDefault(x => x.Id == actionid);
                if (actionDb is not null)
                {
                    var vdaAction = JsonSerializer.Deserialize<Action>(actionDb.Content, JsonOptionExtends.Read);
                    if (vdaAction is not null) Actions.Add(vdaAction);
                }
            }
        }
        return Actions;
    }

    private static Edge? ConvertToOrderEdge(EdgeDto edge, MapDataDto map)
    {
        var startNode = map.Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
        var endNode = map.Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
        var dataEdge = map.Edges.FirstOrDefault(e => e.Id == edge.Id);

        if (startNode is null || endNode is null || dataEdge is null) return null;

        List<ControlPoint> ControlPoints = [];
        ControlPoints.Add(new()
        {
            X = startNode.X,
            Y = startNode.Y,
            Weight = 1,
        });
        if (dataEdge.TrajectoryDegree == TrajectoryDegree.Two)
        {
            ControlPoints.Add(new()
            {
                X = dataEdge.ControlPoint1X,
                Y = dataEdge.ControlPoint1Y,
                Weight = 1,
            });
        }
        else if (dataEdge.TrajectoryDegree == TrajectoryDegree.Three)
        {
            ControlPoint controlPoint1 = new();
            ControlPoint controlPoint2 = new();
            if (dataEdge.StartNodeId == edge.StartNodeId)
            {
                controlPoint1.X = dataEdge.ControlPoint1X;
                controlPoint1.Y = dataEdge.ControlPoint1Y;
                controlPoint1.Weight = 1;
                controlPoint2.X = dataEdge.ControlPoint2X;
                controlPoint2.Y = dataEdge.ControlPoint2Y;
                controlPoint2.Weight = 1;
            }
            else
            {
                controlPoint2.X = dataEdge.ControlPoint1X;
                controlPoint2.Y = dataEdge.ControlPoint1Y;
                controlPoint2.Weight = 1;
                controlPoint1.X = dataEdge.ControlPoint2X;
                controlPoint1.Y = dataEdge.ControlPoint2Y;
                controlPoint1.Weight = 1;
            }
            ControlPoints.Add(controlPoint1);
            ControlPoints.Add(controlPoint2);
        }
        ControlPoints.Add(new()
        {
            X = endNode.X,
            Y = endNode.Y,
            Weight = 1,
        });

        var result = new Edge()
        {
            EdgeId = edge.Id.ToString(),
            EdgeDescription = $"{startNode.Name} - {endNode.Name}",
            EndNodeId = edge.EndNodeId.ToString(),
            StartNodeId = edge.StartNodeId.ToString(),
            SequenceId = 1,
            Direction = dataEdge.DirectionAllowed.ToString(),
            Orientation = 0,
            Released = true,
            Length = MapEditorHelper.GetEdgeLength(new()
            {
                X1 = startNode.X,
                X2 = endNode.X,
                Y1 = startNode.Y,
                Y2 = endNode.Y,
                TrajectoryDegree = dataEdge.TrajectoryDegree,
                ControlPoint1X = dataEdge.ControlPoint1X,
                ControlPoint1Y = dataEdge.ControlPoint1Y,
                ControlPoint2X = dataEdge.ControlPoint2X,
                ControlPoint2Y = dataEdge.ControlPoint2Y,
            }),
            MaxHeight = dataEdge.MaxHeight,
            MaxRotationSpeed = dataEdge.MaxRotationSpeed,
            OrientationType = "",
            MaxSpeed = dataEdge.MaxSpeed,
            MinHeight = dataEdge.MinHeight,
            RotationAllowed = dataEdge.RotationAllowed,
            Actions = [.. ConvertAction(dataEdge.Actions, [.. map.Actions])],
            Trajectory = new()
            {
                Degree = dataEdge.TrajectoryDegree == TrajectoryDegree.One ? 1 : dataEdge.TrajectoryDegree == TrajectoryDegree.Two ? 2 : 3,
                KnotVector = dataEdge.TrajectoryDegree == TrajectoryDegree.One ? [0, 0, 1, 1] : dataEdge.TrajectoryDegree == TrajectoryDegree.Two ? [0, 0, 0, 1, 1, 1] : [0, 0, 0, 0, 1, 1, 1, 1],
                ControlPoints = [.. ControlPoints],
            },
        };
        return result;
    }

    public static void CreateOrderMsg(ref OrderMsg orderMsgTemplate, NodeDto[] nodes, EdgeDto[] edges, MapDataDto map, IDictionary<string, IEnumerable<Action>> actions, NavigationType navigationType, bool trafficEnable)
    {
        if (nodes.Length <= 1 || edges.Length < 1) throw new ArgumentException("Dữ liệu đường đi không hợp lệ");
        List<Node> OrderNodes = [];
        List<Edge> OrderEdges = [];
        for (int i = 0; i < nodes.Length; i++)
        {
            var nodeDb = map.Nodes.FirstOrDefault(n => n.Id == nodes[i].Id);
            if (nodeDb is null)
            {
                if (i != 0) throw new ArgumentException("Đường đi tồn tại node không hợp lệ");

                var angleForward = Math.Atan2(nodes[1].Y - nodes[0].Y, nodes[1].X - nodes[0].X);
                var angleBackward = Math.Atan2(nodes[0].Y - nodes[1].Y, nodes[0].X - nodes[1].X);
                OrderNodes.Add(new()
                {
                    NodeId = nodes[i].Id.ToString(),
                    Released = true,
                    SequenceId = i,
                    NodeDescription = nodes[i].Name ?? "",
                    NodePosition = new NodePosition()
                    {
                        X = nodes[i].X,
                        Y = nodes[i].Y,
                        Theta = navigationType == NavigationType.OmniDrive ? 10 : nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward,
                        MapDescription = map.Name,
                        MapId = map.Id.ToString(),
                        AllowedDeviationTheta = nodes[i].AllowedDeviationTheta,
                        AllowedDeviationXY = nodes[i].AllowedDeviationXy,
                    },
                    Actions = actions.TryGetValue(nodes[i].Name, out IEnumerable<Action>? anoCustomActions) ? [.. anoCustomActions] : [],
                });
                continue;
            }

            double angle;
            if (i == 0)
            {
                var nearNode = MapEditorHelper.GetNearByNode(nodes[0], nodes[1], edges[0], 0.01);
                var angleForward = Math.Atan2(nearNode.Y - nodes[0].Y, nearNode.X - nodes[0].X);
                var angleBackward = Math.Atan2(nodes[0].Y - nearNode.Y, nodes[0].X - nearNode.X);
                angle = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
            }
            else
            {
                var nearNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i - 1], edges[i - 1], 0.01);
                var angleForward = Math.Atan2(nodeDb.Y - nearNode.Y, nodeDb.X - nearNode.X);
                var angleBackward = Math.Atan2(nearNode.Y - nodeDb.Y, nearNode.X - nodeDb.X);

                if (nodes[i].Direction == nodes[i - 1].Direction) angle = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
                else angle = nodes[i - 1].Direction == Direction.FORWARD ? angleForward : angleBackward;
            }

            List<Action> nodeActions = ConvertAction(nodeDb.Actions, [.. map.Actions]);
            if (actions.TryGetValue(nodes[i].Name, out IEnumerable<Action>? customActions) && customActions is not null)
            {
                nodeActions.AddRange(customActions);
            }
            OrderNodes.Add(new()
            {
                NodeId = nodes[i].Id.ToString(),
                Released = i == 0 || !trafficEnable,
                SequenceId = i,
                NodeDescription = nodes[i].Name ?? "",
                NodePosition = new NodePosition()
                {
                    X = nodeDb.X,
                    Y = nodeDb.Y,
                    Theta = i == (nodes.Length - 1) ? (nodeDb.Theta * Math.PI / 180) : (navigationType == NavigationType.OmniDrive ? 10 : angle),
                    MapDescription = map.Name,
                    MapId = map.Id.ToString(),
                    AllowedDeviationTheta = nodeDb.AllowedDeviationTheta,
                    AllowedDeviationXY = nodeDb.AllowedDeviationXy,
                },
                Actions = [.. nodeActions],
            });
        }
        foreach (var edge in edges)
        {
            var orderEdge = ConvertToOrderEdge(edge, map);
            if (orderEdge is null)
            {
                if (edge == edges.First())
                {
                    orderEdge = new Edge()
                    {
                        EdgeId = edge.Id.ToString(),
                        StartNodeId = edge.StartNodeId.ToString(),
                        EndNodeId = edge.EndNodeId.ToString(),
                        EdgeDescription = $"{edge.StartNodeId} - {edge.EndNodeId}",
                        Direction = edge.DirectionAllowed.ToString(),
                        Orientation = 0,
                        Released = false,
                        Length = 0,
                        MaxRotationSpeed = 0.5,
                        OrientationType = "",
                        MaxSpeed = 0.5,
                        MinHeight = 0,
                        RotationAllowed = true,
                        Actions = [.. ConvertAction(edge.Actions, [.. map.Actions])],
                        Trajectory = new()
                        {
                            Degree = 1,
                            KnotVector = [0, 0, 1, 1],
                            ControlPoints = [new()
                                {
                                    X = nodes[0].X,
                                    Y = nodes[0].Y,
                                    Weight = 1,
                                },
                                new()
                                {
                                    X = nodes[1].X,
                                    Y = nodes[1].Y,
                                    Weight = 1,
                                }],
                        },
                    };
                }
                else throw new ArgumentException("Đường đi tồn tại edge không hợp lệ");
            }
            orderEdge.SequenceId = Array.IndexOf(edges, edge);
            orderEdge.Released = !trafficEnable;
            OrderEdges.Add(orderEdge);
        }
        orderMsgTemplate.Nodes = [.. OrderNodes];
        orderMsgTemplate.Edges = [.. OrderEdges];
    }

    private async Task<MessageResult<OrderMsg?>> Order(List<NodeDto> nodes, List<EdgeDto> edges, MapDataDto map, IDictionary<string, IEnumerable<Action>>? actions, bool trafficManager)
    {
        try
        {
            var OrderMsg = new OrderMsg()
            {
                HeaderId = 1,
                Manufacturer = Manufacturer,
                Version = Version,
                Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
                SerialNumber = SerialNumber,
                OrderId = Guid.NewGuid().ToString(),
                OrderUpdateId = 1,
                ZoneSetId = map.Name ?? "",
            };
            CreateOrderMsg(ref OrderMsg, [.. nodes], [.. edges], map, actions ?? new Dictionary<string, IEnumerable<Action>>(), NavigationType, trafficManager);
            var pubOrder = await Order(OrderMsg);
            if (pubOrder.IsSuccess) return new(true) { Data = OrderMsg };
            return new(false, pubOrder.Message);
        }
        catch (Exception ex)
        {
            string msg = "Có lỗi xảy ra trong quá trình tạo Order";
            Log($"{msg}: {ex.Message}", LogLevel.Warning);
            return new(true, msg);
        }
    }

    private string GetState()
    {
        if (StateMsg.Information is not null)
        {
            var RobotGeneral = StateMsg.Information.FirstOrDefault(info => info.InfoType == InformationType.robot_general.ToString());
            if (RobotGeneral is not null)
            {
                var references = RobotGeneral.InfoReferences.FirstOrDefault(key => key.ReferenceKey == InformationReferencesKey.robot_state.ToString());
                if (references is not null) return references.ReferenceValue;
            }
        }
        return "";
    }

    private RobotOrderDto GetOrderState()
    {
        return new()
        {
            OrderId = StateMsg.OrderId,
            IsCompleted = (RobotOrder is not null && RobotOrder.IsCompleted) || RobotOrder is null,
            IsError = RobotOrder is not null && RobotOrder.IsError,
            IsProcessing = RobotOrder is not null && RobotOrder.IsProcessing,
            IsCanceled = RobotOrder is not null && RobotOrder.IsCanceled,
            Errors = RobotOrder is not null ? RobotOrder.Errors : [],
        };
    }

    private RobotActionDto[] GetActionsState()
    {
        List<RobotActionDto> Actions = [];
        foreach (var action in StateMsg.ActionStates)
        {
            Actions.Add(new RobotActionDto()
            {
                ActionId = action.ActionId,
                Action = action,
                IsError = ActionFailed(action.ActionStatus),
                IsCompleted = ActionFinish(action.ActionStatus),
                IsProcessing = ActionRunning(action.ActionStatus),
                Errors = [.. GetErrorString()],
            });
        }
        return [.. Actions];
    }

    private ErrorLevel GetErrorLevel()
    {
        if (StateMsg.Errors is not null)
        {
            if (StateMsg.Errors.Any(error => error.ErrorLevel == ErrorLevel.FATAL.ToString())) return ErrorLevel.FATAL;
            if (StateMsg.Errors.Any(error => error.ErrorLevel == ErrorLevel.WARNING.ToString())) return ErrorLevel.WARNING;
        }
        return ErrorLevel.NONE;
    }

    private static bool ActionFinish(string status) => status == ActionStatus.FINISHED.ToString();

    private static bool ActionFailed(string status) => status == ActionStatus.FAILED.ToString();

    private static bool ActionRunning(string status) => status == ActionStatus.RUNNING.ToString();

    private IEnumerable<string> GetErrorString()
    {
        if (StateMsg.Errors is not null && StateMsg.Errors.Length > 0)
        {
            foreach (var error in StateMsg.Errors)
            {
                yield return $"{error.ErrorType} - {error.ErrorDescription}";
            }
        }
        yield break;
    }

    public Task<MessageResult> MoveRandom(List<string> nodes)
    {
        try
        {
            CancelRandom = new CancellationTokenSource();
            var random = new Random();
            var randomTask = Task.Run(async () =>
            {
                while (!CancelRandom.IsCancellationRequested)
                {
                    var index = random.Next(nodes.Count);
                    await MoveToNode(nodes[index]);
                    while (!CancelRandom.IsCancellationRequested)
                    {
                        if (!IsWorking) break;
                        await Task.Delay(1000);
                    }
                    await Task.Delay(2000);
                }
            }, cancellationToken: CancelRandom.Token);
        }
        catch { }
        return Task.FromResult<MessageResult>(new(true));
    }

    public void Dispose()
    {
        if (IsOnline) IsOnline = false;
        GC.SuppressFinalize(this);
    }
}