using RobotNet.VDA5050.Order;
using RobotNet.VDA5050.State;
using RobotNet10.Common;
using RobotNet10.FleetManager.Data;
using RobotNet10.FleetManager.Services.ConfigManager;
using RobotNet10.FleetManager.Services.OpenACS;
using RobotNet10.FleetManager.Services.RobotController;
using RobotNet10.FleetManager.Services.RobotManager;
using RobotNet10.FleetManager.Services.TrafficControl.Models;
using RobotNet10.MapManager.Services;
using System.Collections.Concurrent;

namespace RobotNet10.FleetManager.Services.TrafficControl.ACS;

public class OrderACSControl : IOrderControlService, IDisposable
{
    private readonly IServiceScopeFactory _serviceScopeFactory;
    private readonly IACSTrafficConfig _acsTrafficConfig;
    private readonly TrafficACS _trafficACS;
    private readonly Logger<OrderACSControl> _logger;
    private readonly ILogger<OrderACSControl> _loggerTimer;

    // Store order state per robot
    private readonly ConcurrentDictionary<string, OrderACSState> _orderStates = new();

    private WatchTimerAsync<OrderACSControl>? _processingTimer;
    private readonly Lock _timerLock = new();
    private bool _disposed = false;

    public OrderACSControl(
        IServiceScopeFactory serviceScopeFactory,
        IACSTrafficConfig acsTrafficConfig,
        TrafficACS trafficACS,
        Logger<OrderACSControl> logger,
        ILoggerFactory loggerFactory)
    {
        _serviceScopeFactory = serviceScopeFactory ?? throw new ArgumentNullException(nameof(serviceScopeFactory));
        _acsTrafficConfig = acsTrafficConfig ?? throw new ArgumentNullException(nameof(acsTrafficConfig));
        _trafficACS = trafficACS ?? throw new ArgumentNullException(nameof(trafficACS));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));
        _loggerTimer = loggerFactory?.CreateLogger<OrderACSControl>() ?? throw new ArgumentNullException(nameof(loggerFactory));

        // Subscribe to config changes
        _acsTrafficConfig.ConfigChanged += OnConfigChanged;

        _logger.Info($"Started OrderACSControl processing timer with interval {_acsTrafficConfig.TrafficInterval}ms");
    }

    private void OnConfigChanged(object? sender, EventArgs e)
    {
        try
        {
            var newInterval = _acsTrafficConfig.TrafficInterval;
            if (newInterval == _processingTimer?.Interval) return;
            _logger.Info($"ACSTrafficConfig changed, updating timer interval to {newInterval}ms");

            lock (_timerLock)
            {
                // Stop old timer
                _processingTimer?.Stop();
                _processingTimer?.Dispose();

                // Start new timer with new interval
                StartProcessingTimer();
            }

            _logger.Info($"OrderACSControl processing timer updated to interval {newInterval}ms");
        }
        catch (Exception ex)
        {
            _logger.Error($"Error updating timer interval: {ex.Message}");
        }
    }

    private void StartProcessingTimer()
    {
        var interval = _acsTrafficConfig.TrafficInterval;
        _processingTimer = new WatchTimerAsync<OrderACSControl>(
            interval,
            ProcessAllOrdersAsync,
            _loggerTimer
        );
        _processingTimer.Start();
    }

    public void Dispose()
    {
        if (_disposed) return;

        // Unsubscribe from config changes
        _acsTrafficConfig.ConfigChanged -= OnConfigChanged;

        lock (_timerLock)
        {
            _processingTimer?.Stop();
            _processingTimer?.Dispose();
        }

        _disposed = true;
        _logger.Info("Stopped OrderACSControl processing timer");
        GC.SuppressFinalize(this);

    }

    private async Task ProcessAllOrdersAsync()
    {
        try
        {
            // Process all active orders
            foreach (var kvp in _orderStates)
            {
                var robotId = kvp.Key;
                var orderState = kvp.Value;

                if (orderState.Status != OrderStatus.IsProccessing)
                {
                    // Skip non-processing orders
                    continue;
                }

                // Get robot controller and data using service scope to avoid circular dependency
                IRobotController? robotController = null;
                try
                {
                    using var scope = _serviceScopeFactory.CreateScope();
                    var robotManagerService = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
                    robotController = robotManagerService.GetRobotController(robotId);
                }
                catch (Exception ex)
                {
                    _logger.Error($"ProcessAllOrdersAsync: Error getting robot controller for {robotId}: {ex.Message}");
                    continue;
                }

                if (robotController == null)
                {
                    continue;
                }

                // Check if robot is online
                if (!robotController.IsOnline)
                {
                    // Check if robot has been offline for more than 1 minute
                    var offlineDuration = DateTime.UtcNow - orderState.LastUpdated;
                    if (offlineDuration.TotalMinutes > 1)
                    {
                        orderState.Status = OrderStatus.IsError;
                        orderState.Error = $"Robot {robotId} has been offline for more than 1 minute";
                        orderState.LastUpdated = DateTime.UtcNow;
                        _logger.Warning($"CheckRobotOnline: Order for robot {robotId} marked as Error due to offline timeout ({offlineDuration.TotalMinutes:F1} minutes)");
                        continue;
                    }
                    // Robot is offline but less than 1 minute, skip processing
                    continue;
                }

                var robotData = robotController.Data;
                var stateMsg = robotData?.State;

                if (stateMsg == null)
                {
                    continue;
                }

                // Update last updated time
                orderState.LastUpdated = DateTime.UtcNow;

                // Check if robot has reached a mapped node
                var currentLastNodeId = stateMsg.LastNodeId;
                await CheckAndProcessMappedNodesAsync(robotId, currentLastNodeId, orderState, stateMsg);
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"ProcessAllOrdersAsync: Error processing orders: {ex.Message}");
        }
    }

    public OrderStatus GetRobotOrderStatus(string robotId)
    {
        if (_orderStates.TryGetValue(robotId, out var state))
        {
            return state.Status;
        }
        return OrderStatus.Empty; // No order found
    }

    public async Task<bool> CreateRobotOrderAsync(string robotId, RobotRoute route)
    {
        try
        {
            if (string.IsNullOrEmpty(robotId))
            {
                _logger.Warning("CreateRobotOrderAsync: robotId is null or empty");
                return false;
            }

            if (route == null || route.FullRoute == null || route.FullRoute.Count == 0)
            {
                _logger.Warning($"CreateRobotOrderAsync: Invalid route for robot {robotId}");
                return false;
            }

            // Find all nodes that are mapped to ACS zones (IN and OUT separately)
            var (inMappedNodes, outMappedNodes) = await FindMappedNodesAsync(route);

            // Find first IN mapped node index (only IN nodes affect Base/Horizon calculation)
            var firstMappedNodeIndex = FindFirstMappedNodeIndex(route, inMappedNodes);

            int baseSegmentCount;
            if (inMappedNodes.Count == 0 || firstMappedNodeIndex < 0)
            {
                // No IN mapped nodes, base = full route
                baseSegmentCount = route.FullRoute.Count;
                _logger.Info($"CreateRobotOrderAsync: No IN mapped nodes found for robot {robotId}, base = full route");
            }
            else
            {
                // Calculate Base: segments from start to first mapped node + 1 segment
                baseSegmentCount = firstMappedNodeIndex + 3;
                if (baseSegmentCount > route.FullRoute.Count)
                {
                    baseSegmentCount = route.FullRoute.Count;
                }
            }

            // Split route into Base and Horizon
            route.Base = [.. route.FullRoute.Take(baseSegmentCount)];
            route.Horizon = [.. route.FullRoute.Skip(baseSegmentCount)];

            // Mark base segments as released
            foreach (var segment in route.Base)
            {
                segment.Released = true;
            }

            _logger.Info($"CreateRobotOrderAsync: Order created for robot {robotId}, Base segments: {route.Base.Count}, Horizon segments: {route.Horizon.Count}, IN mapped nodes: {inMappedNodes.Count}, OUT mapped nodes: {outMappedNodes.Count}");

            // Send order to robot
            var send = await SendOrderToRobotAsync(robotId, route, isInitial: true);
            if (send)
            {
                // Create order state
                var orderState = new OrderACSState
                {
                    RobotId = robotId,
                    Status = OrderStatus.IsProccessing,
                    Route = route,
                    InMappedNodes = inMappedNodes,
                    OutMappedNodes = outMappedNodes,
                    CurrentInMappedNodeIndex = 0,
                    CreatedAt = DateTime.UtcNow,
                    LastUpdated = DateTime.UtcNow,
                    ZoneRequestInCompleting = [],
                    ZoneRequestOutCompleting = [],
                };

                _orderStates.AddOrUpdate(robotId, orderState, (key, old) => orderState);

            }

            return send;
        }
        catch (Exception ex)
        {
            _logger.Error($"CreateRobotOrderAsync: Error creating order for robot {robotId}: {ex.Message}");
            return false;
        }
    }

    private async Task<(List<(string NodeIdString, string ZoneId)> InMappedNodes, List<(string NodeIdString, string ZoneId)> OutMappedNodes)> FindMappedNodesAsync(RobotRoute route)
    {
        var inMappedNodes = new List<(string NodeIdString, string ZoneId)>();
        var outMappedNodes = new List<(string NodeIdString, string ZoneId)>();
        var acsZoneMapping = _acsTrafficConfig.ACSZoneMaping;
        var acsOutMapping = _acsTrafficConfig.ACSOutMaping;

        // Get all unique NodeIds (Guid) from route segments
        var nodeIds = route.FullRoute
            .Where(s => s.VdaNode != null)
            .Select(s => s.NodeId)
            .Distinct()
            .ToList();

        if (nodeIds.Count == 0)
        {
            return (inMappedNodes, outMappedNodes);
        }

        // Create mapping: NodeId (Guid) -> NodeName
        var nodeIdToNodeName = new Dictionary<Guid, string?>();
        try
        {
            using var scope = _serviceScopeFactory.CreateScope();
            var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();

            foreach (var nodeId in nodeIds)
            {
                var node = await nodeService.GetByIdAsync(nodeId, includeVehicleProperties: false);
                if (node != null && !string.IsNullOrEmpty(node.NodeName))
                {
                    nodeIdToNodeName[nodeId] = node.NodeName;
                }
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"FindMappedNodesAsync: Error getting NodeName from database: {ex.Message}");
            // Continue with empty mapping - will skip nodes without NodeName
        }

        foreach (var segment in route.FullRoute)
        {
            if (segment.VdaNode != null)
            {
                var nodeIdString = segment.VdaNode.NodeId;

                // Get NodeName from mapping (using NodeId Guid)
                if (!nodeIdToNodeName.TryGetValue(segment.NodeId, out var nodeName) || string.IsNullOrEmpty(nodeName))
                {
                    // Skip if NodeName not found
                    continue;
                }

                // Check if node is in ACSZoneMapping (RequestIn) using NodeName
                if (acsZoneMapping.TryGetValue(nodeName, out var zoneId))
                {
                    inMappedNodes.Add((nodeIdString, zoneId));
                }

                // Check if node is in ACSOutMapping (RequestOut) using NodeName
                if (acsOutMapping.TryGetValue(nodeName, out var outZoneId))
                {
                    outMappedNodes.Add((nodeIdString, outZoneId));
                }
            }
        }

        return (inMappedNodes, outMappedNodes);
    }

    private static int FindFirstMappedNodeIndex(RobotRoute route, List<(string NodeIdString, string ZoneId)> mappedNodes)
    {
        if (mappedNodes.Count == 0) return -1;

        (string NodeIdString, _) = mappedNodes[0];

        for (int i = 0; i < route.FullRoute.Count; i++)
        {
            var segment = route.FullRoute[i];
            if (segment.VdaNode != null && segment.VdaNode.NodeId == NodeIdString)
            {
                return i;
            }
        }

        return -1;
    }

    private async Task CheckAndProcessMappedNodesAsync(string robotId, string lastNodeId, OrderACSState orderState, StateMsg stateMsg)
    {
        try
        {
            foreach (var (NodeIdString, ZoneId) in orderState.OutMappedNodes)
            {
                if (lastNodeId == NodeIdString && !string.IsNullOrEmpty(ZoneId) && !orderState.ZoneRequestOutCompleting.Contains(ZoneId) && !orderState.ZoneRequestOutCompleted.Contains(ZoneId))
                {
                    orderState.ZoneRequestOutCompleting.Add(ZoneId);
                }
            }
            var requestOut = ProcessRequestOutAsync(robotId, orderState);

            foreach (var (NodeIdString, ZoneId) in orderState.InMappedNodes)
            {
                if (lastNodeId == NodeIdString && !string.IsNullOrEmpty(ZoneId) && !orderState.ZoneRequestInCompleting.Contains(ZoneId) && !orderState.ZoneRequestInCompleted.Contains(ZoneId))
                {
                    orderState.ZoneRequestInCompleting.Add(ZoneId);
                }
            }
            await ProcessRequestInAsync(robotId, orderState);

            await requestOut.WaitAsync(CancellationToken.None);

            // Check if order is completed
            CheckOrderCompletion(robotId, orderState, stateMsg);
        }
        catch (Exception ex)
        {
            _logger.Error($"CheckAndProcessMappedNodes: Error for robot {robotId}: {ex.Message}");
            orderState.Status = OrderStatus.IsError;
            orderState.Error = ex.Message;
        }
    }

    private async Task ProcessRequestInAsync(string robotId, OrderACSState orderState)
    {
        string[] zoneIdsIn = [.. orderState.ZoneRequestInCompleting];
        if (zoneIdsIn.Length == 0) return;
        foreach (var zoneId in zoneIdsIn)
        {
            try
            {

                if (orderState.ZoneRequestInCompleted.Contains(zoneId)) continue;
                _logger.Info($"ProcessRequestInAsync: Robot {robotId} requesting into zone {zoneId}");

                var result = await _trafficACS.RequestIn(robotId, zoneId);

                if (result.IsSuccess && result.Data)
                {
                    // RequestIn successful - save zone to cache for future RequestOut validation
                    orderState.ZoneRequestInCompleted.Add(zoneId);
                    orderState.ZoneRequestInCompleting.Remove(zoneId);
                    orderState.ZoneRequestOutCompleted.Remove(zoneId);
                    orderState.LastUpdated = DateTime.UtcNow;

                    _logger.Info($"ProcessRequestInAsync: Robot {robotId} successfully requested into zone {zoneId}");
                }
                else
                {
                    // RequestIn failed, just log warning - will retry on next timer cycle
                    _logger.Warning($"ProcessRequestInAsync: Robot {robotId} failed to request into zone {zoneId}: {result.Message}. Will retry on next cycle.");
                }
            }
            catch (Exception ex)
            {
                _logger.Error($"ProcessRequestInAsync: Error for robot {robotId}, zone {zoneId}: {ex.Message}. Will retry on next cycle.");
            }
        }
        if (orderState.ZoneRequestInCompleting.Count == 0)
        {
            orderState.CurrentInMappedNodeIndex++;
            await TryReleaseNextHorizonSegmentAsync(robotId, orderState);
        }
    }

    private async Task ProcessRequestOutAsync(string robotId, OrderACSState orderState)
    {
        string[] zoneIdsOut = [.. orderState.ZoneRequestOutCompleting];
        if (zoneIdsOut.Length == 0) return;
        foreach (var zoneId in zoneIdsOut)
        {
            try
            {
                if (orderState.ZoneRequestOutCompleted.Contains(zoneId)) continue;
                _logger.Info($"ProcessRequestOutAsync: Robot {robotId} requesting out of zone {zoneId}");

                var result = await _trafficACS.RequestOut(robotId, zoneId);

                if (result.IsSuccess && result.Data)
                {
                    orderState.LastUpdated = DateTime.UtcNow;
                    orderState.ZoneRequestOutCompleted.Add(zoneId);
                    orderState.ZoneRequestOutCompleting.Remove(zoneId);
                    orderState.ZoneRequestInCompleted.Remove(zoneId);

                    _logger.Info($"ProcessRequestOutAsync: Robot {robotId} successfully requested out of zone {zoneId}");
                    // Note: OUT does NOT affect Base/Horizon, so we don't call TryReleaseNextHorizonSegmentAsync
                }
                else
                {
                    // RequestOut failed - will retry on next timer cycle until successful
                    // OUT must retry until successful (unlike IN which can be skipped)
                    _logger.Warning($"ProcessRequestOutAsync: Robot {robotId} failed to request out of zone {zoneId}: {result.Message}. Will retry on next cycle until successful.");
                }
            }
            catch (Exception ex)
            {
                // RequestOut error - will retry on next timer cycle until successful
                _logger.Error($"ProcessRequestOutAsync: Error for robot {robotId}, zone {zoneId}: {ex.Message}. Will retry on next cycle until successful.");
            }
        }
    }

    private async Task TryReleaseNextHorizonSegmentAsync(string robotId, OrderACSState orderState)
    {
        try
        {
            bool horizonUpdated = false;
            int baseCountBeforeRelease = orderState.Route.Base.Count; // Store base count before release

            // Check if there are more IN mapped nodes to process (only IN affects Base/Horizon)
            if (orderState.CurrentInMappedNodeIndex >= orderState.InMappedNodes.Count)
            {
                // All mapped nodes processed, check if we can release all remaining horizon
                if (orderState.Route.Horizon.Count > 0)
                {
                    // Move all remaining horizon to base
                    var segmentsToMove = orderState.Route.Horizon.ToList();
                    orderState.Route.Base.AddRange(segmentsToMove);
                    orderState.Route.Horizon.Clear();

                    // Only set Released = true for newly released segments
                    foreach (var segment in segmentsToMove)
                    {
                        segment.Released = true;
                    }

                    orderState.LastUpdated = DateTime.UtcNow;
                    horizonUpdated = true;
                    _logger.Info($"TryReleaseNextHorizonSegmentAsync: Released all remaining horizon segments for robot {robotId}");
                }
            }
            else
            {
                // Find next IN mapped node
                var (NodeIdString, _) = orderState.InMappedNodes[orderState.CurrentInMappedNodeIndex];

                // Find index of next mapped node in full route
                int nextMappedNodeIndex = -1;
                for (int i = 0; i < orderState.Route.FullRoute.Count; i++)
                {
                    var segment = orderState.Route.FullRoute[i];
                    if (segment.VdaNode != null && segment.VdaNode.NodeId == NodeIdString)
                    {
                        nextMappedNodeIndex = i;
                        break;
                    }
                }

                if (nextMappedNodeIndex >= 0)
                {
                    // Calculate how many segments to release: from current base end to next mapped node + 1
                    var currentBaseEndIndex = orderState.Route.Base.Count;
                    var segmentsToRelease = nextMappedNodeIndex - currentBaseEndIndex + 3;

                    if (segmentsToRelease > 0 && segmentsToRelease <= orderState.Route.Horizon.Count)
                    {
                        // Release segments to base
                        var segmentsToMove = orderState.Route.Horizon.Take(segmentsToRelease).ToList();
                        orderState.Route.Base.AddRange(segmentsToMove);
                        orderState.Route.Horizon.RemoveRange(0, segmentsToRelease);

                        foreach (var segment in segmentsToMove)
                        {
                            segment.Released = true;
                        }

                        orderState.LastUpdated = DateTime.UtcNow;
                        horizonUpdated = true;
                        _logger.Info($"TryReleaseNextHorizonSegmentAsync: Released {segmentsToRelease} segments to base for robot {robotId}");
                    }
                }
            }

            // If horizon was updated, send OrderUpdate to robot
            if (horizonUpdated)
            {
                await SendOrderToRobotAsync(robotId, orderState.Route, isInitial: false, baseCountBeforeRelease);
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"TryReleaseNextHorizonSegmentAsync: Error for robot {robotId}: {ex.Message}");
        }
    }

    private async Task<bool> SendOrderToRobotAsync(string robotId, RobotRoute route, bool isInitial, int baseCountBeforeRelease = -1)
    {
        try
        {
            // Get robot controller using service scope to avoid circular dependency
            IRobotController? robotController = null;
            try
            {
                using var scope = _serviceScopeFactory.CreateScope();
                var robotManagerService = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
                robotController = robotManagerService.GetRobotController(robotId);
            }
            catch (Exception ex)
            {
                _logger.Error($"SendOrderToRobotAsync: Error getting robot controller for {robotId}: {ex.Message}");
                return false;
            }

            if (robotController == null)
            {
                _logger.Warning($"SendOrderToRobotAsync: Robot controller not found for robot {robotId}");
                return false;
            }

            // Build nodes and edges
            var nodes = new List<Node>();
            var edges = new List<Edge>();

            if (isInitial)
            {
                // Initial order: send ALL Base + Horizon segments
                // Add Base segments (released = true)
                foreach (var segment in route.Base)
                {
                    if (segment.VdaNode != null)
                    {
                        var node = CloneNode(segment.VdaNode);
                        node.Released = true; // Base segments are always released
                        nodes.Add(node);
                    }

                    if (segment.VdaEdge != null)
                    {
                        var edge = CloneEdge(segment.VdaEdge);
                        edge.Released = true; // Base segments are always released
                        edges.Add(edge);
                    }
                }

                // Add Horizon segments (released = false)
                foreach (var segment in route.Horizon)
                {
                    if (segment.VdaNode != null)
                    {
                        var node = CloneNode(segment.VdaNode);
                        node.Released = false; // Horizon segments are not released
                        nodes.Add(node);
                    }

                    if (segment.VdaEdge != null)
                    {
                        var edge = CloneEdge(segment.VdaEdge);
                        edge.Released = false; // Horizon segments are not released
                        edges.Add(edge);
                    }
                }

                // Create initial Order
                var order = new OrderMsg
                {
                    OrderId = route.OrderId,
                    OrderUpdateId = 0,
                    SerialNumber = robotId,
                    Timestamp = DateTime.UtcNow,
                    Nodes = [.. nodes],
                    Edges = [.. edges]
                };

                var result = await robotController.SendOrderAsync(order);
                if (result.IsSuccess)
                {
                    route.OrderUpdateId = order.OrderUpdateId;
                    _logger.Info($"SendOrderToRobotAsync: Successfully sent initial Order (ID: {route.OrderId}, UpdateID: {order.OrderUpdateId}) to robot {robotId}");
                    return true;
                }
                else
                {
                    _logger.Warning($"SendOrderToRobotAsync: Failed to send initial order to robot {robotId}: {result.Message}");
                    return false;
                }
            }
            else
            {
                // OrderUpdate: Send stitching node + new Base segments + new Horizon segments
                // 1. Stitching node: last node of old Base (before release)
                // 2. New Base: segments that were just released from Horizon
                // 3. New Horizon: remaining segments in Horizon

                if (baseCountBeforeRelease < 0)
                {
                    // Fallback: use current Base.Count - 1 (assume only 1 segment was released)
                    // This shouldn't happen if called from TryReleaseNextHorizonSegmentAsync
                    baseCountBeforeRelease = Math.Max(0, route.Base.Count - 1);
                }

                // Get stitching node: last node of Base before release
                var oldBaseSegments = route.Base.Take(baseCountBeforeRelease).ToList();
                var lastOldBaseNode = oldBaseSegments.LastOrDefault(s => s.VdaNode != null);
                if (lastOldBaseNode?.VdaNode == null)
                {
                    _logger.Warning($"SendOrderToRobotAsync: Cannot create OrderUpdate for robot {robotId}: no old base node found");
                    return false;
                }

                // 1. Add stitching node (last node of old Base, released = true)
                var stitchingNode = CloneNode(lastOldBaseNode.VdaNode);
                stitchingNode.Released = true;
                nodes.Add(stitchingNode);

                // 2. Add new Base segments (segments that were just released, released = true)
                var newBaseSegments = route.Base.Skip(baseCountBeforeRelease).ToList();
                foreach (var segment in newBaseSegments)
                {
                    if (segment.VdaNode != null)
                    {
                        var node = CloneNode(segment.VdaNode);
                        node.Released = true; // New Base segments are released
                        nodes.Add(node);
                    }

                    if (segment.VdaEdge != null)
                    {
                        var edge = CloneEdge(segment.VdaEdge);
                        edge.Released = true; // New Base segments are released
                        edges.Add(edge);
                    }
                }

                // 3. Add new Horizon segments (remaining segments in Horizon, released = false)
                foreach (var segment in route.Horizon)
                {
                    if (segment.VdaNode != null)
                    {
                        var node = CloneNode(segment.VdaNode);
                        node.Released = false; // Horizon segments are not released
                        nodes.Add(node);
                    }

                    if (segment.VdaEdge != null)
                    {
                        var edge = CloneEdge(segment.VdaEdge);
                        edge.Released = false; // Horizon segments are not released
                        edges.Add(edge);
                    }
                }

                // Get current order to increment OrderUpdateId
                var currentOrder = robotController.Data.Order;
                var orderUpdateId = currentOrder?.OrderUpdateId ?? 0;

                var orderUpdate = new OrderMsg
                {
                    OrderId = route.OrderId,
                    OrderUpdateId = orderUpdateId + 1,
                    SerialNumber = robotId,
                    Timestamp = DateTime.UtcNow,
                    Nodes = [.. nodes],
                    Edges = [.. edges]
                };

                var result = await robotController.SendOrderAsync(orderUpdate);
                if (result.IsSuccess)
                {
                    route.OrderUpdateId = orderUpdate.OrderUpdateId;
                    _logger.Info($"SendOrderToRobotAsync: Successfully sent OrderUpdate (ID: {route.OrderId}, UpdateID: {orderUpdate.OrderUpdateId}) to robot {robotId}");
                    return true;
                }
                else
                {
                    _logger.Warning($"SendOrderToRobotAsync: Failed to send OrderUpdate to robot {robotId}: {result.Message}");
                    return false;
                }
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"SendOrderToRobotAsync: Error sending order to robot {robotId}: {ex.Message}");
            return false;
        }
    }

    private static Node CloneNode(Node source)
    {
        return new Node
        {
            NodeId = source.NodeId,
            SequenceId = source.SequenceId,
            Released = source.Released,
            NodeDescription = source.NodeDescription,
            NodePosition = source.NodePosition is null ? null : new NodePosition
            {
                X = source.NodePosition.X,
                Y = source.NodePosition.Y,
                Theta = source.NodePosition.Theta,
                AllowedDeviationXY = source.NodePosition.AllowedDeviationXY,
                AllowedDeviationTheta = source.NodePosition.AllowedDeviationTheta,
                MapId = source.NodePosition.MapId,
                MapDescription = source.NodePosition.MapDescription
            },
            Actions = source.Actions?.Select(a => CloneAction(a)).ToArray() ?? [] // Clone each action
        };
    }

    private static RobotNet.VDA5050.InstantAction.Action CloneAction(RobotNet.VDA5050.InstantAction.Action source)
    {
        return new RobotNet.VDA5050.InstantAction.Action
        {
            ActionType = source.ActionType,
            ActionId = source.ActionId,
            ActionDescription = source.ActionDescription,
            BlockingType = source.BlockingType,
            ActionParameters = source.ActionParameters?.Select(p => new RobotNet.VDA5050.InstantAction.ActionParameter
            {
                Key = p.Key,
                Value = p.Value
            }).ToArray() ?? null
        };
    }

    private static Edge CloneEdge(Edge source)
    {
        return new Edge
        {
            EdgeId = source.EdgeId,
            SequenceId = source.SequenceId,
            Released = source.Released,
            EdgeDescription = source.EdgeDescription,
            StartNodeId = source.StartNodeId,
            EndNodeId = source.EndNodeId,
            MaxSpeed = source.MaxSpeed,
            MaxHeight = source.MaxHeight,
            MinHeight = source.MinHeight,
            Orientation = source.Orientation,
            OrientationType = source.OrientationType,
            Direction = source.Direction,
            RotationAllowed = source.RotationAllowed,
            MaxRotationSpeed = source.MaxRotationSpeed,
            Length = source.Length,
            Trajectory = source.Trajectory == null ? null : new Trajectory
            {
                Degree = source.Trajectory.Degree,
                KnotVector = [.. source.Trajectory.KnotVector], // Clone array
                ControlPoints = [.. source.Trajectory.ControlPoints.Select(cp => new ControlPoint
                {
                    X = cp.X,
                    Y = cp.Y,
                    Weight = cp.Weight
                })]
            },
            Corridor = source.Corridor == null ? null : new Corridor
            {
                LeftWidth = source.Corridor.LeftWidth,
                RightWidth = source.Corridor.RightWidth,
                CorridorRefPoint = source.Corridor.CorridorRefPoint
            },
            Actions = source.Actions?.Select(a => CloneAction(a)).ToArray() ?? [] // Clone each action
        };
    }

    private void CheckOrderCompletion(string robotId, OrderACSState orderState, StateMsg stateMsg)
    {
        try
        {
            // Check if NodeStates and EdgeStates are empty
            var nodeStatesEmpty = stateMsg.NodeStates == null || stateMsg.NodeStates.Length == 0;
            var edgeStatesEmpty = stateMsg.EdgeStates == null || stateMsg.EdgeStates.Length == 0;

            if (!nodeStatesEmpty || !edgeStatesEmpty)
            {
                // Still processing, not completed yet
                return;
            }

            // NodeStates and EdgeStates are empty - check completion conditions
            var lastNodeInRoute = orderState.Route.FullRoute.LastOrDefault()?.VdaNode?.NodeId;
            var lastNodeId = stateMsg.LastNodeId;

            // Check if LastNodeId matches the last node in route
            bool isAtLastNode = lastNodeId == lastNodeInRoute;

            // Check actions on last node - get last node's actions from route
            bool allActionsFinished = true;
            bool hasActionFailed = false;

            if (stateMsg.ActionStates != null && stateMsg.ActionStates.Length > 0)
            {
                // Get actions from last node in route
                var lastNodeSegment = orderState.Route.FullRoute.LastOrDefault(s => s.VdaNode != null);
                if (lastNodeSegment?.VdaNode?.Actions != null && lastNodeSegment.VdaNode.Actions.Length > 0)
                {
                    // Check if all actions from last node are finished
                    var lastNodeActionIds = lastNodeSegment.VdaNode.Actions.Select(a => a.ActionId).ToHashSet();
                    var lastNodeActionStates = stateMsg.ActionStates
                        .Where(a => lastNodeActionIds.Contains(a.ActionId))
                        .ToList();

                    if (lastNodeActionStates.Count > 0)
                    {
                        foreach (var actionState in lastNodeActionStates)
                        {
                            if (actionState.ActionStatus == RobotNet.VDA5050.Type.ActionStatus.FAILED)
                            {
                                hasActionFailed = true;
                                allActionsFinished = false;
                                break;
                            }
                            else if (actionState.ActionStatus != RobotNet.VDA5050.Type.ActionStatus.FINISHED)
                            {
                                allActionsFinished = false;
                            }
                        }
                    }
                }
            }

            // Determine order status
            if (isAtLastNode && allActionsFinished)
            {
                if (orderState.Status != OrderStatus.IsCompleted) _logger.Info($"CheckOrderCompletion: Order completed successfully for robot {robotId}");
                // Order completed successfully
                orderState.Status = OrderStatus.IsCompleted;
                orderState.LastUpdated = DateTime.UtcNow;
            }
            else if (!isAtLastNode || hasActionFailed)
            {
                // Order error: not at last node or action failed
                orderState.Status = OrderStatus.IsError;
                if (!isAtLastNode)
                {
                    orderState.Error = $"Robot {robotId} finished at node {lastNodeId} but expected last node {lastNodeInRoute}";
                }
                else if (hasActionFailed)
                {
                    orderState.Error = $"Robot {robotId} has failed actions on last node {lastNodeId}";
                }
                orderState.LastUpdated = DateTime.UtcNow;
                _logger.Warning($"CheckOrderCompletion: Order error for robot {robotId}: {orderState.Error}");
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"CheckOrderCompletion: Error for robot {robotId}: {ex.Message}");
            orderState.Status = OrderStatus.IsError;
            orderState.Error = ex.Message;
        }
    }

    public RobotRoute? GetRobotRoute(string robotId)
    {
        if (_orderStates.TryGetValue(robotId, out var state))
        {
            return state.Route;
        }
        return null;
    }
}