Initial commit

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/ACS/OrderACSControl.cs

@@ -0,0 +1,923 @@
+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;
+    }
+}

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Enums/ConflictSeverity.cs

@@ -0,0 +1,25 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Severity levels for conflicts
+/// </summary>
+public enum ConflictSeverity
+{
+    /// <summary>
+    /// Low severity - can be resolved by waiting
+    /// </summary>
+    Low,
+
+    /// <summary>
+    /// Medium severity - needs route adjustment
+    /// </summary>
+    Medium,
+
+    /// <summary>
+    /// High severity - needs complete reroute
+    /// </summary>
+    High
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Enums/ConflictType.cs

@@ -0,0 +1,62 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Types of conflicts between robots
+/// </summary>
+public enum ConflictType
+{
+    /// <summary>
+    /// Xảy ra khi hai robot sử dụng cùng một cạnh (edge) trong biểu đồ đường đi (graph) 
+    /// trong khoảng thời gian trùng lặp, đồng thời lộ trình tiếp theo của robot chồng lên nhau.
+    /// </summary>
+    Confrontation,
+
+    /// <summary>
+    /// Xảy ra khi hai robot sử dụng cùng một cạnh (edge) trong biểu đồ đường đi (graph) 
+    /// trong khoảng thời gian trùng lặp nhưng lộ trình tiếp theo của 2 robot không chồng lấn lên nhau.
+    /// </summary>
+    Edge,
+
+    /// <summary>
+    /// Xảy ra khi hai robot chiếm cùng một nút (vertex/node) trong biểu đồ đường đi 
+    /// tại cùng một thời điểm hoặc trong khoảng thời gian trùng lặp.
+    /// </summary>
+    Vertex,
+
+    /// <summary>
+    /// Xảy ra khi hai robot ở quá gần nhau (dựa trên khoảng cách Euclidean) trong không gian liên tục, 
+    /// vi phạm khoảng cách an toàn (minDistance).
+    /// </summary>
+    Proximity,
+
+    /// <summary>
+    /// Xảy ra khi hai robot di chuyển qua một hành lang hẹp (thường được biểu diễn bằng một chuỗi cạnh hoặc node) 
+    /// theo hướng ngược nhau, dẫn đến tình trạng không thể vượt qua nhau.
+    /// </summary>
+    Corridor,
+
+    /// <summary>
+    /// Xảy ra khi hai robot có lộ trình giao nhau về mặt thời gian, nhưng không nhất thiết ở cùng một cạnh hoặc nút, 
+    /// mà ở các vị trí khiến chúng không thể di chuyển tiếp mà không va chạm.
+    /// </summary>
+    Temporal,
+
+    /// <summary>
+    /// Xảy ra khi hai robot cần xoay tại một điểm (thường là node) và không gian xoay bị chồng lấn, 
+    /// dẫn đến va chạm hoặc cản trở.
+    /// </summary>
+    Rotation,
+
+    /// <summary>
+    /// Xảy ra khi các robot cạnh tranh cho một tài nguyên chung (ví dụ: một khu vực làm việc, điểm sạc, hoặc thiết bị nâng)
+    /// </summary>
+    Resource,
+
+    /// <summary>
+    /// Xảy ra lỗi khi kiểm tra xung đột
+    /// </summary>
+    None
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Enums/PriorityReason.cs

@@ -0,0 +1,35 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Reasons for robot priority
+/// </summary>
+public enum PriorityReason
+{
+    /// <summary>
+    /// Emergency situation
+    /// </summary>
+    Emergency,
+
+    /// <summary>
+    /// High value order
+    /// </summary>
+    HighValueOrder,
+
+    /// <summary>
+    /// Time critical requirement
+    /// </summary>
+    TimeCritical,
+
+    /// <summary>
+    /// Manual override by user
+    /// </summary>
+    ManualOverride,
+
+    /// <summary>
+    /// Default priority
+    /// </summary>
+    Default
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Enums/ReservationStatus.cs

@@ -0,0 +1,25 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Status of edge reservations
+/// </summary>
+public enum ReservationStatus
+{
+    /// <summary>
+    /// Edge is reserved but not yet in use
+    /// </summary>
+    Reserved,
+
+    /// <summary>
+    /// Robot is currently using the edge
+    /// </summary>
+    InUse,
+
+    /// <summary>
+    /// Reservation has been released
+    /// </summary>
+    Released
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Enums/ResolutionAction.cs

@@ -0,0 +1,20 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Specific actions for conflict resolution
+/// </summary>
+public enum ResolutionAction
+{
+    /// <summary>
+    /// Wait at node (add wait node to Horizon)
+    /// </summary>
+    Wait,
+
+    /// <summary>
+    /// Reroute (calculate new route for Horizon)
+    /// </summary>
+    Reroute
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Enums/ResolutionStrategy.cs

@@ -0,0 +1,20 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Strategies for resolving conflicts
+/// </summary>
+public enum ResolutionStrategy
+{
+    /// <summary>
+    /// Robot waits at node (can only add wait node to Horizon, NOT to Base)
+    /// </summary>
+    WaitAtNode,
+
+    /// <summary>
+    /// Robot reroutes (can only reroute Horizon, NOT Base)
+    /// </summary>
+    Reroute
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Helpers/MapDataConverter.cs

@@ -0,0 +1,82 @@
+using RobotNet.VDA5050;
+using RobotNet.VDA5050.Order;
+using RobotNet10.GlobalPathPlanner.Model;
+using System.Text.Json;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Helpers;
+
+/// <summary>
+/// Helper class to convert MapManager entities to GlobalPathPlanner models
+/// </summary>
+public static class MapDataConverter
+{
+    /// <summary>
+    /// Convert Node (MapManager) to GlobalNode (PathPlanner)
+    /// </summary>
+    public static GlobalNode ToGlobalNode(MapManager.Data.Node node, Guid mapId)
+    {
+        return new GlobalNode
+        {
+            Id = node.Id,
+            MapId = mapId,
+            Name = node.NodeName ?? node.NodeId,
+            X = node.X,
+            Y = node.Y,
+            Orientation = Orientation.NONE // Default, can be enhanced later
+        };
+    }
+
+    /// <summary>
+    /// Convert Edge (MapManager) to GlobalEdge (PathPlanner)
+    /// </summary>
+    public static GlobalEdge ToGlobalEdge(MapManager.Data.Edge edge, Guid mapId, Guid vehicleId)
+    {
+        var globalEdge = new GlobalEdge
+        {
+            Id = edge.Id,
+            MapId = mapId,
+            StartNodeId = edge.StartNodeId,
+            EndNodeId = edge.EndNodeId,
+            Degree = 1,
+            ControlPoint1X = 0.0,
+            ControlPoint1Y = 0.0,
+            ControlPoint2X = 0.0,
+            ControlPoint2Y = 0.0
+        };
+        // Get edge vehicle properties for trajectory (if available)
+        var vehicleProperty = edge.VehicleProperties?.FirstOrDefault(prop => prop.VehicleTypeId == vehicleId);
+        if(vehicleProperty is not null)
+        {
+            // Use trajectory fields directly from Entity
+            if (vehicleProperty.TrajectoryDegree.HasValue)
+            {
+                globalEdge.Degree = vehicleProperty.TrajectoryDegree.Value;
+            }
+            
+            globalEdge.ControlPoint1X = vehicleProperty.TrajectoryControlPoint1X ?? 0.0;
+            globalEdge.ControlPoint1Y = vehicleProperty.TrajectoryControlPoint1Y ?? 0.0;
+            globalEdge.ControlPoint2X = vehicleProperty.TrajectoryControlPoint2X ?? 0.0;
+            globalEdge.ControlPoint2Y = vehicleProperty.TrajectoryControlPoint2Y ?? 0.0;
+        }
+        return globalEdge;
+    }
+
+    /// <summary>
+    /// Convert list of Nodes to GlobalNodes
+    /// </summary>
+    public static GlobalNode[] ToGlobalNodes(IEnumerable<MapManager.Data.Node> nodes, Guid mapId)
+    {
+        return [.. nodes.Select(n => ToGlobalNode(n, mapId))];
+    }
+
+    /// <summary>
+    /// Convert list of Edges to GlobalEdges
+    /// </summary>
+    public static GlobalEdge[] ToGlobalEdges(IEnumerable<MapManager.Data.Edge> edges, Guid mapId, Guid vehicleId)
+    {
+        return [.. edges.Select(e => ToGlobalEdge(e, mapId, vehicleId))];
+    }
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Helpers/RouteConverter.cs

@@ -0,0 +1,462 @@
+using RobotNet.VDA5050;
+using RobotNet.VDA5050.Order;
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+using RobotNet10.GlobalPathPlanner.Model;
+using RobotNet10.MapManager.Data;
+using Edge = RobotNet10.MapManager.Data.Edge;
+using Node = RobotNet10.MapManager.Data.Node;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Helpers;
+
+/// <summary>
+/// Helper class to convert path planner results to RobotRoute
+/// </summary>
+public static class RouteConverter
+{
+    /// <summary>
+    /// Convert path planner result (GlobalNode[], GlobalEdge[]) to RobotRoute
+    /// </summary>
+    public static RobotRoute ConvertToRobotRoute(
+        string robotId,
+        GlobalNode[] pathNodes,
+        GlobalEdge[] pathEdges,
+        List<Node> allNodes,
+        List<Edge> allEdges,
+        double? lastAngle,
+        object? logger = null, // Accept any logger type for flexibility
+        Guid? vehicleTypeId = null, // VehicleTypeId to get VehicleProperties
+        string? mapId = null) // MapId (LevelId as string) for NodePosition
+    {
+        // Log virtual node creation
+        if (logger != null)
+        {
+            try
+            {
+                var loggerType = logger.GetType();
+                var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
+                infoMethod?.Invoke(logger, [$"ConvertToRobotRoute: {pathNodes.Length} node, {pathEdges.Length}"]);
+            }
+            catch { }
+        }
+        var route = new RobotRoute
+        {
+            RobotId = robotId,
+            OrderId = Guid.NewGuid().ToString(), // Generate new order ID
+            OrderUpdateId = 0,
+            CreatedAt = DateTime.UtcNow,
+            LastUpdated = DateTime.UtcNow
+        };
+
+        var segments = new List<RouteSegment>();
+        var nodeMap = allNodes.ToDictionary(n => n.Id, n => n);
+        var edgeMap = allEdges.ToDictionary(e => e.Id, e => e);
+
+        // Get LevelId from first available node (for creating virtual nodes/edges)
+        var levelId = allNodes.FirstOrDefault()?.LevelId ?? Guid.Empty;
+
+        // Create segments following VDA5050 pattern:
+        // Node (seq 0), Edge (seq 1), Node (seq 2), Edge (seq 3), ..., Node (seq N)
+        // Route: n nodes, n-1 edges
+
+        int sequenceId = 0;
+
+        for (int i = 0; i < pathNodes.Length; i++)
+        {
+            var globalNode = pathNodes[i];
+
+            // Check if node exists in map, if not (first node when robot is on edge), create virtual node
+            if (!nodeMap.TryGetValue(globalNode.Id, out Node? node))
+            {
+                // This is a virtual node created by A* when robot is on an edge
+                // Only the first node can be virtual
+                if (i == 0)
+                {
+                    // Get LevelId from next node if available, otherwise use from allNodes
+                    if (pathNodes.Length > 1 && nodeMap.TryGetValue(pathNodes[1].Id, out var nextNode))
+                    {
+                        levelId = nextNode.LevelId;
+                    }
+
+                    // Create virtual node from GlobalNode
+                    node = new Node
+                    {
+                        Id = globalNode.Id,
+                        LevelId = levelId,
+                        NodeId = globalNode.Id.ToString(),
+                        NodeName = "Virtual Start Node",
+                        X = globalNode.X,
+                        Y = globalNode.Y
+                    };
+
+                    // Log virtual node creation
+                    if (logger != null)
+                    {
+                        try
+                        {
+                            var loggerType = logger.GetType();
+                            var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
+                            infoMethod?.Invoke(logger, [$"Created virtual start node {globalNode.Id} at ({globalNode.X:F2}, {globalNode.Y:F2}) - robot is on edge"]);
+                        }
+                        catch { }
+                    }
+                }
+                else
+                {
+                    // Node not found and not first node - this is an error
+                    if (logger != null)
+                    {
+                        try
+                        {
+                            var loggerType = logger.GetType();
+                            var errorMethod = loggerType.GetMethod("Error", [typeof(string)]);
+                            errorMethod?.Invoke(logger, [$"Node {globalNode.Id} not found in map at index {i}"]);
+                        }
+                        catch
+                        {
+                            // Ignore if Error method doesn't exist
+                        }
+                    }
+                    throw new InvalidOperationException($"Node {globalNode.Id} not found in map at index {i}");
+                }
+            }
+
+            // Get NodeVehicleProperty for this vehicle type if available
+            NodeVehicleProperty? nodeVehicleProperty = null;
+            if (vehicleTypeId.HasValue && node.VehicleProperties != null)
+            {
+                nodeVehicleProperty = node.VehicleProperties.FirstOrDefault(vp => vp.VehicleTypeId == vehicleTypeId.Value);
+            }
+
+            // Parse actions from NodeVehicleProperty
+            RobotNet.VDA5050.InstantAction.Action[] nodeActions = [];
+            if (!string.IsNullOrEmpty(nodeVehicleProperty?.Actions))
+            {
+                try
+                {
+                    var parsedActions = System.Text.Json.JsonSerializer.Deserialize<RobotNet.VDA5050.InstantAction.ActionLIF[]>(nodeVehicleProperty.Actions, JsonOptionExtends.Read);
+                    if (parsedActions != null)
+                    {
+                        nodeActions = [..parsedActions.Where(a => a.RequirementType == RobotNet.VDA5050.Type.RequirementType.REQUIRED).Select(a => new RobotNet.VDA5050.InstantAction.Action()
+                        {
+                            ActionId = Guid.NewGuid().ToString(),
+                            ActionDescription = a.ActionDescription,
+                            ActionParameters = [..a.ActionParameters],
+                            BlockingType = a.BlockingType,
+                            ActionType = a.ActionType
+                        })];
+                    }
+                }
+                catch { }
+            }
+
+            // Create VDA5050 Node with full information
+            var vdaNode = new RobotNet.VDA5050.Order.Node
+            {
+                NodeId = node.Id.ToString(),
+                SequenceId = sequenceId,
+                Released = false,
+                NodeDescription = node.NodeDescription ?? string.Empty,
+                NodePosition = new NodePosition
+                {
+                    X = node.X,
+                    Y = node.Y,
+                    Theta = i == pathNodes.Length - 1 && lastAngle.HasValue ? lastAngle.Value : nodeVehicleProperty?.Theta, 
+                    AllowedDeviationXY = nodeVehicleProperty?.AllowedDeviationXY,
+                    AllowedDeviationTheta = nodeVehicleProperty?.AllowedDeviationTheta,
+                    MapId = node.MapId ?? mapId ?? string.Empty
+                },
+                Actions = nodeActions
+            };
+
+            // Add node segment (even sequence IDs) with VDA5050 Node
+            var nodeSegment = new RouteSegment
+            {
+                NodeId = node.Id,
+                EdgeId = null,
+                StartNodeId = node.Id,
+                EndNodeId = null,
+                Released = false,
+                VdaNode = vdaNode,
+                VdaEdge = null
+            };
+
+            segments.Add(nodeSegment);
+            sequenceId++;
+
+            // Add edge segment if not the last node
+            if (i < pathNodes.Length - 1 && i < pathEdges.Length)
+            {
+                var globalEdge = pathEdges[i];
+
+                // Find edge by Id or by StartNodeId and EndNodeId
+                Edge? edge = allEdges.FirstOrDefault(e => e.StartNodeId == globalEdge.StartNodeId &&
+                                                          e.EndNodeId == globalEdge.EndNodeId);
+                if (edge is null && edgeMap.TryGetValue(globalEdge.Id, out Edge? value))
+                {
+                    edge = value;
+                    if(i == 0)
+                    {
+                        edge.StartNodeId = globalEdge.StartNodeId;
+                        edge.EndNodeId = globalEdge.EndNodeId;
+                        edge.VehicleProperties = [];
+                    }
+                }
+
+                // If edge not found and this is the first edge (i == 0), create virtual edge
+                if (edge == null && i == 0)
+                {
+                    // Create virtual edge from GlobalEdge
+                    edge = new Edge
+                    {
+                        Id = globalEdge.Id,
+                        LevelId = levelId,
+                        EdgeId = globalEdge.Id.ToString(), // Virtual edge identifier
+                        StartNodeId = globalEdge.StartNodeId, // Current node (may be virtual)
+                        EndNodeId = globalEdge.EndNodeId,
+                        EdgeDescription = "Virtual Start Edge",
+                    };
+
+                    // Log virtual edge creation
+                    if (logger != null)
+                    {
+                        try
+                        {
+                            var loggerType = logger.GetType();
+                            var infoMethod = loggerType.GetMethod("Info", [typeof(string)]);
+                            infoMethod?.Invoke(logger, [$"Created virtual start edge from node {globalEdge.StartNodeId} to node {globalEdge.EndNodeId} - robot is on edge"]);
+                        }
+                        catch { }
+                    }
+                }
+
+                if (edge != null)
+                {
+                    // Get EdgeVehicleProperty for this vehicle type if available
+                    EdgeVehicleProperty? edgeVehicleProperty = null;
+                    if (vehicleTypeId.HasValue && edge.VehicleProperties != null)
+                    {
+                        edgeVehicleProperty = edge.VehicleProperties.FirstOrDefault(vp => vp.VehicleTypeId == vehicleTypeId.Value);
+                    }
+
+                    // Calculate edge length (Euclidean distance between start and end nodes)
+                    var dx = pathNodes[i].X - pathNodes[i + 1].X;
+                    var dy = pathNodes[i].Y - pathNodes[i + 1].Y;
+                    double edgeLength = Math.Sqrt(dx * dx + dy * dy);
+
+                    // Build trajectory from EdgeVehicleProperty fields
+                    var startNode = pathNodes[i];
+                    var endNode = pathNodes[i + 1];
+                    Trajectory? trajectory = null;
+                    if (edgeVehicleProperty?.TrajectoryDegree.HasValue == true)
+                    {
+                        var degree = edgeVehicleProperty.TrajectoryDegree.Value;
+
+                        // Build control points array based on degree
+                        List<ControlPoint> controlPoints =
+                        [
+                            // Always add start node as first control point
+                            new() {
+                                X = startNode.X,
+                                Y = startNode.Y,
+                                Weight = 1.0
+                            }
+                        ];
+                        
+                        // Add control point 1 for degree 2 and 3
+                        if (degree >= 2 && edgeVehicleProperty.TrajectoryControlPoint1X.HasValue && edgeVehicleProperty.TrajectoryControlPoint1Y.HasValue)
+                        {
+                            controlPoints.Add(new ControlPoint
+                            {
+                                X = edgeVehicleProperty.TrajectoryControlPoint1X.Value,
+                                Y = edgeVehicleProperty.TrajectoryControlPoint1Y.Value,
+                                Weight = 1.0
+                            });
+                        }
+                        else if (degree >= 2)
+                        {
+                            // Default: midpoint between start and end
+                            controlPoints.Add(new ControlPoint
+                            {
+                                X = (startNode.X + endNode.X) / 2.0,
+                                Y = (startNode.Y + endNode.Y) / 2.0,
+                                Weight = 1.0
+                            });
+                        }
+                        
+                        // Add control point 2 for degree 3
+                        if (degree >= 3 && edgeVehicleProperty.TrajectoryControlPoint2X.HasValue && edgeVehicleProperty.TrajectoryControlPoint2Y.HasValue)
+                        {
+                            controlPoints.Add(new ControlPoint
+                            {
+                                X = edgeVehicleProperty.TrajectoryControlPoint2X.Value,
+                                Y = edgeVehicleProperty.TrajectoryControlPoint2Y.Value,
+                                Weight = 1.0
+                            });
+                        }
+                        else if (degree >= 3)
+                        {
+                            // Default: one-third point from start
+                            controlPoints.Add(new ControlPoint
+                            {
+                                X = startNode.X + (endNode.X - startNode.X) / 3.0,
+                                Y = startNode.Y + (endNode.Y - startNode.Y) / 3.0,
+                                Weight = 1.0
+                            });
+                        }
+                        
+                        // Always add end node as last control point
+                        controlPoints.Add(new ControlPoint
+                        {
+                            X = endNode.X,
+                            Y = endNode.Y,
+                            Weight = 1.0
+                        });
+                        
+                        // Build knot vector based on degree
+                        double[] knotVector = degree switch
+                        {
+                            1 => [0, 0, 1, 1],
+                            2 => [0, 0, 0, 1, 1, 1],
+                            3 => [0, 0, 0, 0, 1, 1, 1, 1],
+                            _ => [0, 0, 1, 1] // Default to degree 1
+                        };
+                        
+                        trajectory = new Trajectory
+                        {
+                            Degree = degree,
+                            KnotVector = knotVector,
+                            ControlPoints = [.. controlPoints]
+                        };
+                    }
+
+                    // Build corridor from EdgeVehicleProperty fields
+                    Corridor? corridor = null;
+                    if (edgeVehicleProperty != null && 
+                        (edgeVehicleProperty.CorridorLeftWidth.HasValue || 
+                         edgeVehicleProperty.CorridorRightWidth.HasValue || 
+                         edgeVehicleProperty.CorridorRefPoint.HasValue))
+                    {
+                        corridor = new Corridor
+                        {
+                            LeftWidth = edgeVehicleProperty.CorridorLeftWidth ?? 0.0,
+                            RightWidth = edgeVehicleProperty.CorridorRightWidth ?? 0.0,
+                            CorridorRefPoint = edgeVehicleProperty.CorridorRefPoint ?? RobotNet.VDA5050.Type.CorridorRefPoint.KINEMATICCENTER
+                        };
+                    }
+
+                    // Parse actions from EdgeVehicleProperty
+                    RobotNet.VDA5050.InstantAction.Action[] edgeActions = [];
+                    if (!string.IsNullOrEmpty(edgeVehicleProperty?.Actions))
+                    {
+                        try
+                        {
+                            var parsedActions = System.Text.Json.JsonSerializer.Deserialize<RobotNet.VDA5050.InstantAction.ActionLIF[]?>(edgeVehicleProperty.Actions, JsonOptionExtends.Read);
+                            if (parsedActions != null)
+                            {
+                                edgeActions = [..parsedActions.Where(a => a.RequirementType == RobotNet.VDA5050.Type.RequirementType.REQUIRED).Select(a => new RobotNet.VDA5050.InstantAction.Action()
+                                {
+                                    ActionId = Guid.NewGuid().ToString(),
+                                    ActionDescription = a.ActionDescription,
+                                    ActionParameters = [..a.ActionParameters],
+                                    BlockingType = a.BlockingType,
+                                    ActionType = a.ActionType
+                                })];
+                            }
+                        }
+                        catch
+                        {
+                            // Ignore parse errors, use empty array
+                        }
+                    }
+
+                    // tính toán orientation cho robot
+
+                    // Create VDA5050 Edge with full information
+                    var vdaEdge = new RobotNet.VDA5050.Order.Edge
+                    {
+                        EdgeId = edge.Id.ToString(),
+                        SequenceId = sequenceId,
+                        Released = false,
+                        EdgeDescription = edge.EdgeDescription,
+                        StartNodeId = edge.StartNodeId.ToString(),
+                        EndNodeId = edge.EndNodeId.ToString(),
+                        MaxSpeed = edgeVehicleProperty?.MaxSpeed,
+                        MaxHeight = edgeVehicleProperty?.MaxHeight,
+                        MinHeight = edgeVehicleProperty?.MinHeight ,
+                        Orientation = startNode.Orientation == Orientation.FORWARD ? 0 : startNode.Orientation == Orientation.BACKWARD ? Math.PI : null,
+                        OrientationType = RobotNet.VDA5050.Type.OrientationType.TANGENTIAL,
+                        Direction = string.Empty, // Not in EdgeVehicleProperty
+                        RotationAllowed = edgeVehicleProperty?.RotationAllowed ,
+                        MaxRotationSpeed = edgeVehicleProperty?.MaxRotationSpeed,
+                        Length = edgeLength,
+                        Trajectory = trajectory,
+                        Corridor = corridor,
+                        Actions = edgeActions
+                    };
+
+                    // Add edge segment (odd sequence IDs) with VDA5050 Edge
+                    var edgeSegment = new RouteSegment
+                    {
+                        NodeId = edge.EndNodeId, // Target node of this edge
+                        EdgeId = edge.Id,
+                        StartNodeId = edge.StartNodeId,
+                        EndNodeId = edge.EndNodeId,
+                        Released = false,
+                        VdaNode = null, // Edge segment doesn't have node
+                        VdaEdge = vdaEdge
+                    };
+
+                    segments.Add(edgeSegment);
+                    sequenceId++;
+                }
+                else
+                {
+                    // Edge not found and not first edge - this is an error
+                    if (logger != null)
+                    {
+                        try
+                        {
+                            var loggerType = logger.GetType();
+                            var warningMethod = loggerType.GetMethod("Warning", [typeof(string)]);
+                            warningMethod?.Invoke(logger, [$"Edge not found for path segment {i}: StartNodeId={globalEdge.StartNodeId}, EndNodeId={globalEdge.EndNodeId}"]);
+                        }
+                        catch
+                        {
+                            // Ignore if Warning method doesn't exist
+                        }
+                    }
+                }
+            }
+        }
+
+        route.FullRoute = segments;
+        route.CurrentSegmentIndex = 0;
+
+        return route;
+    }
+
+    /// <summary>
+    /// Split route into Base and Horizon
+    /// </summary>
+    public static void SplitRouteIntoBaseAndHorizon(RobotRoute route, int baseSegmentCount)
+    {
+        if (route.FullRoute.Count == 0)
+            return;
+
+        // Ensure baseSegmentCount doesn't exceed available segments
+        var actualBaseCount = Math.Min(baseSegmentCount, route.FullRoute.Count - 1);
+        if (actualBaseCount < 1)
+            actualBaseCount = 1; // At least 1 segment in base
+
+        // Split: Base gets first N segments, Horizon gets the rest
+        route.Base = [.. route.FullRoute.Take(actualBaseCount)];
+        route.Horizon = [.. route.FullRoute.Skip(actualBaseCount)];
+
+        // Mark base segments as released
+        foreach (var segment in route.Base)
+        {
+            segment.Released = true;
+        }
+    }
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/IOrderControlService.cs

@@ -0,0 +1,20 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl;
+
+public enum OrderStatus
+{
+    IsError,
+    IsCompleted,
+    IsProccessing,
+    IsCanceled,
+    Empty
+}
+
+
+public interface IOrderControlService
+{
+    OrderStatus GetRobotOrderStatus(string robotId);
+    RobotRoute? GetRobotRoute(string robotId);
+    Task<bool> CreateRobotOrderAsync(string robotId, RobotRoute route);
+}

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/ITrafficControlService.cs

@@ -0,0 +1,239 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl;
+
+/// <summary>
+/// Service for traffic control and conflict management between robots
+/// </summary>
+public interface ITrafficControlService
+{
+    /// <summary>
+    /// Plans a route from start node to goal node for a robot
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="startNodeId">Start node ID</param>
+    /// <param name="goalNodeId">Goal node ID</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>RobotRoute if successful, null otherwise</returns>
+    Task<RobotRoute?> PlanRouteAsync(
+        string robotId,
+        Guid startNodeId,
+        Guid goalNodeId,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Plans a route with optional constraints (angle, startDirection, finalDirection)
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="startNodeId">Start node ID</param>
+    /// <param name="goalNodeId">Goal node ID</param>
+    /// <param name="goalAngle">Optional goal angle in degrees</param>
+    /// <param name="startDirection">Optional start direction constraint</param>
+    /// <param name="finalDirection">Optional final direction constraint</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>RobotRoute if successful, null otherwise</returns>
+    Task<RobotRoute?> PlanRouteAsync(
+        string robotId,
+        Guid startNodeId,
+        Guid goalNodeId,
+        double? goalAngle,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Plans a route from current position (x, y, theta) to goal node
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="x">Current X position</param>
+    /// <param name="y">Current Y position</param>
+    /// <param name="theta">Current orientation in degrees</param>
+    /// <param name="goalNodeId">Goal node ID</param>
+    /// <param name="goalAngle">Optional goal angle in degrees</param>
+    /// <param name="startDirection">Optional start direction constraint</param>
+    /// <param name="finalDirection">Optional final direction constraint</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>RobotRoute if successful, null otherwise</returns>
+    Task<RobotRoute?> PlanRouteFromPositionAsync(
+        string robotId,
+        double x,
+        double y,
+        double theta,
+        Guid goalNodeId,
+        double? goalAngle,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Plan route from current position (x, y, theta) to goal node
+    /// </summary>
+    Task<RobotRoute?> PlanRouteFromPositionACSTrafficAsync(
+        string robotId,
+        double x,
+        double y,
+        double theta,
+        Guid goalNodeId,
+        double? goalAngle,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Detects all conflicts between active robots
+    /// </summary>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>List of detected conflicts</returns>
+    Task<List<Conflict>> DetectConflictsAsync(CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Resolves a conflict
+    /// </summary>
+    /// <param name="conflict">Conflict to resolve</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>True if resolved successfully</returns>
+    Task<bool> ResolveConflictAsync(Conflict conflict, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Releases horizon segments into base when safe
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="segmentCount">Number of segments to release</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>True if released successfully</returns>
+    Task<bool> ReleaseHorizonSegmentAsync(
+        string robotId,
+        int segmentCount,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Updates robot route (typically for rerouting)
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="newRoute">New route</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>True if updated successfully</returns>
+    Task<bool> UpdateRobotRouteAsync(
+        string robotId,
+        RobotRoute newRoute,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Gets all active routes for all robots
+    /// </summary>
+    /// <returns>Dictionary of robot ID to RobotRoute</returns>
+    Task<Dictionary<string, RobotRoute>> GetAllActiveRoutesAsync();
+
+    /// <summary>
+    /// Gets route for a specific robot
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <returns>RobotRoute if exists, null otherwise</returns>
+    Task<RobotRoute?> GetRobotRouteAsync(string robotId);
+
+    /// <summary>
+    /// Sets priority for a robot
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="priority">Priority information</param>
+    /// <returns>True if set successfully</returns>
+    Task<bool> SetRobotPriorityAsync(string robotId, RobotPriority priority);
+
+    /// <summary>
+    /// Gets priority for a robot
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <returns>RobotPriority (default if not set)</returns>
+    Task<RobotPriority> GetRobotPriorityAsync(string robotId);
+
+    /// <summary>
+    /// Removes priority for a robot (resets to default)
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <returns>True if removed successfully</returns>
+    Task<bool> RemoveRobotPriorityAsync(string robotId);
+
+    /// <summary>
+    /// Evaluates conflicts for resolution optimization
+    /// </summary>
+    /// <param name="conflicts">List of conflicts to evaluate</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>Sorted list of conflicts by priority</returns>
+    Task<List<Conflict>> EvaluateConflictsForResolutionAsync(
+        List<Conflict> conflicts,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Sends OrderUpdate to robot with new segments
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="newSegments">New segments to add to order</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>True if sent successfully</returns>
+    Task<bool> SendOrderUpdateAsync(
+        string robotId,
+        List<RouteSegment> newSegments,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Reserves edges for a robot's route segments
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="orderId">Order ID</param>
+    /// <param name="segments">Route segments containing edges to reserve</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>True if reserved successfully</returns>
+    Task<bool> ReserveEdgesAsync(
+        string robotId,
+        string orderId,
+        List<RouteSegment> segments,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Gets all reservations for a specific edge
+    /// </summary>
+    /// <param name="edgeId">Edge ID</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>List of edge reservations</returns>
+    Task<List<EdgeReservation>> GetEdgeReservationsAsync(
+        Guid edgeId,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Checks if an edge is available during a time period
+    /// </summary>
+    /// <param name="edgeId">Edge ID</param>
+    /// <param name="fromTime">Start time</param>
+    /// <param name="toTime">End time</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>True if available, false otherwise</returns>
+    Task<bool> IsEdgeAvailableAsync(
+        Guid edgeId,
+        DateTime fromTime,
+        DateTime toTime,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Releases all reservations for a robot's order
+    /// </summary>
+    /// <param name="robotId">Robot ID</param>
+    /// <param name="orderId">Order ID</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    /// <returns>True if released successfully</returns>
+    Task<bool> ReleaseReservationsAsync(
+        string robotId,
+        string orderId,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Checks and releases horizon segments for robots near end of Base
+    /// </summary>
+    /// <param name="robotId">Optional: specific robot ID, null for all robots</param>
+    /// <param name="cancellationToken">Cancellation token</param>
+    Task CheckAndReleaseHorizonsAsync(
+        string? robotId = null,
+        CancellationToken cancellationToken = default);
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/Conflict.cs

@@ -0,0 +1,60 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Represents a conflict between robots
+/// </summary>
+public class Conflict
+{
+    /// <summary>
+    /// Type of conflict
+    /// </summary>
+    public ConflictType Type { get; set; }
+
+    /// <summary>
+    /// List of robot IDs involved in this conflict
+    /// </summary>
+    public List<string> InvolvedRobots { get; set; } = new();
+
+    /// <summary>
+    /// List of conflicting edge IDs
+    /// </summary>
+    public List<Guid> ConflictingEdges { get; set; } = new();
+
+    /// <summary>
+    /// List of conflicting node IDs
+    /// </summary>
+    public List<Guid> ConflictingNodes { get; set; } = new();
+
+    /// <summary>
+    /// When the conflict was detected
+    /// </summary>
+    public DateTime DetectedAt { get; set; } = DateTime.UtcNow;
+
+    /// <summary>
+    /// Severity of the conflict
+    /// </summary>
+    public ConflictSeverity Severity { get; set; }
+
+    /// <summary>
+    /// Resolution strategy for this conflict
+    /// </summary>
+    public ConflictResolution? Resolution { get; set; }
+
+    /// <summary>
+    /// Additional details about the conflict
+    /// </summary>
+    public Dictionary<string, object> ConflictDetails { get; set; } = new();
+
+    /// <summary>
+    /// Estimated number of new conflicts that may arise from resolving this conflict
+    /// </summary>
+    public int EstimatedNewConflicts { get; set; }
+
+    /// <summary>
+    /// List of other conflict keys that can be resolved by resolving this conflict
+    /// </summary>
+    public List<string> CanResolveConflicts { get; set; } = new();
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/ConflictResolution.cs

@@ -0,0 +1,35 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Represents a resolution strategy for a conflict
+/// </summary>
+public class ConflictResolution
+{
+    /// <summary>
+    /// Resolution strategy (WaitAtNode or Reroute)
+    /// </summary>
+    public ResolutionStrategy Strategy { get; set; }
+
+    /// <summary>
+    /// Robot ID that needs to take action
+    /// </summary>
+    public string ActionRobotId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Specific action to take
+    /// </summary>
+    public ResolutionAction Action { get; set; }
+
+    /// <summary>
+    /// Wait until this time (if action is Wait)
+    /// </summary>
+    public DateTime? WaitUntil { get; set; }
+
+    /// <summary>
+    /// New route (if action is Reroute)
+    /// </summary>
+    public RobotRoute? NewRoute { get; set; }
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/EdgeReservation.cs

@@ -0,0 +1,55 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Represents an edge reservation for a robot
+/// </summary>
+public class EdgeReservation
+{
+    /// <summary>
+    /// Edge ID (Guid from database)
+    /// </summary>
+    public Guid EdgeId { get; set; }
+
+    /// <summary>
+    /// Edge ID string (VDMA LIF edgeId)
+    /// </summary>
+    public string EdgeIdString { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Start Node ID of the edge
+    /// </summary>
+    public Guid StartNodeId { get; set; }
+
+    /// <summary>
+    /// End Node ID of the edge
+    /// </summary>
+    public Guid EndNodeId { get; set; }
+
+    /// <summary>
+    /// Robot ID that reserved this edge
+    /// </summary>
+    public string RobotId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Order ID associated with this reservation
+    /// </summary>
+    public string OrderId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// When the reservation was created
+    /// </summary>
+    public DateTime ReservedAt { get; set; } = DateTime.UtcNow;
+
+    /// <summary>
+    /// When the reservation expires (based on edge length and robot speed)
+    /// </summary>
+    public DateTime ReservedUntil { get; set; }
+
+    /// <summary>
+    /// Reservation status
+    /// </summary>
+    public ReservationStatus Status { get; set; } = ReservationStatus.Reserved;
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/OrderACSState.cs

@@ -0,0 +1,76 @@
+using RobotNet10.FleetManager.Services.TrafficControl.ACS;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// State information for an order managed by ACS Order Control
+/// </summary>
+public class OrderACSState
+{
+    /// <summary>
+    /// Robot ID
+    /// </summary>
+    public string RobotId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Order status
+    /// </summary>
+    public OrderStatus Status { get; set; } = OrderStatus.IsProccessing;
+
+    /// <summary>
+    /// Full route (Base + Horizon)
+    /// </summary>
+    public RobotRoute Route { get; set; } = new();
+
+    /// <summary>
+    /// Set of zone IDs that have been successfully requested (RequestIn completed)
+    /// </summary>
+    public HashSet<string> ZoneRequestInCompleted { get; set; } = [];
+
+    /// <summary>
+    /// Set of zone IDs that have been successfully requested (RequestOut completed)
+    /// </summary>
+    public HashSet<string> ZoneRequestOutCompleted { get; set; } = [];
+
+    /// <summary>
+    /// Set of region IDs needs to be successfully requested. (IN)
+    /// </summary>
+    public HashSet<string> ZoneRequestInCompleting { get; set; } = [];
+
+    /// <summary>
+    /// Set of region IDs needs to be successfully requested. (OUT)
+    /// </summary>
+    public HashSet<string> ZoneRequestOutCompleting { get; set; } = [];
+
+    /// <summary>
+    /// Error message if status is IsError
+    /// </summary>
+    public string? Error { get; set; }
+
+    /// <summary>
+    /// List of nodes that are mapped to ACS zones for RequestIn (in order of appearance in route)
+    /// Each entry contains: (NodeIdString, ZoneId)
+    /// </summary>
+    public List<(string NodeIdString, string ZoneId)> InMappedNodes { get; set; } = [];
+
+    /// <summary>
+    /// List of nodes that are mapped to ACS zones for RequestOut (in order of appearance in route)
+    /// Each entry contains: (NodeIdString, ZoneId)
+    /// </summary>
+    public List<(string NodeIdString, string ZoneId)> OutMappedNodes { get; set; } = [];
+
+    /// <summary>
+    /// Index of the current IN mapped node being processed
+    /// </summary>
+    public int CurrentInMappedNodeIndex { get; set; } = 0;
+
+    /// <summary>
+    /// Timestamp when order was created
+    /// </summary>
+    public DateTime CreatedAt { get; set; } = DateTime.UtcNow;
+
+    /// <summary>
+    /// Last update timestamp
+    /// </summary>
+    public DateTime LastUpdated { get; set; } = DateTime.UtcNow;
+}

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/PathPlanningMethod.cs

@@ -0,0 +1,27 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Path planning method to use for IPathPlanner
+/// </summary>
+public enum PathPlanningMethod
+{
+    /// <summary>
+    /// Basic path planning (PathPlanning) - No constraints
+    /// </summary>
+    Basic = 0,
+
+    /// <summary>
+    /// Path planning with start direction constraint (PathPlanningWithStartDirection)
+    /// </summary>
+    WithStartDirection = 1,
+
+    /// <summary>
+    /// Path planning with final direction constraint (PathPlanningWithFinalDirection)
+    /// </summary>
+    WithFinalDirection = 2,
+
+    /// <summary>
+    /// Path planning with final angle constraint (PathPlanningWithAngle)
+    /// </summary>
+    WithAngle = 3
+}

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/RobotInfo.cs

@@ -0,0 +1,55 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Robot information needed for conflict detection
+/// </summary>
+public class RobotInfo
+{
+    /// <summary>
+    /// Robot ID
+    /// </summary>
+    public string RobotId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Robot length in meters (from RobotModel)
+    /// </summary>
+    public double Length { get; set; }
+
+    /// <summary>
+    /// Robot width in meters (from RobotModel)
+    /// </summary>
+    public double Width { get; set; }
+
+    /// <summary>
+    /// Navigation point X offset in meters (from RobotModel)
+    /// </summary>
+    public double NavigationPointX { get; set; }
+
+    /// <summary>
+    /// Navigation point Y offset in meters (from RobotModel)
+    /// </summary>
+    public double NavigationPointY { get; set; }
+
+    /// <summary>
+    /// Current X position (from State message)
+    /// </summary>
+    public double CurrentX { get; set; }
+
+    /// <summary>
+    /// Current Y position (from State message)
+    /// </summary>
+    public double CurrentY { get; set; }
+
+    /// <summary>
+    /// Current orientation angle in radians (from State message)
+    /// </summary>
+    public double CurrentTheta { get; set; }
+
+    /// <summary>
+    /// Last node ID the robot passed through (from State message)
+    /// </summary>
+    public string LastNodeId { get; set; } = string.Empty;
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/RobotPriority.cs

@@ -0,0 +1,30 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Represents priority information for a robot
+/// </summary>
+public class RobotPriority
+{
+    /// <summary>
+    /// Robot ID
+    /// </summary>
+    public string RobotId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Priority level (higher number = higher priority)
+    /// </summary>
+    public int PriorityLevel { get; set; }
+
+    /// <summary>
+    /// Reason for the priority
+    /// </summary>
+    public PriorityReason Reason { get; set; }
+
+    /// <summary>
+    /// Priority valid until this time (null if permanent)
+    /// </summary>
+    public DateTime? ValidUntil { get; set; }
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/RobotRoute.cs

@@ -0,0 +1,70 @@
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Represents a complete route for a robot with Base and Horizon segments
+/// </summary>
+public class RobotRoute
+{
+    /// <summary>
+    /// Robot ID (SerialNumber)
+    /// </summary>
+    public string RobotId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Order ID from VDA5050
+    /// </summary>
+    public string OrderId { get; set; } = string.Empty;
+
+    /// <summary>
+    /// Order Update ID from VDA5050
+    /// </summary>
+    public int OrderUpdateId { get; set; }
+
+    /// <summary>
+    /// Full route (Base + Horizon)
+    /// </summary>
+    public List<RouteSegment> FullRoute { get; set; } = [];
+
+    /// <summary>
+    /// Base: Released segments that robot is currently executing
+    /// </summary>
+    public List<RouteSegment> Base { get; set; } = [];
+
+    /// <summary>
+    /// Horizon: Unreleased segments waiting for conditions
+    /// </summary>
+    public List<RouteSegment> Horizon { get; set; } = [];
+
+    /// <summary>
+    /// Current position index in the route
+    /// </summary>
+    public int CurrentSegmentIndex { get; set; }
+
+    /// <summary>
+    /// Route creation timestamp
+    /// </summary>
+    public DateTime CreatedAt { get; set; } = DateTime.UtcNow;
+
+    /// <summary>
+    /// Last update timestamp
+    /// </summary>
+    public DateTime LastUpdated { get; set; } = DateTime.UtcNow;
+
+    /// <summary>
+    /// Number of reroute attempts for this route (to prevent infinite rerouting)
+    /// </summary>
+    public int RerouteAttempts { get; set; } = 0;
+
+    /// <summary>
+    /// Node ID where robot should wait (if WaitAtNode resolution is applied)
+    /// </summary>
+    public Guid? WaitNodeId { get; set; }
+
+    /// <summary>
+    /// Time until which robot should wait at WaitNodeId (if WaitAtNode resolution is applied)
+    /// </summary>
+    public DateTime? WaitUntil { get; set; }
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/RouteSegment.cs

@@ -0,0 +1,71 @@
+using RobotNet.VDA5050.Order;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Represents a segment in a robot route (either a node or an edge)
+/// Uses VDA5050.Order.Node and VDA5050.Order.Edge to store full information
+/// </summary>
+public class RouteSegment
+{
+    // Internal properties needed for traffic control logic
+    /// <summary>
+    /// Node ID (Guid from database) - for conflict detection and edge reservation
+    /// </summary>
+    public Guid NodeId { get; set; }
+
+    /// <summary>
+    /// Edge ID (Guid from database) - Null if this is a node-only segment
+    /// </summary>
+    public Guid? EdgeId { get; set; }
+
+    /// <summary>
+    /// Start Node ID of the edge (Guid from database) - for conflict detection
+    /// </summary>
+    public Guid? StartNodeId { get; set; }
+
+    /// <summary>
+    /// End Node ID of the edge (Guid from database) - for conflict detection
+    /// </summary>
+    public Guid? EndNodeId { get; set; }
+
+    /// <summary>
+    /// Whether this segment has been released into base
+    /// </summary>
+    public bool Released { get; set; }
+
+    /// <summary>
+    /// Time until which this edge is reserved
+    /// </summary>
+    public DateTime? ReservedUntil { get; set; }
+
+    // VDA5050 Order objects containing full information from MapEditor
+    /// <summary>
+    /// VDA5050 Node with full information (NodeId, NodeDescription, NodePosition, Actions, etc.)
+    /// </summary>
+    public Node? VdaNode { get; set; }
+
+    /// <summary>
+    /// VDA5050 Edge with full information (EdgeId, EdgeDescription, MaxSpeed, Trajectory, Actions, etc.)
+    /// Null if this is a node-only segment
+    /// </summary>
+    public Edge? VdaEdge { get; set; }
+
+    /// <summary>
+    /// Get SequenceId from VdaNode or VdaEdge
+    /// </summary>
+    public int SequenceId => VdaNode?.SequenceId ?? VdaEdge?.SequenceId ?? 0;
+
+    /// <summary>
+    /// Check if this segment is a node segment
+    /// </summary>
+    public bool IsNode => VdaNode != null;
+
+    /// <summary>
+    /// Check if this segment is an edge segment
+    /// </summary>
+    public bool IsEdge => VdaEdge != null;
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Models/TrafficControlConfig.cs

@@ -0,0 +1,174 @@
+using RobotNet10.FleetManager.Shared.Enums;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+/// <summary>
+/// Configuration for TrafficControl service
+/// </summary>
+public class TrafficControlConfig
+{
+    /// <summary>
+    /// Conflict detection configuration
+    /// </summary>
+    public ConflictDetectionConfig ConflictDetection { get; set; } = new();
+
+    /// <summary>
+    /// Base/Horizon management configuration
+    /// </summary>
+    public BaseHorizonConfig BaseHorizon { get; set; } = new();
+
+    /// <summary>
+    /// Conflict resolution configuration
+    /// </summary>
+    public ConflictResolutionConfig ConflictResolution { get; set; } = new();
+
+    /// <summary>
+    /// Priority configuration
+    /// </summary>
+    public PriorityConfig Priority { get; set; } = new();
+
+    /// <summary>
+    /// Path planning configuration
+    /// </summary>
+    public PathPlanningConfig PathPlanning { get; set; } = new();
+}
+
+/// <summary>
+/// Conflict detection configuration
+/// </summary>
+public class ConflictDetectionConfig
+{
+    /// <summary>
+    /// Detection interval in milliseconds (default: 500ms = 2 Hz)
+    /// </summary>
+    public int IntervalMs { get; set; } = 500;
+
+    /// <summary>
+    /// Vertex conflict threshold in meters (default: 2.0m)
+    /// </summary>
+    public double VertexConflictThreshold { get; set; } = 2.0;
+
+    /// <summary>
+    /// Minimum safe distance for proximity conflict in meters (default: 1.0m)
+    /// </summary>
+    public double ProximityMinDistance { get; set; } = 1.0;
+
+    /// <summary>
+    /// Time conflict threshold in seconds (default: 5.0s)
+    /// </summary>
+    public double TimeConflictThreshold { get; set; } = 5.0;
+
+    /// <summary>
+    /// Rotation space radius in meters (default: 0.5m)
+    /// </summary>
+    public double RotationSpaceRadius { get; set; } = 0.5;
+
+    /// <summary>
+    /// Corridor width threshold in meters (default: 2.0m)
+    /// </summary>
+    public double CorridorWidthThreshold { get; set; } = 2.0;
+}
+
+/// <summary>
+/// Base/Horizon management configuration
+/// </summary>
+public class BaseHorizonConfig
+{
+    /// <summary>
+    /// Initial number of base segments (default: 2)
+    /// </summary>
+    public int InitialBaseSegments { get; set; } = 2;
+
+    /// <summary>
+    /// Number of segments to release ahead (default: 2)
+    /// </summary>
+    public int ReleaseAheadSegments { get; set; } = 2;
+
+    /// <summary>
+    /// Minimum number of horizon segments to keep (default: 1)
+    /// </summary>
+    public int MinHorizonSegments { get; set; } = 1;
+}
+
+/// <summary>
+/// Conflict resolution configuration
+/// </summary>
+public class ConflictResolutionConfig
+{
+    /// <summary>
+    /// Maximum wait time at node in seconds (default: 5.0s)
+    /// </summary>
+    public double WaitTimeAtNode { get; set; } = 5.0;
+
+    /// <summary>
+    /// Whether to allow reroute on conflict (default: true)
+    /// </summary>
+    public bool RerouteOnConflict { get; set; } = true;
+
+    /// <summary>
+    /// Maximum reroute attempts for one conflict (default: 3)
+    /// </summary>
+    public int MaxRerouteAttempts { get; set; } = 3;
+
+    /// <summary>
+    /// Enable resolution optimization (default: true)
+    /// </summary>
+    public bool ResolutionOptimization { get; set; } = true;
+
+    /// <summary>
+    /// Maximum simulation depth for resolution evaluation (default: 2)
+    /// </summary>
+    public int MaxResolutionSimulationDepth { get; set; } = 2;
+}
+
+/// <summary>
+/// Priority configuration
+/// </summary>
+public class PriorityConfig
+{
+    /// <summary>
+    /// Default priority level (default: 0)
+    /// </summary>
+    public int DefaultPriority { get; set; } = 0;
+
+    /// <summary>
+    /// Emergency priority level (default: 100)
+    /// </summary>
+    public int EmergencyPriority { get; set; } = 100;
+
+    /// <summary>
+    /// High value order priority level (default: 50)
+    /// </summary>
+    public int HighValueOrderPriority { get; set; } = 50;
+
+    /// <summary>
+    /// Time critical priority level (default: 30)
+    /// </summary>
+    public int TimeCriticalPriority { get; set; } = 30;
+}
+
+/// <summary>
+/// Path planning configuration
+/// </summary>
+public class PathPlanningConfig
+{
+    /// <summary>
+    /// Mapping from NavigationType to PathPlanningMethod
+    /// Defines which IPathPlanner method to use for each NavigationType
+    /// </summary>
+    public Dictionary<NavigationType, PathPlanningMethod> NavigationTypeMethodMapping { get; set; } = new()
+    {
+        // Default mappings
+        { NavigationType.Differential, PathPlanningMethod.Basic },
+        { NavigationType.Forklift, PathPlanningMethod.Basic },
+        { NavigationType.OmniDrive, PathPlanningMethod.Basic }
+    };
+
+    /// <summary>
+    /// Default path planning method if NavigationType is not found in mapping (default: Basic)
+    /// </summary>
+    public PathPlanningMethod DefaultMethod { get; set; } = PathPlanningMethod.Basic;
+}
+
+
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/BaseHorizonManagementService.cs

@@ -0,0 +1,570 @@
+using Microsoft.Extensions.DependencyInjection;
+using RobotNet.VDA5050.State;
+using RobotNet10.FleetManager.Services;
+using RobotNet10.FleetManager.Services.ConfigManager;
+using RobotNet10.FleetManager.Services.RobotManager;
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+using RobotNet10.MapManager.Services;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing Base/Horizon segments
+/// </summary>
+public class BaseHorizonManagementService(
+    Logger<BaseHorizonManagementService> logger,
+    IEdgeReservationService edgeReservationService,
+    IOrderUpdateService orderUpdateService,
+    IServiceScopeFactory serviceScopeFactory,
+    IRouteStorageService routeStorageService,
+    IConflictDetectionService conflictDetectionService,
+    ITrafficConfig trafficConfig) : IBaseHorizonManagementService
+{
+    private readonly Logger<BaseHorizonManagementService> _logger = logger;
+    private readonly IEdgeReservationService _edgeReservationService = edgeReservationService;
+    private readonly IOrderUpdateService _orderUpdateService = orderUpdateService;
+    private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
+    private readonly IRouteStorageService _routeStorageService = routeStorageService;
+    private readonly IConflictDetectionService _conflictDetectionService = conflictDetectionService;
+    private readonly ITrafficConfig _trafficConfig = trafficConfig ?? throw new ArgumentNullException(nameof(trafficConfig));
+
+    public async Task<(bool IsInBase, int CurrentSegmentIndex, RouteSegment? CurrentSegment)> CheckRobotPositionAsync(
+        string robotId,
+        RobotRoute route,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            // Get robot current state
+            // Resolve IRobotManagerService lazily to avoid circular dependency
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotManager = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
+            var robotData = robotManager.GetRobotData(robotId);
+            var state = robotData?.State;
+            
+            if (state?.NodeStates == null || state.NodeStates.Length == 0)
+            {
+                // Robot is idle or just started, assume at first node
+                return (false, 0, route.FullRoute.FirstOrDefault());
+            }
+
+            // Find current segment based on lastNodeSequenceId from state
+            var lastNodeSequenceId = state.LastNodeSequenceId;
+            var currentSegment = route.FullRoute.FirstOrDefault(s => 
+                (s.VdaNode != null && s.VdaNode.SequenceId == lastNodeSequenceId) ||
+                (s.VdaEdge != null && s.VdaEdge.SequenceId == lastNodeSequenceId));
+            
+            if (currentSegment == null)
+            {
+                // Cannot find segment, assume at start
+                return (false, 0, route.FullRoute.FirstOrDefault());
+            }
+
+            var currentIndex = route.FullRoute.IndexOf(currentSegment);
+            var isInBase = currentIndex < route.Base.Count;
+
+            return (isInBase, currentIndex, currentSegment);
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error checking robot position for robot {robotId}: {ex.Message}");
+            // Default: assume in Horizon (safer for intervention)
+            return (false, 0, route.FullRoute.FirstOrDefault());
+        }
+    }
+
+    public int CountRemainingBaseSegments(RobotRoute route, StateMsg? state)
+    {
+        try
+        {
+            if (state == null)
+            {
+                // No state available, assume all Base segments remaining
+                return route.Base.Count;
+            }
+
+            if (state.NodeStates == null || state.NodeStates.Length == 0)
+            {
+                return route.Base.Count; // Assume all Base segments remaining
+            }
+
+            // Find current segment based on LastNodeSequenceId
+            var lastNodeSequenceId = state.LastNodeSequenceId;
+            var currentSegment = route.Base.FirstOrDefault(s => 
+                (s.VdaNode != null && s.VdaNode.SequenceId == lastNodeSequenceId) ||
+                (s.VdaEdge != null && s.VdaEdge.SequenceId == lastNodeSequenceId));
+
+            if (currentSegment == null)
+            {
+                return route.Base.Count; // Cannot find current segment, assume all remaining
+            }
+
+            var currentIndex = route.Base.IndexOf(currentSegment);
+            var remainingCount = route.Base.Count - currentIndex - 1;
+
+            return Math.Max(0, remainingCount);
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error counting remaining Base segments: {ex.Message}");
+            return route.Base.Count; // Default: assume all remaining
+        }
+    }
+
+    public async Task<bool> ReleaseHorizonSegmentAsync(
+        string robotId,
+        int segmentCount,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            if (string.IsNullOrWhiteSpace(robotId))
+            {
+                _logger.Warning("Cannot release horizon segments: robotId is null or empty");
+                return false;
+            }
+
+            if (segmentCount <= 0)
+            {
+                _logger.Warning($"Cannot release horizon segments: segmentCount must be > 0, got {segmentCount}");
+                return false;
+            }
+
+            // Get robot route
+            var route = await _routeStorageService.GetRobotRouteAsync(robotId);
+            if (route == null)
+            {
+                _logger.Warning($"Cannot release horizon segments: route not found for robot {robotId}");
+                return false;
+            }
+
+            if (route.Horizon.Count == 0)
+            {
+                _logger.Debug($"No horizon segments to release for robot {robotId}");
+                return false;
+            }
+
+            // Get next segments from Horizon
+            var segmentsToRelease = route.Horizon.Take(segmentCount).ToList();
+            if (segmentsToRelease.Count == 0)
+            {
+                _logger.Debug($"No segments to release for robot {robotId}");
+                return false;
+            }
+
+            // Check conflicts for segments to release
+            var hasConflicts = await _conflictDetectionService.CheckConflictsForSegmentsAsync(robotId, segmentsToRelease, cancellationToken);
+            if (hasConflicts)
+            {
+                _logger.Debug($"Cannot release horizon segments for robot {robotId}: conflicts detected");
+                return false;
+            }
+
+            // Check if robot is waiting at a node (WaitAtNode resolution)
+            // If WaitUntil has passed, allow release
+            if (route.WaitNodeId.HasValue && route.WaitUntil.HasValue)
+            {
+                var waitNodeInBase = route.Base.LastOrDefault(s => s.NodeId == route.WaitNodeId.Value);
+                if (waitNodeInBase != null)
+                {
+                    // Robot is waiting at this node
+                    if (DateTime.UtcNow < route.WaitUntil.Value)
+                    {
+                        // Still waiting, check if conflict is resolved
+                        var conflicts = await _conflictDetectionService.CheckConflictsForSegmentsAsync(robotId, segmentsToRelease, cancellationToken);
+                        if (conflicts)
+                        {
+                            _logger.Debug($"Robot {robotId} is waiting at node {route.WaitNodeId.Value}, conflict still exists, cannot release");
+                            return false;
+                        }
+                        // Conflict resolved, clear wait info and allow release
+                        _logger.Info($"Conflict resolved for robot {robotId} waiting at node {route.WaitNodeId.Value}, allowing release");
+                        route.WaitNodeId = null;
+                        route.WaitUntil = null;
+                    }
+                    else
+                    {
+                        // Wait time expired, clear wait info
+                        _logger.Info($"Wait time expired for robot {robotId} at node {route.WaitNodeId.Value}, clearing wait info");
+                        route.WaitNodeId = null;
+                        route.WaitUntil = null;
+                    }
+                }
+            }
+
+            // Reserve edges for newly released segments
+            var orderId = route.OrderId;
+            if (string.IsNullOrEmpty(orderId))
+            {
+                orderId = $"ORDER_{robotId}_{DateTime.UtcNow:yyyyMMddHHmmss}";
+                route.OrderId = orderId;
+            }
+
+            var reserveSuccess = await _edgeReservationService.ReserveEdgesAsync(robotId, orderId, segmentsToRelease, cancellationToken);
+            if (!reserveSuccess)
+            {
+                _logger.Warning($"Failed to reserve edges for horizon segments of robot {robotId}");
+                return false;
+            }
+
+            // Move segments from Horizon to Base
+            foreach (var segment in segmentsToRelease)
+            {
+                segment.Released = true;
+                route.Base.Add(segment);
+            }
+
+            route.Horizon.RemoveRange(0, segmentsToRelease.Count);
+
+            // Update FullRoute
+            route.FullRoute = route.Base.Concat(route.Horizon).ToList();
+            route.LastUpdated = DateTime.UtcNow;
+
+            // Update route in storage
+            await _routeStorageService.UpdateRobotRouteAsync(robotId, route);
+
+            // Generate and send OrderUpdate
+            var orderUpdateSent = await _orderUpdateService.GenerateAndSendOrderUpdateAsync(robotId, route, cancellationToken);
+            if (!orderUpdateSent)
+            {
+                _logger.Warning($"Failed to send OrderUpdate for horizon release to robot {robotId}");
+                // Still return true as segments are already moved to Base
+            }
+
+            _logger.Info($"Successfully released {segmentsToRelease.Count} horizon segments to Base for robot {robotId}");
+            return true;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error releasing horizon segments for robot {robotId}: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task CheckAndReleaseHorizonsAsync(
+        string? robotId = null,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            List<string> robotIdsToCheck;
+
+            if (!string.IsNullOrWhiteSpace(robotId))
+            {
+                // Check specific robot
+                robotIdsToCheck = new List<string> { robotId };
+            }
+            else
+            {
+                // Check all robots with active routes
+                var allRoutes = await _routeStorageService.GetAllActiveRoutesAsync();
+                robotIdsToCheck = allRoutes.Keys.ToList();
+            }
+
+            foreach (var id in robotIdsToCheck)
+            {
+                try
+                {
+                    // Get robot route
+                    var route = await _routeStorageService.GetRobotRouteAsync(id);
+                    if (route == null || route.Horizon.Count == 0)
+                    {
+                        continue; // No route or no horizon to release
+                    }
+
+                    // Check robot position
+                    var robotPosition = await CheckRobotPositionAsync(id, route, cancellationToken);
+                    if (!robotPosition.IsInBase)
+                    {
+                        continue; // Robot not in Base, skip
+                    }
+
+                    // Count remaining Base segments
+                    // Resolve IRobotManagerService lazily to avoid circular dependency
+                    using var scopeForRobotManager = _serviceScopeFactory.CreateScope();
+                    var robotManager = scopeForRobotManager.ServiceProvider.GetRequiredService<IRobotManagerService>();
+                    var robotData = robotManager.GetRobotData(id);
+                    var state = robotData?.State;
+                    if (state == null)
+                    {
+                        // Cannot determine remaining segments without state, skip
+                        continue;
+                    }
+
+                    var remainingBaseSegments = CountRemainingBaseSegments(route, state);
+
+                    // Only check if robot is near end of Base (1-2 segments remaining)
+                    if (remainingBaseSegments > 2)
+                    {
+                        continue; // Too early to release
+                    }
+
+                    // Determine how many segments to release
+                    var config = _trafficConfig.GetTrafficControlConfig();
+                    var segmentsToRelease = config.BaseHorizon.ReleaseAheadSegments;
+                    if (segmentsToRelease > route.Horizon.Count)
+                    {
+                        segmentsToRelease = route.Horizon.Count;
+                    }
+
+                    if (segmentsToRelease <= 0)
+                    {
+                        continue; // Nothing to release
+                    }
+
+                    // Try to release segments
+                    var released = await ReleaseHorizonSegmentAsync(id, segmentsToRelease, cancellationToken);
+                    if (released)
+                    {
+                        _logger.Debug($"Released {segmentsToRelease} horizon segments for robot {id}");
+                    }
+                }
+                catch (Exception ex)
+                {
+                    _logger.Error($"Error checking horizon release for robot {id}: {ex.Message}");
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error in CheckAndReleaseHorizonsAsync: {ex.Message}");
+        }
+    }
+
+    public async Task<int> CalculateSafeBaseSizeAsync(
+        string robotId,
+        RobotRoute route,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            // Get all active routes (excluding current robot)
+            var allRoutes = await _routeStorageService.GetAllActiveRoutesAsync();
+            var activeRoutes = allRoutes.Values
+                .Where(r => r.RobotId != robotId)
+                .ToList();
+
+            // Get config once
+            var config = _trafficConfig.GetTrafficControlConfig();
+
+            if (activeRoutes.Count == 0)
+            {
+                // No other robots - can use default base size
+                return config.BaseHorizon.InitialBaseSegments;
+            }
+
+            // Start with minimum base size
+            int safeBaseSize = config.BaseHorizon.InitialBaseSegments;
+            int maxBaseSize = route.FullRoute.Count - 1; // At least 1 segment in Horizon
+
+            // Check conflicts for increasing base sizes
+            for (int size = config.BaseHorizon.InitialBaseSegments; size <= maxBaseSize; size++)
+            {
+                var testBase = route.FullRoute.Take(size).ToList();
+                var hasConflict = await CheckConflictsForBaseAsync(
+                    robotId,
+                    testBase,
+                    activeRoutes,
+                    cancellationToken);
+
+                if (!hasConflict)
+                {
+                    // No conflicts - can use this size
+                    safeBaseSize = size;
+                }
+                else
+                {
+                    // Has conflicts - stop here, use previous safe size
+                    _logger.Debug($"Base size {size} has conflicts for robot {robotId}, using safe size {safeBaseSize}");
+                    break;
+                }
+            }
+
+            _logger.Info($"Calculated safe base size for robot {robotId}: {safeBaseSize} segments");
+            return safeBaseSize;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error calculating safe base size for robot {robotId}: {ex.Message}");
+            // Return minimum base size on error
+            var config = _trafficConfig.GetTrafficControlConfig();
+            return config.BaseHorizon.InitialBaseSegments;
+        }
+    }
+
+    /// <summary>
+    /// Check if Base segments have conflicts with other active routes
+    /// </summary>
+    private async Task<bool> CheckConflictsForBaseAsync(
+        string robotId,
+        List<RouteSegment> baseSegments,
+        List<RobotRoute> otherActiveRoutes,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            if (baseSegments == null || baseSegments.Count == 0)
+            {
+                return false; // No segments = no conflict
+            }
+
+            // Get levelId to fetch edge details
+            var levelId = await GetLevelIdForRobotAsync(robotId, cancellationToken);
+            if (levelId == null)
+            {
+                _logger.Warning($"Cannot get levelId for robot {robotId} to check conflicts");
+                return true; // Assume conflict on error
+            }
+
+            // Get edges with nodes for length calculation
+            using var scope = _serviceScopeFactory.CreateAsyncScope();
+            var _nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
+            var _edgeService = scope.ServiceProvider.GetRequiredService<IEdgeService>();
+            var edges = await _edgeService.GetEdgesByLevelAsync(levelId.Value, includeNodes: true, includeVehicleProperties: true);
+            var nodes = await _nodeService.GetNodesByLevelAsync(levelId.Value, includeVehicleProperties: true);
+
+            // Create lookup dictionaries
+            var edgeDict = edges.ToDictionary(e => e.Id);
+            var nodeDict = nodes.ToDictionary(n => n.Id);
+
+            var now = DateTime.UtcNow;
+
+            // Check each edge segment in Base
+            foreach (var segment in baseSegments)
+            {
+                if (segment.EdgeId == null) continue; // Skip node segments
+
+                var edgeId = segment.EdgeId.Value;
+                if (!edgeDict.TryGetValue(edgeId, out var edge))
+                {
+                    continue; // Edge not found, skip
+                }
+
+                // Get start and end nodes
+                if (!nodeDict.TryGetValue(edge.StartNodeId, out var startNode) ||
+                    !nodeDict.TryGetValue(edge.EndNodeId, out var endNode))
+                {
+                    continue; // Nodes not found, skip
+                }
+
+                // Calculate edge length
+                var edgeLength = CalculateEdgeLength(startNode, endNode);
+
+                // Get max speed from EdgeVehicleProperty
+                double? maxSpeed = null;
+                if (edge.VehicleProperties != null && edge.VehicleProperties.Any())
+                {
+                    maxSpeed = edge.VehicleProperties.FirstOrDefault()?.MaxSpeed;
+                }
+
+                // Calculate reservation duration for this edge
+                var duration = CalculateReservationDuration(edgeLength, maxSpeed, 1.0);
+                var reservedFrom = now;
+                var reservedUntil = now.Add(duration);
+
+                // Check if edge is available (not reserved by other robots)
+                var isAvailable = await _edgeReservationService.IsEdgeAvailableAsync(edgeId, reservedFrom, reservedUntil, cancellationToken);
+                if (!isAvailable)
+                {
+                    _logger.Debug($"Edge {edgeId} is not available for robot {robotId} during {reservedFrom} to {reservedUntil}");
+                    return true; // Conflict found
+                }
+
+                // Also check for reverse edge (EdgeBA vs EdgeAB)
+                var reverseEdge = edges.FirstOrDefault(e =>
+                    e.StartNodeId == edge.EndNodeId &&
+                    e.EndNodeId == edge.StartNodeId);
+
+                if (reverseEdge != null)
+                {
+                    var reverseIsAvailable = await _edgeReservationService.IsEdgeAvailableAsync(
+                        reverseEdge.Id,
+                        reservedFrom,
+                        reservedUntil,
+                        cancellationToken);
+                    if (!reverseIsAvailable)
+                    {
+                        _logger.Debug($"Reverse edge {reverseEdge.Id} is not available for robot {robotId}");
+                        return true; // Conflict found (confrontation)
+                    }
+                }
+            }
+
+            return false; // No conflicts found
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error checking conflicts for Base segments of robot {robotId}: {ex.Message}");
+            return true; // Assume conflict on error
+        }
+    }
+
+    /// <summary>
+    /// Get levelId for a robot from Robot.MapId in database
+    /// </summary>
+    private async Task<Guid?> GetLevelIdForRobotAsync(string robotId, CancellationToken cancellationToken)
+    {
+        try
+        {
+            // Use IServiceScopeFactory to create a scope for Scoped services
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
+
+            var robot = await robotService.GetByRobotIdAsync(robotId);
+            if (robot == null)
+            {
+                _logger.Warning($"Robot {robotId} not found in database");
+                return null;
+            }
+
+            if (robot.MapId == null)
+            {
+                _logger.Warning($"Robot {robotId} has no MapId assigned");
+                return null;
+            }
+
+            // Robot.MapId is the levelId (LayoutLevel.Id)
+            return robot.MapId;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error getting levelId for robot {robotId}: {ex.Message}");
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Calculate edge length from start and end nodes (Euclidean distance)
+    /// </summary>
+    private double CalculateEdgeLength(
+        RobotNet10.MapManager.Data.Node startNode,
+        RobotNet10.MapManager.Data.Node endNode)
+    {
+        var dx = endNode.X - startNode.X;
+        var dy = endNode.Y - startNode.Y;
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+
+    /// <summary>
+    /// Calculate reservation duration based on edge length and robot speed
+    /// </summary>
+    private TimeSpan CalculateReservationDuration(
+        double edgeLength,
+        double? maxSpeed = null,
+        double defaultSpeed = 1.0) // Default speed: 1.0 m/s
+    {
+        var speed = maxSpeed ?? defaultSpeed;
+        if (speed <= 0)
+        {
+            speed = defaultSpeed;
+        }
+
+        // Add buffer time (10% of travel time) for safety
+        var travelTime = edgeLength / speed;
+        var bufferTime = travelTime * 0.1;
+        var totalTime = travelTime + bufferTime;
+
+        return TimeSpan.FromSeconds(Math.Max(totalTime, 0.5)); // Minimum 0.5 seconds
+    }
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/EdgeReservationService.cs

@@ -0,0 +1,331 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+using RobotNet10.MapManager.Services;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing edge reservations
+/// </summary>
+public class EdgeReservationService(
+    Logger<EdgeReservationService> logger,
+    IServiceScopeFactory serviceScopeFactory,
+    IRobotInfoService robotInfoService) : IEdgeReservationService
+{
+    private readonly Logger<EdgeReservationService> _logger = logger;
+    private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
+    private readonly IRobotInfoService _robotInfoService = robotInfoService;
+
+    // Edge reservations: EdgeId -> List of reservations
+    private readonly Dictionary<Guid, List<EdgeReservation>> _edgeReservations = [];
+    private readonly Lock _reservationsLock = new();
+
+    /// <summary>
+    /// Calculate reservation duration based on edge length and robot speed
+    /// </summary>
+    private static TimeSpan CalculateReservationDuration(
+        double edgeLength,
+        double? maxSpeed = null,
+        double defaultSpeed = 1.0) // Default speed: 1.0 m/s
+    {
+        var speed = maxSpeed ?? defaultSpeed;
+        if (speed <= 0)
+        {
+            speed = defaultSpeed;
+        }
+
+        // Add buffer time (10% of travel time) for safety
+        var travelTime = edgeLength / speed;
+        var bufferTime = travelTime * 0.1;
+        var totalTime = travelTime + bufferTime;
+
+        return TimeSpan.FromSeconds(Math.Max(totalTime, 0.5)); // Minimum 0.5 seconds
+    }
+
+    /// <summary>
+    /// Calculate edge length from start and end nodes (Euclidean distance)
+    /// If trajectory exists, use more accurate calculation (future enhancement)
+    /// </summary>
+    private static double CalculateEdgeLength(
+        RobotNet10.MapManager.Data.Node startNode,
+        RobotNet10.MapManager.Data.Node endNode)
+    {
+        var dx = endNode.X - startNode.X;
+        var dy = endNode.Y - startNode.Y;
+        return Math.Sqrt(dx * dx + dy * dy);
+    }
+
+    /// <summary>
+    /// Get levelId for a robot from Robot.MapId in database
+    /// </summary>
+    private async Task<Guid?> GetLevelIdForRobotAsync(string robotId)
+    {
+        try
+        {
+            // Use IServiceScopeFactory to create a scope for Scoped services
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
+
+            var robot = await robotService.GetByRobotIdAsync(robotId);
+            if (robot == null)
+            {
+                _logger.Warning($"Robot {robotId} not found in database");
+                return null;
+            }
+
+            if (robot.MapId == null)
+            {
+                _logger.Warning($"Robot {robotId} has no MapId assigned");
+                return null;
+            }
+
+            // Robot.MapId is the levelId (LayoutLevel.Id)
+            return robot.MapId;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error getting levelId for robot {robotId}: {ex.Message}");
+            return null;
+        }
+    }
+
+    public async Task<bool> ReserveEdgesAsync(
+        string robotId,
+        string orderId,
+        List<RouteSegment> segments,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            if (segments == null || segments.Count == 0)
+            {
+                return true; // No segments to reserve
+            }
+
+            var reservations = new List<EdgeReservation>();
+            var now = DateTime.UtcNow;
+
+            // Get robot info for speed calculation
+            var robotInfo = await _robotInfoService.GetRobotInfoAsync(robotId, cancellationToken);
+            var defaultSpeed = 1.0; // Default 1.0 m/s if robot info not available
+
+            // Get levelId to fetch edge details
+            var levelId = await GetLevelIdForRobotAsync(robotId);
+            if (levelId == null)
+            {
+                _logger.Warning($"Cannot get levelId for robot {robotId} to reserve edges");
+                return false;
+            }
+
+            // Get edges with nodes for length calculation
+
+            using var scope = _serviceScopeFactory.CreateAsyncScope();
+            var _nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
+            var _edgeService = scope.ServiceProvider.GetRequiredService<IEdgeService>();
+            var edges = await _edgeService.GetEdgesByLevelAsync(levelId.Value, includeNodes: true, includeVehicleProperties: true);
+            var nodes = await _nodeService.GetNodesByLevelAsync(levelId.Value, includeVehicleProperties: true);
+
+            // Create lookup dictionaries
+            var edgeDict = edges.ToDictionary(e => e.Id);
+            var nodeDict = nodes.ToDictionary(n => n.Id);
+
+            // Reserve edges from segments
+            foreach (var segment in segments)
+            {
+                if (segment.VdaEdge == null) continue; // Skip node segments
+
+                // Skip virtual edges (created when robot is on edge, not in database)
+                if (segment.VdaEdge.EdgeId.StartsWith("VIRTUAL_", StringComparison.OrdinalIgnoreCase))
+                {
+                    _logger.Debug($"Skipping virtual edge {segment.EdgeId} for reservation (robot is on edge)");
+                    continue;
+                }
+
+                if(segment.EdgeId is null) continue;
+
+                var edgeId = segment.EdgeId.Value;
+                if (!edgeDict.TryGetValue(edgeId, out var edge))
+                {
+                    _logger.Warning($"Edge {edgeId} not found for reservation");
+                    continue;
+                }
+
+                // Get start and end nodes
+                if (!nodeDict.TryGetValue(edge.StartNodeId, out var startNode) ||
+                    !nodeDict.TryGetValue(edge.EndNodeId, out var endNode))
+                {
+                    _logger.Warning($"Start or end node not found for edge {edgeId}");
+                    continue;
+                }
+
+                // Calculate edge length
+                var edgeLength = CalculateEdgeLength(startNode, endNode);
+
+                // Get max speed from EdgeVehicleProperty (if available)
+                // TODO: Match with robot's vehicle type
+                double? maxSpeed = null;
+                if (edge.VehicleProperties != null && edge.VehicleProperties.Count != 0)
+                {
+                    // Use first vehicle property's max speed (in future, match with robot's vehicle type)
+                    maxSpeed = edge.VehicleProperties.FirstOrDefault()?.MaxSpeed;
+                }
+
+                // Calculate reservation duration
+                var duration = CalculateReservationDuration(edgeLength, maxSpeed, defaultSpeed);
+
+                var reservation = new EdgeReservation
+                {
+                    EdgeId = edgeId,
+                    EdgeIdString = edge.EdgeId,
+                    StartNodeId = edge.StartNodeId,
+                    EndNodeId = edge.EndNodeId,
+                    RobotId = robotId,
+                    OrderId = orderId,
+                    ReservedAt = now,
+                    ReservedUntil = now.Add(duration),
+                    Status = ReservationStatus.Reserved
+                };
+
+                reservations.Add(reservation);
+            }
+
+            // Add reservations to dictionary (thread-safe)
+            lock (_reservationsLock)
+            {
+                foreach (var reservation in reservations)
+                {
+                    if (!_edgeReservations.TryGetValue(reservation.EdgeId, out List<EdgeReservation>? value))
+                    {
+                        value = [];
+                        _edgeReservations[reservation.EdgeId] = value;
+                    }
+
+                    value.Add(reservation);
+                }
+            }
+
+            _logger.Debug($"Reserved {reservations.Count} edges for robot {robotId}, order {orderId}");
+            return true;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error reserving edges for robot {robotId}: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task<bool> ReleaseReservationsAsync(
+        string robotId,
+        string? orderId = null,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            var releasedCount = 0;
+
+            lock (_reservationsLock)
+            {
+                foreach (var edgeReservations in _edgeReservations.Values)
+                {
+                    for (int i = edgeReservations.Count - 1; i >= 0; i--)
+                    {
+                        var reservation = edgeReservations[i];
+                        if (reservation.RobotId == robotId &&
+                            (orderId == null || reservation.OrderId == orderId))
+                        {
+                            reservation.Status = ReservationStatus.Released;
+                            edgeReservations.RemoveAt(i);
+                            releasedCount++;
+                        }
+                    }
+                }
+
+                // Clean up empty lists
+                var emptyEdges = _edgeReservations
+                    .Where(kvp => kvp.Value.Count == 0)
+                    .Select(kvp => kvp.Key)
+                    .ToList();
+
+                foreach (var edgeId in emptyEdges)
+                {
+                    _edgeReservations.Remove(edgeId);
+                }
+            }
+
+            _logger.Debug($"Released {releasedCount} reservations for robot {robotId}" + 
+                (orderId != null ? $", order {orderId}" : ""));
+            return true;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error releasing reservations for robot {robotId}: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task<List<EdgeReservation>> GetEdgeReservationsAsync(
+        Guid edgeId,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            lock (_reservationsLock)
+            {
+                if (_edgeReservations.TryGetValue(edgeId, out var reservations))
+                {
+                    // Filter out expired reservations
+                    var now = DateTime.UtcNow;
+                    var activeReservations = reservations
+                        .Where(r => r.ReservedUntil > now && r.Status == ReservationStatus.Reserved)
+                        .ToList();
+
+                    // Remove expired reservations
+                    var expiredCount = reservations.Count - activeReservations.Count;
+                    if (expiredCount > 0)
+                    {
+                        _edgeReservations[edgeId] = activeReservations;
+                        _logger.Debug($"Cleaned up {expiredCount} expired reservations for edge {edgeId}");
+                    }
+
+                    return [.. activeReservations];
+                }
+
+                return [];
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error getting reservations for edge {edgeId}: {ex.Message}");
+            return [];
+        }
+    }
+
+    public async Task<bool> IsEdgeAvailableAsync(
+        Guid edgeId,
+        DateTime fromTime,
+        DateTime toTime,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            var reservations = await GetEdgeReservationsAsync(edgeId, cancellationToken);
+
+            // Check if any reservation overlaps with the requested time range
+            foreach (var reservation in reservations)
+            {
+                // Check for overlap: reservation.ReservedAt < toTime && reservation.ReservedUntil > fromTime
+                if (reservation.ReservedAt < toTime && reservation.ReservedUntil > fromTime)
+                {
+                    return false; // Edge is reserved during this time
+                }
+            }
+
+            return true; // Edge is available
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error checking edge availability for edge {edgeId}: {ex.Message}");
+            return false; // Assume not available on error
+        }
+    }
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IBaseHorizonManagementService.cs

@@ -0,0 +1,46 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing Base/Horizon segments
+/// </summary>
+public interface IBaseHorizonManagementService
+{
+    /// <summary>
+    /// Check robot position (Base vs Horizon)
+    /// </summary>
+    Task<(bool IsInBase, int CurrentSegmentIndex, RouteSegment? CurrentSegment)> CheckRobotPositionAsync(
+        string robotId,
+        RobotRoute route,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Release horizon segments to base when safe
+    /// </summary>
+    Task<bool> ReleaseHorizonSegmentAsync(
+        string robotId,
+        int segmentCount,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Check and release horizons for all robots (called periodically or on state update)
+    /// </summary>
+    Task CheckAndReleaseHorizonsAsync(
+        string? robotId = null,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Calculate safe base size - largest base size without conflicts
+    /// </summary>
+    Task<int> CalculateSafeBaseSizeAsync(
+        string robotId,
+        RobotRoute route,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Count remaining Base segments based on robot's current position
+    /// </summary>
+    int CountRemainingBaseSegments(RobotRoute route, RobotNet.VDA5050.State.StateMsg? state);
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IConflictDetectionService.cs

@@ -0,0 +1,23 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for detecting conflicts between robots
+/// </summary>
+public interface IConflictDetectionService
+{
+    /// <summary>
+    /// Detect all conflicts between active robots
+    /// </summary>
+    Task<List<Conflict>> DetectConflictsAsync(CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Check conflicts for specific route segments
+    /// </summary>
+    Task<bool> CheckConflictsForSegmentsAsync(
+        string robotId,
+        List<RouteSegment> segments,
+        CancellationToken cancellationToken = default);
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IConflictResolutionService.cs

@@ -0,0 +1,22 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for resolving conflicts between robots
+/// </summary>
+public interface IConflictResolutionService
+{
+    /// <summary>
+    /// Resolve a conflict
+    /// </summary>
+    Task<bool> ResolveConflictAsync(Conflict conflict, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Evaluate conflicts for resolution optimization
+    /// </summary>
+    Task<List<Conflict>> EvaluateConflictsForResolutionAsync(
+        List<Conflict> conflicts,
+        CancellationToken cancellationToken = default);
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IEdgeReservationService.cs

@@ -0,0 +1,43 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing edge reservations
+/// </summary>
+public interface IEdgeReservationService
+{
+    /// <summary>
+    /// Reserve edges for a robot's route segments
+    /// </summary>
+    Task<bool> ReserveEdgesAsync(
+        string robotId,
+        string orderId,
+        List<RouteSegment> segments,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Release reservations for a robot
+    /// </summary>
+    Task<bool> ReleaseReservationsAsync(
+        string robotId,
+        string? orderId = null,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Get all reservations for a specific edge
+    /// </summary>
+    Task<List<EdgeReservation>> GetEdgeReservationsAsync(
+        Guid edgeId,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Check if an edge is available for reservation at a given time
+    /// </summary>
+    Task<bool> IsEdgeAvailableAsync(
+        Guid edgeId,
+        DateTime fromTime,
+        DateTime toTime,
+        CancellationToken cancellationToken = default);
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IOrderUpdateService.cs

@@ -0,0 +1,34 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for generating and sending OrderUpdate messages
+/// </summary>
+public interface IOrderUpdateService
+{
+    /// <summary>
+    /// Generate OrderUpdate from Horizon segments and send to robot
+    /// </summary>
+    Task<bool> GenerateAndSendOrderUpdateAsync(
+        string robotId,
+        RobotRoute route,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Send OrderUpdate to robot (general method for adding new segments)
+    /// </summary>
+    Task<bool> SendOrderUpdateAsync(
+        string robotId,
+        List<RouteSegment> newSegments,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Send initial Order to robot (Base segments only)
+    /// </summary>
+    Task<bool> SendInitialOrderAsync(
+        string robotId,
+        RobotRoute route,
+        CancellationToken cancellationToken = default);
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IPriorityService.cs

@@ -0,0 +1,30 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing robot priorities
+/// </summary>
+public interface IPriorityService
+{
+    /// <summary>
+    /// Set priority for a robot
+    /// </summary>
+    Task<bool> SetRobotPriorityAsync(string robotId, RobotPriority priority);
+
+    /// <summary>
+    /// Get priority for a robot
+    /// </summary>
+    Task<RobotPriority> GetRobotPriorityAsync(string robotId);
+
+    /// <summary>
+    /// Remove priority for a robot (reset to default)
+    /// </summary>
+    Task<bool> RemoveRobotPriorityAsync(string robotId);
+
+    /// <summary>
+    /// Clean up expired priorities (called periodically)
+    /// </summary>
+    void CleanupExpiredPriorities();
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IRobotInfoService.cs

@@ -0,0 +1,26 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing robot information cache
+/// </summary>
+public interface IRobotInfoService
+{
+    /// <summary>
+    /// Get robot information (dimensions, navigation point) from RobotModel
+    /// Caches the information to avoid repeated database queries
+    /// </summary>
+    Task<RobotInfo?> GetRobotInfoAsync(string robotId, CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Clear robot info cache for a specific robot
+    /// </summary>
+    void ClearRobotInfoCache(string robotId);
+
+    /// <summary>
+    /// Clear all robot info cache
+    /// </summary>
+    void ClearAllRobotInfoCache();
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IRoutePlanningService.cs

@@ -0,0 +1,60 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+using RobotNet10.GlobalPathPlanner.Model;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for route planning
+/// </summary>
+public interface IRoutePlanningService
+{
+    /// <summary>
+    /// Plan route for a robot from start to goal
+    /// </summary>
+    Task<RobotRoute?> PlanRouteAsync(
+        string robotId,
+        Guid startNodeId,
+        Guid goalNodeId,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Plan route with optional constraints (angle, startDirection, finalDirection)
+    /// </summary>
+    Task<RobotRoute?> PlanRouteAsync(
+        string robotId,
+        Guid startNodeId,
+        Guid goalNodeId,
+        double? goalAngle,
+        Orientation? startDirection,
+        Orientation? finalDirection,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Plan route from current position (x, y, theta) to goal node
+    /// </summary>
+    Task<RobotRoute?> PlanRouteFromPositionAsync(
+        string robotId,
+        double x,
+        double y,
+        double theta,
+        Guid goalNodeId,
+        double? goalAngle,
+        Orientation? startDirection,
+        Orientation? finalDirection,
+        CancellationToken cancellationToken = default);
+
+    /// <summary>
+    /// Plan route from current position (x, y, theta) to goal node
+    /// </summary>
+    Task<RobotRoute?> PlanRouteFromPositionACSTrafficAsync(
+        string robotId,
+        double x,
+        double y,
+        double theta,
+        Guid goalNodeId,
+        double? goalAngle,
+        Orientation? startDirection,
+        Orientation? finalDirection,
+        CancellationToken cancellationToken = default);
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/IRouteStorageService.cs

@@ -0,0 +1,25 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing active robot routes storage
+/// </summary>
+public interface IRouteStorageService
+{
+    /// <summary>
+    /// Get all active routes
+    /// </summary>
+    Task<Dictionary<string, RobotRoute>> GetAllActiveRoutesAsync();
+
+    /// <summary>
+    /// Get route for a specific robot
+    /// </summary>
+    Task<RobotRoute?> GetRobotRouteAsync(string robotId);
+
+    /// <summary>
+    /// Update robot route
+    /// </summary>
+    Task<bool> UpdateRobotRouteAsync(string robotId, RobotRoute route);
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/OrderUpdateService.cs

@@ -0,0 +1,405 @@
+using RobotNet10.FleetManager.Services.RobotManager;
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for generating and sending OrderUpdate messages
+/// </summary>
+public class OrderUpdateService(
+    Logger<OrderUpdateService> logger,
+    IServiceScopeFactory serviceScopeFactory) : IOrderUpdateService
+{
+    private readonly Logger<OrderUpdateService> _logger = logger;
+    private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
+
+    public async Task<bool> GenerateAndSendOrderUpdateAsync(
+        string robotId,
+        RobotRoute route,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            // Resolve IRobotManagerService lazily to avoid circular dependency
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotManager = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
+            var robotController = robotManager.GetRobotController(robotId);
+            if (robotController == null)
+            {
+                _logger.Warning($"Cannot send OrderUpdate: robot controller not found for robot {robotId}");
+                return false;
+            }
+
+            // Generate OrderUpdate from Horizon
+            var orderUpdate = await GenerateOrderUpdateFromHorizonAsync(route, robotId);
+            if (orderUpdate == null)
+            {
+                _logger.Warning($"Cannot send OrderUpdate: failed to generate order update for robot {robotId}");
+                return false;
+            }
+
+            // Send OrderUpdate
+            var sent = await robotController.SendOrderAsync(orderUpdate, cancellationToken);
+            if (sent.IsSuccess)
+            {
+                // Increment OrderUpdateId
+                route.OrderUpdateId++;
+                _logger.Info($"Sent OrderUpdate (ID: {route.OrderUpdateId}) to robot {robotId}");
+            }
+
+            return sent.IsSuccess;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error generating and sending OrderUpdate to robot {robotId}: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task<bool> SendOrderUpdateAsync(
+        string robotId,
+        List<RouteSegment> newSegments,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            if (string.IsNullOrWhiteSpace(robotId))
+            {
+                _logger.Warning("Cannot send OrderUpdate: robotId is null or empty");
+                return false;
+            }
+
+            if (newSegments == null || newSegments.Count == 0)
+            {
+                _logger.Warning($"Cannot send OrderUpdate: no new segments provided for robot {robotId}");
+                return false;
+            }
+
+            // Resolve IRobotManagerService lazily to avoid circular dependency
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotManager = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
+            var robotController = robotManager.GetRobotController(robotId);
+            if (robotController == null)
+            {
+                _logger.Warning($"Cannot send OrderUpdate: robot controller not found for robot {robotId}");
+                return false;
+            }
+
+            // Get current order from RobotController
+            var currentOrder = robotController.Data.Order;
+            if (currentOrder == null)
+            {
+                _logger.Warning($"Cannot send OrderUpdate: no current order found for robot {robotId}");
+                return false;
+            }
+
+            // Create OrderUpdate (keep same orderId, increment orderUpdateId)
+            var orderUpdate = new RobotNet.VDA5050.Order.OrderMsg
+            {
+                OrderId = currentOrder.OrderId,
+                OrderUpdateId = currentOrder.OrderUpdateId + 1,
+                SerialNumber = robotId,
+                Timestamp = DateTime.UtcNow,
+                Version = currentOrder.Version,
+                Manufacturer = currentOrder.Manufacturer
+            };
+
+            // Copy existing nodes and edges from current order
+            var existingNodes = currentOrder.Nodes?.ToList() ?? [];
+            var existingEdges = currentOrder.Edges?.ToList() ?? [];
+
+            // Get MapId for NodePosition (once for all segments)
+            var mapId = await GetMapIdForRobotAsync(robotId);
+            if (string.IsNullOrEmpty(mapId))
+            {
+                _logger.Warning($"Cannot get MapId for robot {robotId} to create NodePosition");
+                return false;
+            }
+
+            // Add new nodes and edges from segments (using VdaNode and VdaEdge directly)
+            foreach (var segment in newSegments)
+            {
+                if (segment.VdaNode != null)
+                {
+                    // Clone node and set released = true for new segments
+                    var node = CloneNode(segment.VdaNode);
+                    node.Released = true;
+                    existingNodes.Add(node);
+                }
+
+                if (segment.VdaEdge != null)
+                {
+                    // Clone edge and set released = true for new segments
+                    var edge = CloneEdge(segment.VdaEdge);
+                    edge.Released = true;
+                    existingEdges.Add(edge);
+                }
+            }
+
+            // Set arrays
+            orderUpdate.Nodes = [.. existingNodes];
+            orderUpdate.Edges = [.. existingEdges];
+
+            // Send via RobotController
+            var sent = await robotController.SendOrderAsync(orderUpdate, cancellationToken);
+            if (sent.IsSuccess)
+            {
+                _logger.Info($"Sent OrderUpdate (ID: {orderUpdate.OrderUpdateId}) to robot {robotId} with {newSegments.Count} new segments");
+            }
+
+            return sent.IsSuccess;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error sending OrderUpdate to robot {robotId}: {ex.Message}");
+            return false;
+        }
+    }
+
+    public async Task<bool> SendInitialOrderAsync(
+        string robotId,
+        RobotRoute route,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            // Resolve IRobotManagerService lazily to avoid circular dependency
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotManager = scope.ServiceProvider.GetRequiredService<IRobotManagerService>();
+            var robotController = robotManager.GetRobotController(robotId);
+            if (robotController == null)
+            {
+                _logger.Warning($"Cannot send initial Order: robot controller not found for robot {robotId}");
+                return false;
+            }
+
+            // Get MapId (LevelId) for NodePosition
+            var mapId = await GetMapIdForRobotAsync(robotId);
+            if (string.IsNullOrEmpty(mapId))
+            {
+                _logger.Warning($"Cannot get MapId for robot {robotId} to create NodePosition");
+                return false;
+            }
+
+            // Build nodes and edges from Base segments
+            var nodes = new List<RobotNet.VDA5050.Order.Node>();
+            var edges = new List<RobotNet.VDA5050.Order.Edge>();
+
+            foreach (var segment in route.Base)
+            {
+                if (segment.VdaNode != null)
+                {
+                    // Node segment with full information
+                    // Base segments are always released
+                    var node = CloneNode(segment.VdaNode);
+                    node.Released = true; // Base segments are always released
+                    nodes.Add(node);
+                }
+
+                if (segment.VdaEdge != null)
+                {
+                    // Edge segment with full information
+                    var edge = CloneEdge(segment.VdaEdge);
+                    edge.Released = true; // Base segments are always released
+                    edges.Add(edge);
+                }
+            }
+
+            // Create initial Order
+            var order = new RobotNet.VDA5050.Order.OrderMsg
+            {
+                OrderId = route.OrderId,
+                OrderUpdateId = 0, // Initial order has OrderUpdateId = 0
+                SerialNumber = robotId,
+                Timestamp = DateTime.UtcNow,
+                Nodes = [.. nodes],
+                Edges = [.. edges]
+            };
+
+            // Send via RobotController
+            var sent = await robotController.SendOrderAsync(order, cancellationToken);
+            if (sent.IsSuccess)
+            {
+                route.OrderUpdateId = 0; // Ensure OrderUpdateId is set
+                _logger.Info($"Sent initial Order (ID: {route.OrderId}, UpdateID: 0) to robot {robotId} with {route.Base.Count} base segments");
+            }
+
+            return sent.IsSuccess;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error sending initial Order to robot {robotId}: {ex.Message}");
+            return false;
+        }
+    }
+
+    /// <summary>
+    /// Generate OrderUpdate from Horizon segments
+    /// </summary>
+    private async Task<RobotNet.VDA5050.Order.OrderMsg?> GenerateOrderUpdateFromHorizonAsync(RobotRoute route, string robotId)
+    {
+        try
+        {
+            // Get MapId (LevelId) for NodePosition
+            var mapId = await GetMapIdForRobotAsync(robotId);
+            if (string.IsNullOrEmpty(mapId))
+            {
+                _logger.Warning($"Cannot get MapId for robot {robotId} to create NodePosition");
+                return null;
+            }
+
+            // Get last node of Base (decision point)
+            var lastBaseNode = route.Base.LastOrDefault(s => s.VdaNode != null);
+            if (lastBaseNode == null && route.Horizon.Count > 0)
+            {
+                // If no Base, use first node of Horizon
+                lastBaseNode = route.Horizon.FirstOrDefault(s => s.VdaNode != null);
+            }
+
+            if (lastBaseNode?.VdaNode == null)
+            {
+                _logger.Warning("Cannot generate OrderUpdate: no base node found");
+                return null;
+            }
+
+            // Build nodes and edges from Horizon
+            var nodes = new List<RobotNet.VDA5050.Order.Node>();
+            var edges = new List<RobotNet.VDA5050.Order.Edge>();
+
+            // Start with last base node (released) - Base nodes cannot have actions
+            var lastBaseVdaNode = CloneNode(lastBaseNode.VdaNode);
+            lastBaseVdaNode.Released = true;
+            nodes.Add(lastBaseVdaNode);
+
+            // Add Horizon segments (only Horizon nodes can have wait actions)
+            foreach (var segment in route.Horizon)
+            {
+                if (segment.VdaNode != null)
+                {
+                    // Node segment with full information
+                    var node = CloneNode(segment.VdaNode);
+                    nodes.Add(node);
+                }
+                else if (segment.VdaEdge != null)
+                {
+                    // Edge segment with full information
+                    var edge = CloneEdge(segment.VdaEdge);
+                    edges.Add(edge);
+                }
+            }
+
+            // Create OrderUpdate
+            var orderUpdate = new RobotNet.VDA5050.Order.OrderMsg
+            {
+                OrderId = route.OrderId,
+                OrderUpdateId = route.OrderUpdateId + 1, // Increment for new update
+                Nodes = [.. nodes],
+                Edges = [.. edges]
+            };
+
+            return orderUpdate;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error generating OrderUpdate from Horizon: {ex.Message}");
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Clone VDA5050 Node (create a copy to avoid modifying original)
+    /// </summary>
+    private static RobotNet.VDA5050.Order.Node CloneNode(RobotNet.VDA5050.Order.Node source)
+    {
+        return new RobotNet.VDA5050.Order.Node
+        {
+            NodeId = source.NodeId,
+            SequenceId = source.SequenceId,
+            Released = source.Released,
+            NodeDescription = source.NodeDescription,
+            NodePosition = source.NodePosition is null ? null : new RobotNet.VDA5050.Order.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] // Clone array
+        };
+    }
+
+    /// <summary>
+    /// Clone VDA5050 Edge (create a copy to avoid modifying original)
+    /// </summary>
+    private static RobotNet.VDA5050.Order.Edge CloneEdge(RobotNet.VDA5050.Order.Edge source)
+    {
+        return new RobotNet.VDA5050.Order.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 RobotNet.VDA5050.Order.Trajectory
+            {
+                Degree = source.Trajectory.Degree,
+                KnotVector = [.. source.Trajectory.KnotVector], // Clone array
+                ControlPoints = [.. source.Trajectory.ControlPoints.Select(cp => new RobotNet.VDA5050.Order.ControlPoint
+                {
+                    X = cp.X,
+                    Y = cp.Y,
+                    Weight = cp.Weight
+                })]
+            },
+            Corridor = source.Corridor == null ? null : new RobotNet.VDA5050.Order.Corridor
+            {
+                LeftWidth = source.Corridor.LeftWidth,
+                RightWidth = source.Corridor.RightWidth,
+                CorridorRefPoint = source.Corridor.CorridorRefPoint
+            },
+            Actions = [.. source.Actions] // Clone array
+        };
+    }
+
+    /// <summary>
+    /// Get MapId (LevelId) for a robot to use in NodePosition
+    /// </summary>
+    private async Task<string> GetMapIdForRobotAsync(string robotId)
+    {
+        try
+        {
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
+
+            var robot = await robotService.GetByRobotIdAsync(robotId);
+            if (robot == null || robot.MapId == null)
+            {
+                _logger.Warning($"Robot {robotId} not found or has no MapId assigned");
+                return string.Empty;
+            }
+
+            // Robot.MapId is the LevelId, convert to string for MapId in NodePosition
+            return robot.MapId.ToString() ?? "";
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error getting MapId for robot {robotId}: {ex.Message}");
+            return string.Empty;
+        }
+    }
+
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/PriorityService.cs

@@ -0,0 +1,169 @@
+using RobotNet10.FleetManager.Services;
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing robot priorities
+/// </summary>
+public class PriorityService(Logger<PriorityService> logger) : IPriorityService
+{
+    private readonly Logger<PriorityService> _logger = logger;
+
+    // Robot priorities
+    private readonly Dictionary<string, RobotPriority> _robotPriorities = [];
+    private readonly Lock _prioritiesLock = new();
+
+    public Task<bool> SetRobotPriorityAsync(string robotId, RobotPriority priority)
+    {
+        try
+        {
+            if (string.IsNullOrWhiteSpace(robotId))
+            {
+                _logger.Warning("Cannot set priority: robotId is null or empty");
+                return Task.FromResult(false);
+            }
+
+            if (priority == null)
+            {
+                _logger.Warning($"Cannot set priority for robot {robotId}: priority is null");
+                return Task.FromResult(false);
+            }
+
+            // Map PriorityReason to PriorityLevel if not set
+            if (priority.PriorityLevel == 0 && priority.Reason != PriorityReason.Default)
+            {
+                priority.PriorityLevel = GetPriorityLevelFromReason(priority.Reason);
+            }
+
+            // Ensure RobotId matches
+            priority.RobotId = robotId;
+
+            lock (_prioritiesLock)
+            {
+                _robotPriorities[robotId] = priority;
+            }
+
+            _logger.Info($"Set priority for robot {robotId}: Level={priority.PriorityLevel}, Reason={priority.Reason}");
+            return Task.FromResult(true);
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error setting priority for robot {robotId}: {ex.Message}");
+            return Task.FromResult(false);
+        }
+    }
+
+    public Task<RobotPriority> GetRobotPriorityAsync(string robotId)
+    {
+        var priority = GetRobotPriority(robotId);
+        return Task.FromResult(priority);
+    }
+
+    public Task<bool> RemoveRobotPriorityAsync(string robotId)
+    {
+        try
+        {
+            lock (_prioritiesLock)
+            {
+                if (_robotPriorities.Remove(robotId))
+                {
+                    _logger.Info($"Removed priority for robot {robotId}");
+                    return Task.FromResult(true);
+                }
+            }
+
+            _logger.Debug($"No priority found for robot {robotId} to remove");
+            // Return true even if no priority exists (graceful handling)
+            return Task.FromResult(true);
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error removing priority for robot {robotId}: {ex.Message}");
+            return Task.FromResult(false);
+        }
+    }
+
+    public void CleanupExpiredPriorities()
+    {
+        try
+        {
+            var now = DateTime.UtcNow;
+            var expiredRobots = new List<string>();
+
+            lock (_prioritiesLock)
+            {
+                foreach (var (robotId, priority) in _robotPriorities)
+                {
+                    if (priority.ValidUntil.HasValue && priority.ValidUntil.Value < now)
+                    {
+                        expiredRobots.Add(robotId);
+                    }
+                }
+
+                foreach (var robotId in expiredRobots)
+                {
+                    _robotPriorities.Remove(robotId);
+                    _logger.Debug($"Removed expired priority for robot {robotId}");
+                }
+            }
+
+            if (expiredRobots.Count > 0)
+            {
+                _logger.Info($"Cleaned up {expiredRobots.Count} expired priorities");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error cleaning up expired priorities: {ex.Message}");
+        }
+    }
+
+    /// <summary>
+    /// Get priority level from PriorityReason
+    /// </summary>
+    private static int GetPriorityLevelFromReason(PriorityReason reason)
+    {
+        return reason switch
+        {
+            PriorityReason.Emergency => 100,
+            PriorityReason.HighValueOrder => 50,
+            PriorityReason.TimeCritical => 30,
+            PriorityReason.ManualOverride => 75, // Between Emergency and HighValueOrder
+            PriorityReason.Default => 0,
+            _ => 0
+        };
+    }
+
+    /// <summary>
+    /// Get robot priority (private helper method)
+    /// </summary>
+    private RobotPriority GetRobotPriority(string robotId)
+    {
+        lock (_prioritiesLock)
+        {
+            if (_robotPriorities.TryGetValue(robotId, out var priority))
+            {
+                // Check if priority has expired
+                if (priority.ValidUntil.HasValue && priority.ValidUntil.Value < DateTime.UtcNow)
+                {
+                    _logger.Debug($"Priority for robot {robotId} has expired, removing");
+                    _robotPriorities.Remove(robotId);
+                }
+                else
+                {
+                    return priority;
+                }
+            }
+        }
+
+        // Return default priority if not found
+        return new RobotPriority
+        {
+            RobotId = robotId,
+            PriorityLevel = 0,
+            Reason = PriorityReason.Default
+        };
+    }
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/RobotInfoService.cs

@@ -0,0 +1,220 @@
+using RobotNet10.FleetManager.Events;
+using RobotNet10.FleetManager.Events.Events;
+using RobotNet10.FleetManager.Services.RobotManager;
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing robot information cache
+/// Uses event-based cache invalidation for accuracy and performance
+/// </summary>
+public class RobotInfoService : IRobotInfoService
+{
+    private readonly Logger<RobotInfoService> _logger;
+    private readonly IServiceScopeFactory _serviceScopeFactory;
+    private readonly IRobotEventBus? _eventBus;
+
+    // Robot static information cache (from RobotModel)
+    // Only cache static info: Length, Width, NavigationPoint
+    // Dynamic info (CurrentX, CurrentY, etc.) is always fetched fresh
+    // Cache is invalidated via events when RobotModel is updated
+    private readonly Dictionary<string, (double Length, double Width, double NavigationPointX, double NavigationPointY)> _staticInfoCache = [];
+    private readonly Lock _staticInfoCacheLock = new();
+
+    public RobotInfoService(
+        Logger<RobotInfoService> logger,
+        IServiceScopeFactory serviceScopeFactory,
+        IRobotEventBus? eventBus = null)
+    {
+        _logger = logger;
+        _serviceScopeFactory = serviceScopeFactory;
+        _eventBus = eventBus;
+
+        // Subscribe to events for automatic cache invalidation
+        if (_eventBus != null)
+        {
+            _eventBus.RobotModelUpdated += OnRobotModelUpdated;
+            _eventBus.RobotModelIdChanged += OnRobotModelIdChanged;
+        }
+        else
+        {
+            _logger.Warning("IRobotEventBus not available - cache invalidation via events disabled");
+        }
+    }
+
+    /// <summary>
+    /// Event handler: RobotModel updated - invalidate cache for all affected robots
+    /// </summary>
+    private void OnRobotModelUpdated(object? sender, RobotModelUpdatedEvent e)
+    {
+        try
+        {
+            if (e.AffectedRobotIds == null || e.AffectedRobotIds.Count == 0)
+            {
+                // If no specific robot IDs, clear all cache (safe but less efficient)
+                _logger.Warning($"RobotModelUpdated event for ModelId {e.ModelId} has no AffectedRobotIds - clearing all cache");
+                ClearAllRobotInfoCache();
+            }
+            else
+            {
+                // Clear cache only for affected robots
+                lock (_staticInfoCacheLock)
+                {
+                    foreach (var robotId in e.AffectedRobotIds)
+                    {
+                        if (_staticInfoCache.Remove(robotId))
+                        {
+                            _logger.Debug($"Invalidated cache for robot {robotId} due to RobotModel {e.ModelId} update");
+                        }
+                    }
+                }
+                _logger.Info($"Invalidated cache for {e.AffectedRobotIds.Count} robot(s) due to RobotModel {e.ModelId} update");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error handling RobotModelUpdated event: {ex.Message}");
+        }
+    }
+
+    /// <summary>
+    /// Event handler: Robot's ModelId changed - invalidate cache for that robot
+    /// </summary>
+    private void OnRobotModelIdChanged(object? sender, RobotModelIdChangedEvent e)
+    {
+        try
+        {
+            if (string.IsNullOrWhiteSpace(e.RobotId))
+            {
+                return;
+            }
+
+            lock (_staticInfoCacheLock)
+            {
+                if (_staticInfoCache.Remove(e.RobotId))
+                {
+                    _logger.Info($"Invalidated cache for robot {e.RobotId} due to ModelId change (from {e.PreviousModelId} to {e.NewModelId})");
+                }
+                else
+                {
+                    _logger.Debug($"Cache for robot {e.RobotId} was not found (may not have been cached yet)");
+                }
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error handling RobotModelIdChanged event: {ex.Message}");
+        }
+    }
+
+    public async Task<RobotInfo?> GetRobotInfoAsync(string robotId, CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            // Step 1: Get static info (from cache or database)
+            // Cache is invalidated via events, so if it exists, it's valid
+            double length = 0, width = 0, navPointX = 0, navPointY = 0;
+            bool needToFetch = true;
+            
+            lock (_staticInfoCacheLock)
+            {
+                if (_staticInfoCache.TryGetValue(robotId, out var cachedStaticInfo))
+                {
+                    // Cache exists and is valid (invalidated via events)
+                    length = cachedStaticInfo.Length;
+                    width = cachedStaticInfo.Width;
+                    navPointX = cachedStaticInfo.NavigationPointX;
+                    navPointY = cachedStaticInfo.NavigationPointY;
+                    needToFetch = false;
+                    _logger.Debug($"Using cached static info for robot {robotId}");
+                }
+            }
+
+            // If cache miss, fetch from database
+            if (needToFetch)
+            {
+                using var scope = _serviceScopeFactory.CreateScope();
+                var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
+                var robotModelService = scope.ServiceProvider.GetRequiredService<IRobotModelService>();
+
+                // Get robot from database
+                var robot = await robotService.GetByRobotIdAsync(robotId);
+                if (robot == null)
+                {
+                    _logger.Warning($"Robot {robotId} not found in database");
+                    return null;
+                }
+
+                // Get robot model
+                var robotModel = await robotModelService.GetByIdAsync(robot.ModelId);
+                if (robotModel == null)
+                {
+                    _logger.Warning($"RobotModel {robot.ModelId} not found for robot {robotId}");
+                    return null;
+                }
+
+                // Extract static info
+                length = robotModel.Length;
+                width = robotModel.Width;
+                navPointX = robotModel.NavigationPointX;
+                navPointY = robotModel.NavigationPointY;
+
+                // Cache static info (no timestamp needed - invalidated via events)
+                lock (_staticInfoCacheLock)
+                {
+                    _staticInfoCache[robotId] = (length, width, navPointX, navPointY);
+                }
+                
+                _logger.Debug($"Fetched and cached fresh static info for robot {robotId}");
+            }
+
+            // Step 2: Get dynamic info (ALWAYS fresh from RobotManager - never cached)
+            // Resolve IRobotManagerService lazily to avoid circular dependency
+            using var scopeForRobotManager = _serviceScopeFactory.CreateScope();
+            var robotManager = scopeForRobotManager.ServiceProvider.GetRequiredService<IRobotManagerService>();
+            var robotController = robotManager.GetRobotController(robotId);
+            var currentState = robotController?.Data?.State;
+
+            // Create RobotInfo with static info (cached) + dynamic info (fresh)
+            var robotInfo = new RobotInfo
+            {
+                RobotId = robotId,
+                Length = length,
+                Width = width,
+                NavigationPointX = navPointX,
+                NavigationPointY = navPointY,
+                // Dynamic info - always fresh, never cached
+                CurrentX = currentState?.AgvPosition?.X ?? 0.0,
+                CurrentY = currentState?.AgvPosition?.Y ?? 0.0,
+                CurrentTheta = currentState?.AgvPosition?.Theta ?? 0.0,
+                LastNodeId = currentState?.LastNodeId ?? string.Empty
+            };
+
+            _logger.Debug($"Retrieved robot info for {robotId}: Length={robotInfo.Length}, Width={robotInfo.Width}, NavPoint=({robotInfo.NavigationPointX}, {robotInfo.NavigationPointY}), Position=({robotInfo.CurrentX}, {robotInfo.CurrentY})");
+
+            return robotInfo;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error getting robot info for robot {robotId}: {ex.Message}");
+            return null;
+        }
+    }
+
+    public void ClearRobotInfoCache(string robotId)
+    {
+        lock (_staticInfoCacheLock)
+        {
+            _staticInfoCache.Remove(robotId);
+        }
+    }
+
+    public void ClearAllRobotInfoCache()
+    {
+        lock (_staticInfoCacheLock)
+        {
+            _staticInfoCache.Clear();
+        }
+    }
+}

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/RoutePlanningService.cs

@@ -0,0 +1,649 @@
+using RobotNet10.FleetManager.Data;
+using RobotNet10.FleetManager.Services.ConfigManager;
+using RobotNet10.FleetManager.Services.RobotManager;
+using RobotNet10.FleetManager.Services.TrafficControl.ACS;
+using RobotNet10.FleetManager.Services.TrafficControl.Helpers;
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+using RobotNet10.FleetManager.Shared.Enums;
+using RobotNet10.GlobalPathPlanner;
+using RobotNet10.GlobalPathPlanner.Model;
+using RobotNet10.MapManager.Data;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for route planning
+/// </summary>
+public class RoutePlanningService(
+    Logger<RoutePlanningService> logger,
+    IPathPlannerFactory pathPlannerFactory,
+    IServiceScopeFactory serviceScopeFactory,
+    IBaseHorizonManagementService baseHorizonManagementService,
+    IEdgeReservationService edgeReservationService,
+    IRouteStorageService routeStorageService,
+    IOrderUpdateService orderUpdateService,
+    IOrderControlService orderACSControl,
+    ITrafficConfig trafficConfig) : IRoutePlanningService
+{
+    private readonly Logger<RoutePlanningService> _logger = logger;
+    private readonly IPathPlannerFactory _pathPlannerFactory = pathPlannerFactory;
+    private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
+    private readonly IBaseHorizonManagementService _baseHorizonManagementService = baseHorizonManagementService;
+    private readonly IEdgeReservationService _edgeReservationService = edgeReservationService;
+    private readonly IRouteStorageService _routeStorageService = routeStorageService;
+    private readonly IOrderUpdateService _orderUpdateService = orderUpdateService;
+    private readonly IOrderControlService _orderACSControl = orderACSControl;
+    private readonly ITrafficConfig _trafficConfig = trafficConfig;
+
+    public async Task<RobotRoute?> PlanRouteAsync(
+        string robotId,
+        Guid startNodeId,
+        Guid goalNodeId,
+        CancellationToken cancellationToken = default)
+    {
+        return await PlanRouteAsync(robotId, startNodeId, goalNodeId, null, null, null, cancellationToken);
+    }
+
+    public async Task<RobotRoute?> PlanRouteAsync(
+        string robotId,
+        Guid startNodeId,
+        Guid goalNodeId,
+        double? goalAngle,
+        Orientation? startDirection,
+        Orientation? finalDirection,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            _logger.Info($"PlanRouteAsync called for robot {robotId} from {startNodeId} to {goalNodeId}" +
+                (goalAngle.HasValue ? $", goalAngle={goalAngle}°" : "") +
+                (startDirection.HasValue ? $", startDirection={startDirection}" : "") +
+                (finalDirection.HasValue ? $", finalDirection={finalDirection}" : ""));
+
+            // 1. Get robot current state
+            // Resolve IRobotManagerService lazily to avoid circular dependency
+            using var scopeForRobotManager = _serviceScopeFactory.CreateScope();
+            var robotManager = scopeForRobotManager.ServiceProvider.GetRequiredService<IRobotManagerService>();
+            var robotController = robotManager.GetRobotController(robotId);
+            if (robotController == null)
+            {
+                _logger.Warning($"Robot {robotId} not found");
+                return null;
+            }
+
+            var currentState = robotController.Data.State;
+            if (currentState == null)
+            {
+                _logger.Warning($"Robot {robotId} has no state");
+                return null;
+            }
+
+            // Check AgvPosition - must not be null
+            if (currentState.AgvPosition == null)
+            {
+                _logger.Warning($"Robot {robotId} has no AgvPosition in state");
+                return null;
+            }
+
+            // 2. Get levelId, vehicleTypeId, robotModelId, and navigationType
+            var (levelId, vehicleTypeId, robotModelId, navigationType) = await GetLevelIdAndVehicleTypeIdForRobotAsync(robotId);
+            if (levelId == null)
+            {
+                _logger.Error($"Cannot determine levelId for robot {robotId}");
+                return null;
+            }
+
+            // If vehicleTypeId is null, treat it like levelId null (return null)
+            if (!vehicleTypeId.HasValue)
+            {
+                _logger.Error($"Cannot determine vehicleTypeId for robot {robotId}");
+                return null;
+            }
+
+            // NavigationType must be set
+            if (!navigationType.HasValue)
+            {
+                _logger.Error($"Cannot determine NavigationType for robot {robotId}");
+                return null;
+            }
+
+            // 3. Get graph data from MapManager using RobotModelMapService
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotModelMapService = scope.ServiceProvider.GetRequiredService<IRobotModelMapService>();
+
+            List<Node> nodes;
+            List<Edge> edges;
+
+            try
+            {
+                _logger.Info($"Getting filtered nodes and edges for robot {robotId} (RobotModelId={robotModelId}, LevelId={levelId.Value}, VehicleTypeId={vehicleTypeId.Value})");
+
+                nodes = await robotModelMapService.GetFilteredNodesByLevelAsync(robotModelId, levelId.Value);
+                edges = await robotModelMapService.GetFilteredEdgesByLevelAsync(robotModelId, levelId.Value);
+
+                _logger.Info($"Found {nodes.Count} nodes and {edges.Count} edges filtered by VehicleTypeId={vehicleTypeId.Value}");
+            }
+            catch (Exception ex)
+            {
+                _logger.Error($"Error getting filtered nodes/edges for robot {robotId}: {ex.Message}");
+                return null;
+            }
+
+            if (nodes.Count == 0 || edges.Count == 0)
+            {
+                _logger.Warning($"No nodes or edges found for level {levelId}");
+                return null;
+            }
+
+            // 4. Convert to GlobalNode and GlobalEdge
+            var globalNodes = MapDataConverter.ToGlobalNodes(nodes, levelId.Value);
+            var globalEdges = MapDataConverter.ToGlobalEdges(edges, levelId.Value, vehicleTypeId.Value);
+
+            // 5. Create planner based on NavigationType (already retrieved above)
+            IPathPlanner planner = navigationType.Value switch
+            {
+                NavigationType.Differential => _pathPlannerFactory.CreateDifferentialPlanner(),
+                NavigationType.Forklift => _pathPlannerFactory.CreateForkliftPlanner(),
+                NavigationType.OmniDrive => _pathPlannerFactory.CreateOmniDrivePlanner(),
+                _ => _pathPlannerFactory.CreateDifferentialPlanner() // Default fallback
+            };
+
+            _logger.Info($"Using {navigationType.Value} planner for robot {robotId}");
+            planner.SetData(globalNodes, globalEdges);
+
+            // 6. Get path planning method from config based on NavigationType
+            var pathPlanningConfig = _trafficConfig.GetTrafficControlConfig().PathPlanning;
+            var planningMethod = pathPlanningConfig.NavigationTypeMethodMapping.TryGetValue(navigationType.Value, out var method)
+                ? method
+                : pathPlanningConfig.DefaultMethod;
+
+            _logger.Info($"Using path planning method {planningMethod} for NavigationType {navigationType.Value} (robot {robotId})");
+
+            // 7. Calculate path using selected method with optional constraints
+            // AgvPosition is already checked to be not null above
+            var currentTheta = currentState.AgvPosition.Theta; // radians, convert to degrees if needed
+            var thetaDegrees = currentTheta * 180.0 / Math.PI;
+
+            var (pathNodes, pathEdges) = ExecutePathPlanning(
+                planner,
+                planningMethod,
+                currentState.AgvPosition.X,
+                currentState.AgvPosition.Y,
+                thetaDegrees,
+                goalNodeId,
+                goalAngle,
+                startDirection,
+                finalDirection,
+                cancellationToken);
+
+            if (pathNodes.Length == 0)
+            {
+                _logger.Warning($"No path found from position ({currentState.AgvPosition.X:F2}, {currentState.AgvPosition.Y:F2}) to {goalNodeId}");
+                return null;
+            }
+
+            // 8. Convert A* result to RobotRoute with vehicleTypeId for VehicleProperties
+            var mapIdString = levelId.Value.ToString();
+            var route = RouteConverter.ConvertToRobotRoute(robotId, pathNodes, pathEdges, nodes, edges, goalAngle, _logger, vehicleTypeId, mapIdString);
+
+            // 9. Calculate safe base size (without conflicts)
+            var safeBaseSize = await _baseHorizonManagementService.CalculateSafeBaseSizeAsync(robotId, route, cancellationToken);
+
+            // Ensure at least 1 segment in Horizon
+            if (safeBaseSize >= route.FullRoute.Count)
+            {
+                safeBaseSize = Math.Max(1, route.FullRoute.Count - 1);
+            }
+
+            // 10. Split into Base and Horizon with safe base size
+            RouteConverter.SplitRouteIntoBaseAndHorizon(route, safeBaseSize);
+
+            // 11. Reserve edges for Base segments
+            var orderId = route.OrderId;
+            if (string.IsNullOrEmpty(orderId))
+            {
+                orderId = $"ORDER_{robotId}_{DateTime.UtcNow:yyyyMMddHHmmss}";
+                route.OrderId = orderId;
+            }
+
+            var reserveSuccess = await _edgeReservationService.ReserveEdgesAsync(robotId, orderId, route.Base, cancellationToken);
+            if (!reserveSuccess)
+            {
+                _logger.Warning($"Failed to reserve edges for robot {robotId}, but route is still created");
+            }
+
+            // 12. Store route
+            await _routeStorageService.UpdateRobotRouteAsync(robotId, route);
+
+            // 13. Send initial Order to robot (Base segments only, Horizon will be sent later)
+            var orderSent = await _orderUpdateService.SendInitialOrderAsync(robotId, route, cancellationToken);
+            if (!orderSent)
+            {
+                _logger.Warning($"Failed to send initial Order to robot {robotId}, but route is still stored");
+            }
+
+            _logger.Info($"Route planned successfully for robot {robotId}: {route.Base.Count} base segments, {route.Horizon.Count} horizon segments. Edges reserved: {reserveSuccess}, Order sent: {orderSent}");
+
+            return route;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error planning route for robot {robotId}: {ex.Message}");
+            return null;
+        }
+    }
+
+    public async Task<RobotRoute?> PlanRouteFromPositionAsync(
+        string robotId,
+        double x,
+        double y,
+        double theta,
+        Guid goalNodeId,
+        double? goalAngle,
+        Orientation? startDirection,
+        Orientation? finalDirection,
+        CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            _logger.Info($"PlanRouteFromPositionAsync called for robot {robotId} from position ({x:F2}, {y:F2}, {theta:F2}°) to {goalNodeId}" +
+                (goalAngle.HasValue ? $", goalAngle={goalAngle}°" : "") +
+                (startDirection.HasValue ? $", startDirection={startDirection}" : "") +
+                (finalDirection.HasValue ? $", finalDirection={finalDirection}" : ""));
+
+            // 1. Get levelId, vehicleTypeId, robotModelId, and navigationType
+            var (levelId, vehicleTypeId, robotModelId, navigationType) = await GetLevelIdAndVehicleTypeIdForRobotAsync(robotId);
+            if (levelId == null)
+            {
+                _logger.Error($"Cannot determine levelId for robot {robotId}");
+                return null;
+            }
+
+            // If vehicleTypeId is null, treat it like levelId null (return null)
+            if (!vehicleTypeId.HasValue)
+            {
+                _logger.Error($"Cannot determine vehicleTypeId for robot {robotId}");
+                return null;
+            }
+
+            // NavigationType must be set
+            if (!navigationType.HasValue)
+            {
+                _logger.Error($"Cannot determine NavigationType for robot {robotId}");
+                return null;
+            }
+
+            // 2. Get graph data from MapManager using RobotModelMapService
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotModelMapService = scope.ServiceProvider.GetRequiredService<IRobotModelMapService>();
+
+            List<Node> nodes;
+            List<Edge> edges;
+
+            try
+            {
+                _logger.Info($"Getting filtered nodes and edges for robot {robotId} (RobotModelId={robotModelId}, LevelId={levelId.Value}, VehicleTypeId={vehicleTypeId.Value})");
+
+                nodes = await robotModelMapService.GetFilteredNodesByLevelAsync(robotModelId, levelId.Value);
+                edges = await robotModelMapService.GetFilteredEdgesByLevelAsync(robotModelId, levelId.Value);
+
+                _logger.Info($"Found {nodes.Count} nodes and {edges.Count} edges filtered by VehicleTypeId={vehicleTypeId.Value}");
+            }
+            catch (Exception ex)
+            {
+                _logger.Error($"Error getting filtered nodes/edges for robot {robotId}: {ex.Message}");
+                return null;
+            }
+
+            if (nodes.Count == 0 || edges.Count == 0)
+            {
+                _logger.Warning($"No nodes or edges found for level {levelId}");
+                return null;
+            }
+
+            // 3. Convert to GlobalNode and GlobalEdge
+            var globalNodes = MapDataConverter.ToGlobalNodes(nodes, levelId.Value);
+            var globalEdges = MapDataConverter.ToGlobalEdges(edges, levelId.Value, vehicleTypeId.Value);
+
+            // 4. Create planner based on NavigationType
+            IPathPlanner planner = navigationType.Value switch
+            {
+                NavigationType.Differential => _pathPlannerFactory.CreateDifferentialPlanner(),
+                NavigationType.Forklift => _pathPlannerFactory.CreateForkliftPlanner(),
+                NavigationType.OmniDrive => _pathPlannerFactory.CreateOmniDrivePlanner(),
+                _ => _pathPlannerFactory.CreateDifferentialPlanner() // Default fallback
+            };
+
+            _logger.Info($"Using {navigationType.Value} planner for robot {robotId}");
+            planner.SetData(globalNodes, globalEdges);
+
+            // 5. Get path planning method from config based on NavigationType
+            var pathPlanningConfig = _trafficConfig.GetTrafficControlConfig().PathPlanning;
+            var planningMethod = pathPlanningConfig.NavigationTypeMethodMapping.TryGetValue(navigationType.Value, out var method)
+                ? method
+                : pathPlanningConfig.DefaultMethod;
+
+            _logger.Info($"Using path planning method {planningMethod} for NavigationType {navigationType.Value} (robot {robotId})");
+
+            // 6. Calculate path using selected method with optional constraints
+            // theta is already in degrees
+            var (pathNodes, pathEdges) = ExecutePathPlanning(
+                planner,
+                planningMethod,
+                x,
+                y,
+                theta,
+                goalNodeId,
+                goalAngle,
+                startDirection,
+                finalDirection,
+                cancellationToken);
+
+            if (pathNodes.Length == 0)
+            {
+                _logger.Warning($"No path found from position ({x:F2}, {y:F2}) to {goalNodeId}");
+                return null;
+            }
+
+            // 7. Convert A* result to RobotRoute with vehicleTypeId for VehicleProperties
+            var mapIdString = levelId.Value.ToString();
+            var route = RouteConverter.ConvertToRobotRoute(robotId, pathNodes, pathEdges, nodes, edges, goalAngle, _logger, vehicleTypeId, mapIdString);
+
+            // 8. Calculate safe base size (without conflicts)
+            var safeBaseSize = await _baseHorizonManagementService.CalculateSafeBaseSizeAsync(robotId, route, cancellationToken);
+
+            // Ensure at least 1 segment in Horizon
+            if (safeBaseSize >= route.FullRoute.Count)
+            {
+                safeBaseSize = Math.Max(1, route.FullRoute.Count - 1);
+            }
+
+            // 9. Split into Base and Horizon with safe base size
+            RouteConverter.SplitRouteIntoBaseAndHorizon(route, route.FullRoute.Count);
+
+            // 10. Reserve edges for Base segments
+            var orderId = route.OrderId;
+            if (string.IsNullOrEmpty(orderId))
+            {
+                orderId = $"ORDER_{robotId}_{DateTime.UtcNow:yyyyMMddHHmmss}";
+                route.OrderId = orderId;
+            }
+
+            var reserveSuccess = await _edgeReservationService.ReserveEdgesAsync(robotId, orderId, route.Base, cancellationToken);
+            if (!reserveSuccess)
+            {
+                _logger.Warning($"Failed to reserve edges for robot {robotId}, but route is still created");
+            }
+
+            // 11. Store route
+            await _routeStorageService.UpdateRobotRouteAsync(robotId, route);
+
+            // 12. Send initial Order to robot (Base segments only, Horizon will be sent later)
+            var orderSent = await _orderUpdateService.SendInitialOrderAsync(robotId, route, cancellationToken);
+            if (!orderSent)
+            {
+                _logger.Warning($"Failed to send initial Order to robot {robotId}, but route is still stored");
+            }
+
+            _logger.Info($"Route planned successfully for robot {robotId}: {route.Base.Count} base segments, {route.Horizon.Count} horizon segments. Edges reserved: {reserveSuccess}, Order sent: {orderSent}");
+
+            return route;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error planning route from position for robot {robotId}: {ex.Message}");
+            return null;
+        }
+    }
+
+    public async Task<RobotRoute?> PlanRouteFromPositionACSTrafficAsync(string robotId, double x, double y, double theta, Guid goalNodeId, double? goalAngle, Orientation? startDirection, Orientation? finalDirection, CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            _logger.Info($"PlanRouteFromPositionAsync called for robot {robotId} from position ({x:F2}, {y:F2}, {theta:F2}°) to {goalNodeId}" +
+                (goalAngle.HasValue ? $", goalAngle={goalAngle}°" : "") +
+                (startDirection.HasValue ? $", startDirection={startDirection}" : "") +
+                (finalDirection.HasValue ? $", finalDirection={finalDirection}" : ""));
+
+            // 1. Get levelId, vehicleTypeId, robotModelId, and navigationType
+            var (levelId, vehicleTypeId, robotModelId, navigationType) = await GetLevelIdAndVehicleTypeIdForRobotAsync(robotId);
+            if (levelId == null)
+            {
+                _logger.Error($"Cannot determine levelId for robot {robotId}");
+                return null;
+            }
+
+            // If vehicleTypeId is null, treat it like levelId null (return null)
+            if (!vehicleTypeId.HasValue)
+            {
+                _logger.Error($"Cannot determine vehicleTypeId for robot {robotId}");
+                return null;
+            }
+
+            // NavigationType must be set
+            if (!navigationType.HasValue)
+            {
+                _logger.Error($"Cannot determine NavigationType for robot {robotId}");
+                return null;
+            }
+
+            // 2. Get graph data from MapManager using RobotModelMapService
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotModelMapService = scope.ServiceProvider.GetRequiredService<IRobotModelMapService>();
+
+            List<Node> nodes;
+            List<Edge> edges;
+
+            try
+            {
+                _logger.Info($"Getting filtered nodes and edges for robot {robotId} (RobotModelId={robotModelId}, LevelId={levelId.Value}, VehicleTypeId={vehicleTypeId.Value})");
+
+                nodes = await robotModelMapService.GetFilteredNodesByLevelAsync(robotModelId, levelId.Value);
+                edges = await robotModelMapService.GetFilteredEdgesByLevelAsync(robotModelId, levelId.Value);
+
+                _logger.Info($"Found {nodes.Count} nodes and {edges.Count} edges filtered by VehicleTypeId={vehicleTypeId.Value}");
+            }
+            catch (Exception ex)
+            {
+                _logger.Error($"Error getting filtered nodes/edges for robot {robotId}: {ex.Message}");
+                return null;
+            }
+
+            if (nodes.Count == 0 || edges.Count == 0)
+            {
+                _logger.Warning($"No nodes or edges found for level {levelId}");
+                return null;
+            }
+
+            // 3. Convert to GlobalNode and GlobalEdge
+            var globalNodes = MapDataConverter.ToGlobalNodes(nodes, levelId.Value);
+            var globalEdges = MapDataConverter.ToGlobalEdges(edges, levelId.Value, vehicleTypeId.Value);
+
+            // 4. Create planner based on NavigationType
+            IPathPlanner planner = navigationType.Value switch
+            {
+                NavigationType.Differential => _pathPlannerFactory.CreateDifferentialPlanner(),
+                NavigationType.Forklift => _pathPlannerFactory.CreateForkliftPlanner(),
+                NavigationType.OmniDrive => _pathPlannerFactory.CreateOmniDrivePlanner(),
+                _ => _pathPlannerFactory.CreateDifferentialPlanner() // Default fallback
+            };
+
+            _logger.Info($"Using {navigationType.Value} planner for robot {robotId}");
+            planner.SetData(globalNodes, globalEdges);
+
+            // 5. Get path planning method from config based on NavigationType
+            var pathPlanningConfig = _trafficConfig.GetTrafficControlConfig().PathPlanning;
+            var planningMethod = pathPlanningConfig.NavigationTypeMethodMapping.TryGetValue(navigationType.Value, out var method)
+                ? method
+                : pathPlanningConfig.DefaultMethod;
+
+            _logger.Info($"Using path planning method {planningMethod} for NavigationType {navigationType.Value} (robot {robotId})");
+
+            // 6. Calculate path using selected method with optional constraints
+            // theta is already in degrees
+            var (pathNodes, pathEdges) = ExecutePathPlanning(
+                planner,
+                planningMethod,
+                x,
+                y,
+                theta,
+                goalNodeId,
+                goalAngle,
+                startDirection,
+                finalDirection,
+                cancellationToken);
+
+            if (pathNodes.Length == 0)
+            {
+                _logger.Warning($"No path found from position ({x:F2}, {y:F2}) to {goalNodeId}");
+                return null;
+            }
+
+            // 7. Convert A* result to RobotRoute with vehicleTypeId for VehicleProperties
+            var mapIdString = levelId.Value.ToString();
+            var route = RouteConverter.ConvertToRobotRoute(robotId, pathNodes, pathEdges, nodes, edges, goalAngle, _logger, vehicleTypeId, mapIdString);
+
+            // xử lí order 
+            var createOrder = await _orderACSControl.CreateRobotOrderAsync(robotId, route);
+            if (!createOrder)
+            {
+                _logger.Warning($"Failed to send initial Order to robot {robotId}");
+                return null;
+            }
+            else _logger.Info($"Route planned successfully for robot {robotId}: {route.Base.Count} base segments");
+
+            return route;
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error planning route from position for robot {robotId}: {ex.Message}");
+            return null;
+        }
+    }
+
+    /// <summary>
+    /// Get levelId, VehicleTypeId, RobotModelId, and NavigationType for a robot from Robot.MapId and RobotModel in database
+    /// </summary>
+    private async Task<(Guid? levelId, Guid? vehicleTypeId, Guid robotModelId, NavigationType? navigationType)> GetLevelIdAndVehicleTypeIdForRobotAsync(string robotId)
+    {
+        try
+        {
+            // Use IServiceScopeFactory to create a scope for Scoped services
+            using var scope = _serviceScopeFactory.CreateScope();
+            var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
+            var appContext = scope.ServiceProvider.GetRequiredService<ApplicationDbContext>();
+
+            var robot = await robotService.GetByRobotIdAsync(robotId);
+            if (robot == null)
+            {
+                _logger.Warning($"Robot {robotId} not found in database");
+                return (null, null, Guid.Empty, null);
+            }
+
+            if (robot.MapId == null)
+            {
+                _logger.Warning($"Robot {robotId} has no MapId assigned");
+                return (null, null, Guid.Empty, null);
+            }
+
+            // Robot.MapId is the levelId (LayoutLevel.Id)
+            var levelId = robot.MapId;
+
+            // Get VehicleTypeId and NavigationType from RobotModel
+            Guid? vehicleTypeId = null;
+            Guid robotModelId = robot.ModelId;
+            NavigationType? navigationType = null;
+
+            if (robot.ModelId != Guid.Empty)
+            {
+                var robotModel = await appContext.RobotModels.FindAsync(robot.ModelId);
+                if (robotModel != null)
+                {
+                    navigationType = robotModel.NavigationType;
+                    if (robotModel.VehicleTypeId.HasValue)
+                    {
+                        vehicleTypeId = robotModel.VehicleTypeId.Value;
+                        _logger.Info($"Robot {robotId} has VehicleTypeId={vehicleTypeId.Value} and NavigationType={navigationType} from RobotModel {robotModel.ModelName}");
+                    }
+                    else
+                    {
+                        _logger.Info($"Robot {robotId} has no VehicleTypeId assigned in RobotModel, but NavigationType={navigationType}");
+                    }
+                }
+                else
+                {
+                    _logger.Info($"Robot {robotId} has no RobotModel found");
+                }
+            }
+
+            return (levelId, vehicleTypeId, robotModelId, navigationType);
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error getting levelId and VehicleTypeId for robot {robotId}: {ex.Message}");
+            return (null, null, Guid.Empty, null);
+        }
+    }
+
+    /// <summary>
+    /// Execute path planning using the specified method with optional constraints
+    /// </summary>
+    private (GlobalNode[] Nodes, GlobalEdge[] Edges) ExecutePathPlanning(
+        IPathPlanner planner,
+        PathPlanningMethod method,
+        double x,
+        double y,
+        double theta,
+        Guid goalId,
+        double? goalAngle,
+        Orientation? startDirection,
+        Orientation? finalDirection,
+        CancellationToken? cancellationToken)
+    {
+        // Priority: Explicit parameters > Config method > Basic
+        // If explicit parameters are provided, use them regardless of config method
+
+        if (goalAngle.HasValue)
+        {
+            // Use angle constraint if provided
+            _logger.Info($"Using PathPlanningWithAngle with goalAngle={goalAngle.Value}°");
+            return planner.PathPlanningWithAngle(x, y, theta, goalId, goalAngle.Value, cancellationToken);
+        }
+
+        if (finalDirection.HasValue && finalDirection.Value != Orientation.NONE)
+        {
+            // Use final direction constraint if provided
+            _logger.Info($"Using PathPlanningWithFinalDirection with finalDirection={finalDirection.Value}");
+            return planner.PathPlanningWithFinalDirection(x, y, theta, goalId, finalDirection.Value, cancellationToken);
+        }
+
+        if (startDirection.HasValue && startDirection.Value != Orientation.NONE)
+        {
+            // Use start direction constraint if provided
+            _logger.Info($"Using PathPlanningWithStartDirection with startDirection={startDirection.Value}");
+            return planner.PathPlanningWithStartDirection(x, y, theta, goalId, startDirection.Value, cancellationToken);
+        }
+
+        // No explicit constraints, use method from config
+        switch (method)
+        {
+            case PathPlanningMethod.Basic:
+                return planner.PathPlanning(x, y, theta, goalId, cancellationToken);
+
+            case PathPlanningMethod.WithStartDirection:
+                return planner.PathPlanningWithStartDirection(x, y, theta, goalId, Orientation.NONE, cancellationToken);
+
+            case PathPlanningMethod.WithFinalDirection:
+                return planner.PathPlanningWithFinalDirection(x, y, theta, goalId, Orientation.NONE, cancellationToken);
+
+            case PathPlanningMethod.WithAngle:
+                // For WithAngle from config, use current theta as goal angle
+                _logger.Info($"Using PathPlanningMethod.WithAngle from config, using current theta {theta}° as goal angle");
+                return planner.PathPlanningWithAngle(x, y, theta, goalId, theta, cancellationToken);
+
+            default:
+                return planner.PathPlanning(x, y, theta, goalId, cancellationToken);
+        }
+    }
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/Services/RouteStorageService.cs

@@ -0,0 +1,47 @@
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+/// <summary>
+/// Service for managing active robot routes storage
+/// </summary>
+public class RouteStorageService : IRouteStorageService
+{
+    // In-memory storage for active routes
+    private readonly Dictionary<string, RobotRoute> _activeRoutes = [];
+    private readonly Lock _routesLock = new();
+
+    public Task<Dictionary<string, RobotRoute>> GetAllActiveRoutesAsync()
+    {
+        lock (_routesLock)
+        {
+            return Task.FromResult(new Dictionary<string, RobotRoute>(_activeRoutes));
+        }
+    }
+
+    public Task<RobotRoute?> GetRobotRouteAsync(string robotId)
+    {
+        lock (_routesLock)
+        {
+            _activeRoutes.TryGetValue(robotId, out var route);
+            return Task.FromResult(route);
+        }
+    }
+
+    public Task<bool> UpdateRobotRouteAsync(string robotId, RobotRoute route)
+    {
+        try
+        {
+            lock (_routesLock)
+            {
+                _activeRoutes[robotId] = route;
+            }
+            return Task.FromResult(true);
+        }
+        catch (Exception)
+        {
+            return Task.FromResult(false);
+        }
+    }
+}
+

srcs/RobotNet10/FleetManager/RobotNet10.FleetManager/Services/TrafficControl/TrafficControlService.cs

@@ -0,0 +1,377 @@
+using RobotNet10.FleetManager.Events;
+using RobotNet10.FleetManager.Events.Events;
+using RobotNet10.FleetManager.Services.ConfigManager;
+using RobotNet10.FleetManager.Services.TrafficControl.Models;
+using RobotNet10.FleetManager.Services.TrafficControl.Services;
+
+namespace RobotNet10.FleetManager.Services.TrafficControl;
+
+/// <summary>
+/// Orchestrator service for traffic control and conflict management between robots
+/// Delegates to specialized sub-services for actual implementation
+/// </summary>
+public class TrafficControlService : BackgroundService, ITrafficControlService
+{
+    private readonly Logger<TrafficControlService> _logger;
+    private readonly ITrafficConfig _trafficConfig;
+    private readonly IRobotEventBus? _eventBus;
+
+    // Sub-services (injected via constructor)
+    private readonly IRoutePlanningService _routePlanningService;
+    private readonly IConflictDetectionService _conflictDetectionService;
+    private readonly IConflictResolutionService _conflictResolutionService;
+    private readonly IBaseHorizonManagementService _baseHorizonManagementService;
+    private readonly IEdgeReservationService _edgeReservationService;
+    private readonly IPriorityService _priorityService;
+    private readonly IRobotInfoService _robotInfoService;
+    private readonly IRouteStorageService _routeStorageService;
+    private readonly IOrderUpdateService _orderUpdateService;
+
+    public TrafficControlService(
+        Logger<TrafficControlService> logger,
+        ITrafficConfig trafficConfig,
+        IRobotEventBus? eventBus,
+        IRoutePlanningService routePlanningService,
+        IConflictDetectionService conflictDetectionService,
+        IConflictResolutionService conflictResolutionService,
+        IBaseHorizonManagementService baseHorizonManagementService,
+        IEdgeReservationService edgeReservationService,
+        IPriorityService priorityService,
+        IRobotInfoService robotInfoService,
+        IRouteStorageService routeStorageService,
+        IOrderUpdateService orderUpdateService)
+    {
+        _logger = logger;
+        _trafficConfig = trafficConfig ?? throw new ArgumentNullException(nameof(trafficConfig));
+        _eventBus = eventBus;
+        _routePlanningService = routePlanningService;
+        _conflictDetectionService = conflictDetectionService;
+        _conflictResolutionService = conflictResolutionService;
+        _baseHorizonManagementService = baseHorizonManagementService;
+        _edgeReservationService = edgeReservationService;
+        _priorityService = priorityService;
+        _robotInfoService = robotInfoService;
+        _routeStorageService = routeStorageService;
+        _orderUpdateService = orderUpdateService;
+
+        // Subscribe to State messages if event bus is available
+        if (_eventBus != null)
+        {
+            _eventBus.StateMessageReceived += OnStateMessageReceived;
+            _logger.Info("Subscribed to State messages for robot progress monitoring");
+        }
+    }
+
+    #region ITrafficControlService Implementation
+
+    public async Task<RobotRoute?> PlanRouteAsync(
+        string robotId,
+        Guid startNodeId,
+        Guid goalNodeId,
+        CancellationToken cancellationToken = default)
+    {
+        return await _routePlanningService.PlanRouteAsync(robotId, startNodeId, goalNodeId, cancellationToken);
+    }
+
+    public async Task<RobotRoute?> PlanRouteAsync(
+        string robotId,
+        Guid startNodeId,
+        Guid goalNodeId,
+        double? goalAngle,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
+        CancellationToken cancellationToken = default)
+    {
+        return await _routePlanningService.PlanRouteAsync(robotId, startNodeId, goalNodeId, goalAngle, startDirection, finalDirection, cancellationToken);
+    }
+
+    public async Task<RobotRoute?> PlanRouteFromPositionAsync(
+        string robotId,
+        double x,
+        double y,
+        double theta,
+        Guid goalNodeId,
+        double? goalAngle,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
+        CancellationToken cancellationToken = default)
+    {
+        return await _routePlanningService.PlanRouteFromPositionAsync(robotId, x, y, theta, goalNodeId, goalAngle, startDirection, finalDirection, cancellationToken);
+    }
+
+    public async Task<RobotRoute?> PlanRouteFromPositionACSTrafficAsync(
+        string robotId,
+        double x,
+        double y,
+        double theta,
+        Guid goalNodeId,
+        double? goalAngle,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? startDirection,
+        RobotNet10.GlobalPathPlanner.Model.Orientation? finalDirection,
+        CancellationToken cancellationToken = default)
+    {
+        return await _routePlanningService.PlanRouteFromPositionACSTrafficAsync(robotId, x, y, theta, goalNodeId, goalAngle, startDirection, finalDirection, cancellationToken);
+    }
+
+    public async Task<List<Conflict>> DetectConflictsAsync(CancellationToken cancellationToken = default)
+    {
+        return await _conflictDetectionService.DetectConflictsAsync(cancellationToken);
+    }
+
+    public async Task<bool> ResolveConflictAsync(Conflict conflict, CancellationToken cancellationToken = default)
+    {
+        return await _conflictResolutionService.ResolveConflictAsync(conflict, cancellationToken);
+    }
+
+    public async Task<bool> ReleaseHorizonSegmentAsync(
+        string robotId,
+        int segmentCount,
+        CancellationToken cancellationToken = default)
+    {
+        return await _baseHorizonManagementService.ReleaseHorizonSegmentAsync(robotId, segmentCount, cancellationToken);
+    }
+
+    public async Task<bool> UpdateRobotRouteAsync(
+        string robotId,
+        RobotRoute newRoute,
+        CancellationToken cancellationToken = default)
+    {
+        return await _routeStorageService.UpdateRobotRouteAsync(robotId, newRoute);
+    }
+
+    public async Task<Dictionary<string, RobotRoute>> GetAllActiveRoutesAsync()
+    {
+        var routes = await _routeStorageService.GetAllActiveRoutesAsync();
+        return routes.ToDictionary(r => r.Key, r => r.Value);
+    }
+
+    public async Task<RobotRoute?> GetRobotRouteAsync(string robotId)
+    {
+        return await _routeStorageService.GetRobotRouteAsync(robotId);
+    }
+
+    public Task<bool> SetRobotPriorityAsync(string robotId, RobotPriority priority)
+    {
+        return _priorityService.SetRobotPriorityAsync(robotId, priority);
+    }
+
+    public Task<RobotPriority> GetRobotPriorityAsync(string robotId)
+    {
+        return _priorityService.GetRobotPriorityAsync(robotId);
+    }
+
+    public Task<bool> RemoveRobotPriorityAsync(string robotId)
+    {
+        return _priorityService.RemoveRobotPriorityAsync(robotId);
+    }
+
+    public async Task<List<Conflict>> EvaluateConflictsForResolutionAsync(
+        List<Conflict> conflicts,
+        CancellationToken cancellationToken = default)
+    {
+        return await _conflictResolutionService.EvaluateConflictsForResolutionAsync(conflicts, cancellationToken);
+    }
+
+    public async Task<bool> SendOrderUpdateAsync(
+        string robotId,
+        List<RouteSegment> newSegments,
+        CancellationToken cancellationToken = default)
+    {
+        return await _orderUpdateService.SendOrderUpdateAsync(robotId, newSegments, cancellationToken);
+    }
+
+    public async Task<bool> ReserveEdgesAsync(
+        string robotId,
+        string orderId,
+        List<RouteSegment> segments,
+        CancellationToken cancellationToken = default)
+    {
+        return await _edgeReservationService.ReserveEdgesAsync(robotId, orderId, segments, cancellationToken);
+    }
+
+    public async Task<List<EdgeReservation>> GetEdgeReservationsAsync(
+        Guid edgeId,
+        CancellationToken cancellationToken = default)
+    {
+        return await _edgeReservationService.GetEdgeReservationsAsync(edgeId, cancellationToken);
+    }
+
+    public async Task<bool> IsEdgeAvailableAsync(
+        Guid edgeId,
+        DateTime fromTime,
+        DateTime toTime,
+        CancellationToken cancellationToken = default)
+    {
+        return await _edgeReservationService.IsEdgeAvailableAsync(edgeId, fromTime, toTime, cancellationToken);
+    }
+
+    public async Task<bool> ReleaseReservationsAsync(
+        string robotId,
+        string orderId,
+        CancellationToken cancellationToken = default)
+    {
+        return await _edgeReservationService.ReleaseReservationsAsync(robotId, orderId, cancellationToken);
+    }
+
+    public async Task CheckAndReleaseHorizonsAsync(
+        string? robotId = null,
+        CancellationToken cancellationToken = default)
+    {
+        await _baseHorizonManagementService.CheckAndReleaseHorizonsAsync(robotId, cancellationToken);
+    }
+
+    #endregion
+
+    #region BackgroundService Implementation
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+        _logger.Info("TrafficControlService started");
+
+        var config = _trafficConfig.GetTrafficControlConfig();
+        var timer = new PeriodicTimer(TimeSpan.FromMilliseconds(config.ConflictDetection.IntervalMs));
+
+        try
+        {
+            while (await timer.WaitForNextTickAsync(stoppingToken))
+            {
+                try
+                {
+                    // Cleanup expired priorities periodically
+                    _priorityService.CleanupExpiredPriorities();
+
+                    // Check and release horizons for all robots
+                    await _baseHorizonManagementService.CheckAndReleaseHorizonsAsync(cancellationToken: stoppingToken);
+
+                    // Real-time conflict detection and resolution loop
+                    await ProcessConflictsRealTimeAsync(stoppingToken);
+                }
+                catch (Exception ex)
+                {
+                    _logger.Error($"Error in conflict detection loop: {ex.Message}");
+                }
+            }
+        }
+        catch (OperationCanceledException)
+        {
+            _logger.Info("TrafficControlService stopping");
+        }
+    }
+
+    #endregion
+
+    #region Event Handlers
+
+    /// <summary>
+    /// Handle State message received event - check if robot is near end of Base
+    /// </summary>
+    private void OnStateMessageReceived(object? sender, StateMessageReceivedEvent e)
+    {
+        try
+        {
+            var robotId = e.RobotId;
+            var stateMsg = e.StateMessage;
+
+            // Get robot route
+            var route = _routeStorageService.GetRobotRouteAsync(robotId).GetAwaiter().GetResult();
+            if (route == null)
+            {
+                return; // No active route, nothing to check
+            }
+
+            // Check if robot is near end of Base (1-2 segments remaining)
+            var remainingBaseSegments = _baseHorizonManagementService.CountRemainingBaseSegments(route, stateMsg);
+            if (remainingBaseSegments <= 2 && route.Horizon.Count > 0)
+            {
+                // Trigger horizon release check (async, fire and forget)
+                _ = Task.Run(async () =>
+                {
+                    try
+                    {
+                        await _baseHorizonManagementService.CheckAndReleaseHorizonsAsync(robotId, CancellationToken.None);
+                    }
+                    catch (Exception ex)
+                    {
+                        _logger.Error($"Error checking horizon release for robot {robotId} after state update: {ex.Message}");
+                    }
+                });
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error handling state message for robot {e.RobotId}: {ex.Message}");
+        }
+    }
+
+    #endregion
+
+    #region Private Helpers
+
+    /// <summary>
+    /// Process conflicts in real-time: detect, evaluate, and resolve
+    /// </summary>
+    private async Task ProcessConflictsRealTimeAsync(CancellationToken cancellationToken = default)
+    {
+        try
+        {
+            // 1. Detect all conflicts
+            var conflicts = await _conflictDetectionService.DetectConflictsAsync(cancellationToken);
+            if (conflicts == null || conflicts.Count == 0)
+            {
+                return; // No conflicts detected
+            }
+
+            _logger.Debug($"Detected {conflicts.Count} conflict(s) in real-time loop");
+
+            // 2. Evaluate conflicts for resolution optimization
+            var evaluatedConflicts = await _conflictResolutionService.EvaluateConflictsForResolutionAsync(conflicts, cancellationToken);
+            if (evaluatedConflicts == null || evaluatedConflicts.Count == 0)
+            {
+                return; // No conflicts to resolve
+            }
+
+            // 3. Resolve conflicts in priority order
+            var resolvedCount = 0;
+            var failedCount = 0;
+            var skippedCount = 0;
+
+            foreach (var conflict in evaluatedConflicts)
+            {
+                try
+                {
+                    var resolved = await _conflictResolutionService.ResolveConflictAsync(conflict, cancellationToken);
+                    var robotIds = string.Join(", ", conflict.InvolvedRobots);
+                    if (resolved)
+                    {
+                        resolvedCount++;
+                        _logger.Info($"Resolved conflict between {robotIds} (Type: {conflict.Type})");
+                    }
+                    else
+                    {
+                        failedCount++;
+                        _logger.Warning($"Failed to resolve conflict between {robotIds} (Type: {conflict.Type})");
+                    }
+                }
+                catch (Exception ex)
+                {
+                    failedCount++;
+                    var robotIds = string.Join(", ", conflict.InvolvedRobots);
+                    _logger.Error($"Error resolving conflict between {robotIds}: {ex.Message}");
+                }
+            }
+
+            if (resolvedCount > 0 || failedCount > 0)
+            {
+                _logger.Info($"Conflict resolution summary: {resolvedCount} resolved, {failedCount} failed, {skippedCount} skipped");
+            }
+        }
+        catch (Exception ex)
+        {
+            _logger.Error($"Error in ProcessConflictsRealTimeAsync: {ex.Message}");
+        }
+    }
+
+
+    #endregion
+}