using Microsoft.AspNetCore.SignalR;
using RobotNet.VDA5050.Order;
using RobotNet.VDA5050.State;
using RobotNet.VDA5050.Visualization;
using RobotNet10.Common;
using RobotNet10.Common.Models;
using RobotNet10.FleetManager.Services;
using RobotNet10.FleetManager.Services.RobotManager;
using RobotNet10.FleetManager.Services.TrafficControl;
using RobotNet10.FleetManager.Services.TrafficControl.Models;
using RobotNet10.FleetManager.Shared.DTOs.Robot;
using System.Collections.Concurrent;

namespace RobotNet10.FleetManager.Hubs;

/// <summary>
/// Hosted service for broadcasting robot state and visualization updates via SignalR.
/// Broadcasts updates at 1Hz (every 1 second) to all subscribed clients.
/// Also manages LevelId subscriptions for Monitor page.
/// </summary>
/// <remarks>
/// This service runs as a BackgroundService and periodically broadcasts
/// StateMsg and VisualizationMsg updates from RobotManagerService to subscribed clients.
/// </remarks>
public class RobotStateHubContext(
    IHubContext<RobotStateHub> hubContext,
    IRobotManagerService robotManagerService,
    IServiceScopeFactory serviceScopeFactory,
    Services.Logger<RobotStateHubContext> logger,
    IOrderControlService orderControlService,
    ILoggerFactory loggerFactory) : BackgroundService
{
    private readonly IHubContext<RobotStateHub> _hubContext = hubContext;
    private readonly IRobotManagerService _robotManagerService = robotManagerService;
    private readonly IServiceScopeFactory _serviceScopeFactory = serviceScopeFactory;
    private readonly Services.Logger<RobotStateHubContext> _logger = logger;
    private readonly ILoggerFactory _loggerFactory = loggerFactory;
    private readonly IOrderControlService _orderControlService = orderControlService;
    private WatchTimerAsync<RobotStateHubContext>? _broadcastTimer;
    private const int BroadcastIntervalMs = 500; // 1Hz = 1000ms

    // ===== MONITOR LEVEL SUBSCRIPTION MANAGEMENT =====
    private readonly ConcurrentDictionary<Guid, Queue<string>> _levelSubscriptions = new();
    private readonly ConcurrentDictionary<string, Guid> _connectionToLevel = new();
    private readonly Lock _subscriptionLock = new();
    private const int MaxConnectionsPerLevel = 5;

    /// <summary>
    /// Subscribe a connection to a levelId for monitor updates.
    /// If connection already subscribed to another level, unsubscribe from old level first.
    /// If level has 5 connections, the oldest connection will be evicted.
    /// </summary>
    /// <returns>ConnectionId that was evicted (if any), null otherwise</returns>
    public string? SubscribeToLevel(string connectionId, Guid levelId)
    {
        lock (_subscriptionLock)
        {
            // If connection already subscribed to another level, unsubscribe first
            if (_connectionToLevel.TryGetValue(connectionId, out var oldLevelId))
            {
                if (oldLevelId == levelId)
                {
                    // Already subscribed to this level, no change needed
                    return null;
                }
                UnsubscribeFromLevelInternal(connectionId, oldLevelId);
            }

            // Get or create queue for this level
            var queue = _levelSubscriptions.GetOrAdd(levelId, _ => new Queue<string>());

            // If queue is full, evict oldest connection
            string? evictedConnectionId = null;
            if (queue.Count >= MaxConnectionsPerLevel)
            {
                evictedConnectionId = queue.Dequeue();
                _connectionToLevel.TryRemove(evictedConnectionId, out _);
            }

            // Add new connection to queue
            queue.Enqueue(connectionId);
            _connectionToLevel[connectionId] = levelId;

            return evictedConnectionId;
        }
    }

    /// <summary>
    /// Unsubscribe a connection from its current level
    /// </summary>
    public bool UnsubscribeFromLevel(string connectionId)
    {
        lock (_subscriptionLock)
        {
            if (!_connectionToLevel.TryGetValue(connectionId, out var levelId))
            {
                return false; // Not subscribed
            }

            return UnsubscribeFromLevelInternal(connectionId, levelId);
        }
    }

    private bool UnsubscribeFromLevelInternal(string connectionId, Guid levelId)
    {
        if (!_levelSubscriptions.TryGetValue(levelId, out var queue))
        {
            return false;
        }

        // Remove connection from queue
        var tempQueue = new Queue<string>();
        bool found = false;
        while (queue.Count > 0)
        {
            var id = queue.Dequeue();
            if (id != connectionId)
            {
                tempQueue.Enqueue(id);
            }
            else
            {
                found = true;
            }
        }

        // Replace queue
        while (tempQueue.Count > 0)
        {
            queue.Enqueue(tempQueue.Dequeue());
        }

        // Remove connection mapping
        _connectionToLevel.TryRemove(connectionId, out _);

        // If queue is empty, remove level entry
        if (queue.Count == 0)
        {
            _levelSubscriptions.TryRemove(levelId, out _);
        }

        return found;
    }

    /// <summary>
    /// Get all connectionIds subscribed to a levelId
    /// </summary>
    public IReadOnlyList<string> GetConnectionsForLevel(Guid levelId)
    {
        lock (_subscriptionLock)
        {
            if (!_levelSubscriptions.TryGetValue(levelId, out var queue))
            {
                return [];
            }

            return [.. queue];
        }
    }

    /// <summary>
    /// Get levelId that a connection is subscribed to
    /// </summary>
    public Guid? GetLevelForConnection(string connectionId)
    {
        _connectionToLevel.TryGetValue(connectionId, out var levelId);
        return levelId;
    }

    /// <summary>
    /// Remove connection (called on disconnect)
    /// </summary>
    public void RemoveConnection(string connectionId)
    {
        UnsubscribeFromLevel(connectionId);
    }

    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
    {
        await Task.Yield();

        // Start broadcast timer at 1Hz
        _broadcastTimer = new WatchTimerAsync<RobotStateHubContext>(
            BroadcastIntervalMs,
            BroadcastAllRobots,
            _loggerFactory.CreateLogger<RobotStateHubContext>()
        );
        _broadcastTimer.Start();
        _logger.Info("Started robot state broadcast service at 1Hz");
    }

    private async Task BroadcastAllRobots()
    {
        try
        {
            // Get all robot data from RobotManagerService
            var allRobotData = _robotManagerService.GetAllRobotData();

            foreach (var (robotId, robotData) in allRobotData)
            {
                // Broadcast State if available
                if (robotData.State != null)
                {
                    await BroadcastStateUpdate(robotId, robotData.State);
                }

                // Broadcast Visualization if available
                if (robotData.Visualization != null && robotData.State != null)
                {
                    await BroadcastMonitorDataUpdate(robotId, robotData.Visualization, robotData.State);
                }
            }
        }
        catch (Exception ex)
        {
            _logger.Error($"Error in broadcast cycle: {ex.Message}");
        }
    }

    /// <summary>
    /// Broadcast state update to all clients subscribed to the robot
    /// </summary>
    /// <param name="robotId">Robot identifier (serialNumber)</param>
    /// <param name="state">VDA5050 State message</param>
    private async Task BroadcastStateUpdate(string robotId, StateMsg state)
    {
        try
        {
            var groupName = $"robot:{robotId}";
            await _hubContext.Clients.Group(groupName).SendAsync("OnStateUpdate", state);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error broadcasting state update for robot {robotId}: {ex.Message}");
        }
    }

    /// <summary>
    /// Broadcast visualization update to clients subscribed to the robot's levelId (for Monitor)
    /// Only broadcasts to connections that have subscribed to the robot's levelId.
    /// </summary>
    /// <param name="robotId">Robot identifier (serialNumber)</param>
    /// <param name="visualization">VDA5050 Visualization message</param>
    /// <param name="state">VDA5050 State message</param>
    private async Task BroadcastMonitorDataUpdate(string robotId, VisualizationMsg? visualization, StateMsg? state)
    {
        try
        {
            if (visualization is null || state is null) return;

            // Get robot's MapId (LevelId) from database
            Guid? levelId = await GetRobotLevelIdAsync(robotId);
            if (!levelId.HasValue)
            {
                // Robot has no MapId assigned, skip broadcast
                return;
            }

            // Get all connections subscribed to this levelId
            var connectionIds = GetConnectionsForLevel(levelId.Value);
            if (connectionIds.Count == 0)
            {
                // No connections subscribed to this levelId, skip broadcast
                return;
            }


            // Get robot path
            var robotPath = GetRobotPath(robotId, visualization.AgvPosition.X, visualization.AgvPosition.Y, state.NodeStates, state.LastNodeId);

            // Prepare broadcast data
            RobotMonitorBoardcastData data = new()
            {
                RobotId = robotId,
                AgvPosition = visualization.AgvPosition,
                AgvVelocity = visualization.Velocity,
                Battery = state.BatteryState,
                Errors = state.Errors,
                Infomations = state.Information,
                Loads = state.Loads,
                Path = robotPath,
            };

            // Broadcast to specific connections (not groups)
            await _hubContext.Clients.Clients(connectionIds).SendAsync("OnMonitorUpdate", data);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error broadcasting visualization update for robot {robotId}: {ex.Message}");
        }
    }

    /// <summary>
    /// Get robot's LevelId (MapId) from database
    /// </summary>
    private async Task<Guid?> GetRobotLevelIdAsync(string robotId)
    {
        try
        {
            using var scope = _serviceScopeFactory.CreateScope();
            var robotService = scope.ServiceProvider.GetRequiredService<IRobotService>();
            var robot = await robotService.GetByRobotIdAsync(robotId);
            return robot?.MapId; // MapId is the LevelId
        }
        catch (Exception ex)
        {
            _logger.Error($"Error getting LevelId for robot {robotId}: {ex.Message}");
            return null;
        }
    }
    /// <summary>
    /// Split edge at robot's current position, returning remaining edge segment(s)
    /// </summary>
    private static NavigationPathEdge[] SplitChecking(double robotX, double robotY, Node lastNode, Node nearLastNode, Edge edge)
    {
        List<NavigationPathEdge> pathEdges = [];
        
        var spaceEdge = new SpaceEdge
        {
            StartX = lastNode.NodePosition?.X ?? 0,
            StartY = lastNode.NodePosition?.Y ?? 0,
            EndX = nearLastNode.NodePosition?.X ?? 0,
            EndY = nearLastNode.NodePosition?.Y ?? 0,
            Degree = edge.Trajectory?.Degree ?? 1,
            ControlPoint1X = edge.Trajectory?.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].X : 0,
            ControlPoint1Y = edge.Trajectory?.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].Y : 0,
            ControlPoint2X = edge.Trajectory?.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[2].X : 0,
            ControlPoint2Y = edge.Trajectory?.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[2].Y : 0,
        };
        
        // Get projection point on edge and the time parameter
        var (projX, projY, _, projTime) = SpaceCompute.GetProjectionOnEdge(robotX, robotY, spaceEdge);
        
        // Clamp projTime to [0, 1]
        projTime = Math.Max(0, Math.Min(1, projTime));
        
        // If edge is degree 1, create a single linear edge
        if (spaceEdge.Degree == 1)
        {
            var linearEdge = new NavigationPathEdge
            {
                StartX = projX,
                StartY = projY,
                EndX = nearLastNode.NodePosition?.X ?? 0,
                EndY = nearLastNode.NodePosition?.Y ?? 0,
                Degree = 1
            };
            pathEdges.Add(linearEdge);
        }
        else
        {
            // For degree 2 or 3: sample points along the curve from projection point to end
            // Resolution: 0.2m per segment
            const double resolution = 0.2;
            
            // Sample points along the curve
            List<(double x, double y)> samplePoints = [];
            samplePoints.Add((projX, projY)); // Start from projection point
            
            double currentTime = projTime;
            var prevPoint = new SpaceNode(projX, projY);
            
            while (currentTime < 1.0)
            {
                // Find next sample point at resolution distance along the curve
                double nextTime = FindNextSampleTime(spaceEdge, currentTime, resolution);
                nextTime = Math.Min(1.0, nextTime);
                
                var nextPoint = SpaceCompute.BezierPoint(nextTime, spaceEdge);
                
                // Check if we've moved enough distance
                double segmentLength = Math.Sqrt(Math.Pow(nextPoint.X - prevPoint.X, 2) + 
                                                Math.Pow(nextPoint.Y - prevPoint.Y, 2));
                
                if (segmentLength >= resolution * 0.5 || nextTime >= 1.0)
                {
                    samplePoints.Add((nextPoint.X, nextPoint.Y));
                    prevPoint = nextPoint;
                    currentTime = nextTime;
                }
                else
                {
                    // If segment is too short, advance time slightly and try again
                    currentTime = Math.Min(1.0, currentTime + 0.01);
                }
                
                // Safety check to avoid infinite loop
                if (nextTime >= 1.0 || currentTime >= 1.0)
                {
                    break;
                }
            }
            
            // Ensure the last point is the end node
            if (samplePoints.Count == 0 || 
                Math.Abs(samplePoints[^1].x - nearLastNode.NodePosition?.X ?? 0) > 1e-6 ||
                Math.Abs(samplePoints[^1].y - nearLastNode.NodePosition?.Y ?? 0) > 1e-6)
            {
                samplePoints.Add((nearLastNode.NodePosition?.X ?? 0, nearLastNode.NodePosition?.Y ?? 0));
            }
            
            // Create linear edges between consecutive sample points
            for (int i = 0; i < samplePoints.Count - 1; i++)
            {
                var linearEdge = new NavigationPathEdge
                {
                    StartX = samplePoints[i].x,
                    StartY = samplePoints[i].y,
                    EndX = samplePoints[i + 1].x,
                    EndY = samplePoints[i + 1].y,
                    Degree = 1
                };
                pathEdges.Add(linearEdge);
            }
        }
        
        return [.. pathEdges];
    }
    
    /// <summary>
    /// Find next sample time at approximately resolution distance from current time along the curve
    /// </summary>
    private static double FindNextSampleTime(SpaceEdge edge, double currentTime, double resolution)
    {
        // Use binary search to find the time where distance from current point is approximately resolution
        double low = currentTime;
        double high = 1.0;
        double targetDistance = resolution;
        double tolerance = 0.01; // 1cm tolerance
        
        var startPoint = SpaceCompute.BezierPoint(currentTime, edge);
        
        // Binary search
        for (int iter = 0; iter < 20; iter++)
        {
            double mid = (low + high) / 2;
            var midPoint = SpaceCompute.BezierPoint(mid, edge);
            double distance = Math.Sqrt(Math.Pow(midPoint.X - startPoint.X, 2) + 
                                       Math.Pow(midPoint.Y - startPoint.Y, 2));
            
            if (Math.Abs(distance - targetDistance) < tolerance)
            {
                return mid;
            }
            
            if (distance < targetDistance)
            {
                low = mid;
            }
            else
            {
                high = mid;
            }
        }
        
        return (low + high) / 2;
    }

    /// <summary>
    /// Get robot navigation path (Base and Full) from current position
    /// </summary>
    private NavigationPath GetRobotPath(string robotId, double robotX, double robotY, NodeState[] nodestates, string lastNodeId)
    {
        try
        {
            var robotRoute = _orderControlService.GetRobotRoute(robotId);
            if (robotRoute is null || robotRoute.FullRoute.Count == 0) 
            {
                return new NavigationPath { NavigationState = "NoRoute" };
            }

            if (nodestates.Length == 0 || string.IsNullOrEmpty(lastNodeId))
            {
                return new NavigationPath { NavigationState = "NoRoute" };
            }

            var fullRoute = robotRoute.FullRoute;
            
            // Find last node segment index
            var lastNodeIndex = fullRoute.FindIndex(r => r.VdaNode != null && r.VdaNode.NodeId == lastNodeId);
            if (lastNodeIndex == -1)
            {
                // Last node not found, return full route from start
                return ConvertRouteToNavigationPath(robotRoute, 0, false, robotX, robotY);
            }

            // Check if robot is on an edge (between lastNodeIndex and next node)
            bool isOnEdge = false;
            int startIndex = lastNodeIndex;
            
            // Check if there's an edge segment after last node
            if (lastNodeIndex + 1 < fullRoute.Count && fullRoute[lastNodeIndex + 1].IsEdge)
            {
                var edgeSegment = fullRoute[lastNodeIndex + 1];
                var lastNode = fullRoute[lastNodeIndex].VdaNode!;
                
                // Find next node segment
                var nextNodeIndex = lastNodeIndex + 2;
                if (nextNodeIndex < fullRoute.Count && fullRoute[nextNodeIndex].IsNode)
                {
                    var nextNode = fullRoute[nextNodeIndex].VdaNode!;
                    
                    // Check if robot is between last node and next node (on the edge)
                    // Simple check: if robot is closer to edge than to either node
                    var distToLastNode = Math.Sqrt(Math.Pow(robotX - lastNode.NodePosition?.X ?? 0, 2) + Math.Pow(robotY - lastNode.NodePosition?.Y ?? 0, 2));
                    var distToNextNode = Math.Sqrt(Math.Pow(robotX - nextNode.NodePosition?.X ?? 0, 2) + Math.Pow(robotY - nextNode.NodePosition?.Y ?? 0, 2));
                    
                    // Get edge length
                    var edgeLength = Math.Sqrt(Math.Pow(nextNode.NodePosition?.X ?? 0 - lastNode.NodePosition?.X ?? 0, 2) + 
                                               Math.Pow(nextNode.NodePosition?.Y ?? 0 - lastNode.NodePosition?.Y ?? 0, 2));
                    
                    // If robot is closer to edge than to nodes, and not too far from edge
                    if (edgeLength > 0.1 && distToLastNode > 0.1 && distToNextNode > 0.1)
                    {
                        isOnEdge = true;
                        startIndex = lastNodeIndex + 1; // Start from edge segment
                    }
                }
            }

            // Convert route to navigation path starting from startIndex
            return ConvertRouteToNavigationPath(robotRoute, startIndex, isOnEdge, robotX, robotY, lastNodeIndex);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error getting robot path for {robotId}: {ex.Message}");
            return new NavigationPath { NavigationState = "Error" };
        }
    }

    /// <summary>
    /// Convert RobotRoute segments to NavigationPath (Base and Full)
    /// </summary>
    private static NavigationPath ConvertRouteToNavigationPath(
        RobotRoute robotRoute, 
        int startIndex, 
        bool isOnEdge, 
        double robotX, 
        double robotY,
        int? lastNodeIndex = null)
    {
        var fullPath = new List<NavigationPathEdge>();
        var basePath = new List<NavigationPathEdge>();
        var fullRoute = robotRoute.FullRoute;

        if (startIndex >= fullRoute.Count)
        {
            return new NavigationPath { NavigationState = "Complete" };
        }

        // If robot is on edge, split the edge
        if (isOnEdge && startIndex < fullRoute.Count && fullRoute[startIndex].IsEdge)
        {
            var edgeSegment = fullRoute[startIndex];
            if (lastNodeIndex.HasValue && lastNodeIndex.Value >= 0 && lastNodeIndex.Value < fullRoute.Count)
            {
                var lastNode = fullRoute[lastNodeIndex.Value].VdaNode;
                var nextNodeIndex = startIndex + 1;
                if (nextNodeIndex < fullRoute.Count && fullRoute[nextNodeIndex].IsNode && lastNode != null)
                {
                    var nextNode = fullRoute[nextNodeIndex].VdaNode;
                    if (nextNode != null && edgeSegment.VdaEdge != null)
                    {
                        // Split edge at robot position
                        var splitEdges = SplitChecking(robotX, robotY, lastNode, nextNode, edgeSegment.VdaEdge);
                        fullPath.AddRange(splitEdges);
                        
                        // Add to base path if segment is released
                        if (edgeSegment.Released)
                        {
                            basePath.AddRange(splitEdges);
                        }
                        
                        startIndex = nextNodeIndex + 1; // Move to segment after next node (skip the edge and next node)
                    }
                }
            }
        }
        else if (!isOnEdge && startIndex < fullRoute.Count && fullRoute[startIndex].IsNode)
        {
            // Robot is at a node, skip the node segment and start from next edge
            startIndex += 2;
        }

        // Convert remaining segments
        for (int i = startIndex; i < fullRoute.Count; i++)
        {
            var segment = fullRoute[i];
            
            if (segment.IsEdge && segment.VdaEdge != null)
            {
                // Find start and end nodes for this edge
                Node? startNode = null;
                Node? endNode = null;
                
                // Start node: previous segment should be a node
                if (i > 0 && fullRoute[i - 1].IsNode)
                {
                    startNode = fullRoute[i - 1].VdaNode;
                }
                
                // End node: next segment should be a node
                if (i + 1 < fullRoute.Count && fullRoute[i + 1].IsNode)
                {
                    endNode = fullRoute[i + 1].VdaNode;
                }
                
                if (startNode != null && endNode != null)
                {
                    var edge = ConvertEdgeSegmentToNavigationPathEdge(segment, startNode, endNode);
                    if (edge != null)
                    {
                        fullPath.Add(edge);
                        if (segment.Released)
                        {
                            basePath.Add(edge);
                        }
                    }
                }
            }
            // Note: Node segments don't create NavigationPathEdge, only edges do
        }

        return new NavigationPath
        {
            NavigationState = robotRoute.Base.Count > 0 ? "Active" : "Planning",
            RobotPath = [.. fullPath],
            RobotBasePath = [.. basePath]
        };
    }

    /// <summary>
    /// Convert RouteSegment (Edge) to NavigationPathEdge
    /// </summary>
    private static NavigationPathEdge? ConvertEdgeSegmentToNavigationPathEdge(RouteSegment segment, Node? startNode, Node? endNode)
    {
        if (!segment.IsEdge || segment.VdaEdge == null) return null;

        var edge = segment.VdaEdge;
        var trajectory = edge.Trajectory;

        // Get start and end positions from node positions (not from trajectory control points)
        if (startNode == null || endNode == null) return null;

        var navEdge = new NavigationPathEdge
        {
            StartX = startNode.NodePosition?.X ?? 0,
            StartY = startNode.NodePosition?.Y ?? 0,
            EndX = endNode.NodePosition?.X ?? 0,
            EndY = endNode.NodePosition?.Y ?? 0,
            Degree = trajectory?.Degree ?? 1,
        };

        // Set control points from trajectory (if available)
        if (trajectory != null && trajectory.ControlPoints.Length > 1)
        {
            // ControlPoints[1] is first control point, ControlPoints[2] is second control point (for cubic bezier)
            if (trajectory.ControlPoints.Length > 1)
            {
                navEdge.ControlPoint1X = trajectory.ControlPoints[1].X;
                navEdge.ControlPoint1Y = trajectory.ControlPoints[1].Y;
            }

            if (trajectory.ControlPoints.Length > 2)
            {
                navEdge.ControlPoint2X = trajectory.ControlPoints[2].X;
                navEdge.ControlPoint2Y = trajectory.ControlPoints[2].Y;
            }
        }

        return navEdge;
    }

    public override Task StopAsync(CancellationToken cancellationToken)
    {
        _broadcastTimer?.Dispose();
        _logger.Info("Stopped robot state broadcast service");
        return base.StopAsync(cancellationToken);
    }
}