using RobotNet.VDA5050;
using RobotNet.VDA5050.InstantAction;
using RobotNet.VDA5050.Order;
using RobotNet.VDA5050.Type;
using RobotNet10.FleetManager.Data;
using RobotNet10.FleetManager.Script;
using RobotNet10.FleetManager.Services.ConfigManager;
using RobotNet10.FleetManager.Services.RobotConnections;
using RobotNet10.FleetManager.Services.RobotManager.Models;
using RobotNet10.FleetManager.Services.TrafficControl;
using RobotNet10.FleetManager.Services.TrafficControl.Models;
using RobotNet10.MapManager.Services;
using RobotNet10.Shared;
using Action = RobotNet.VDA5050.InstantAction.Action;

namespace RobotNet10.FleetManager.Services.RobotController;

/// <summary>
/// RobotController - instance per robot
/// Mô hình hóa thông tin của 1 robot, định danh bằng RobotId (SerialNumber)
/// </summary>
public class RobotController : IRobotController
{
    private readonly IRobotConnectionsService _robotConnectionsService;
    private readonly IConnectionConfig _configManager;
    private readonly ITrafficControlService _trafficControlService;
    private readonly IServiceScopeFactory _serviceScopeFactory;
    private readonly Logger<RobotController> _logger;
    private readonly SemaphoreSlim _methodLock = new(1, 1);
    private readonly SemaphoreSlim _publishLock = new(1, 1);

    private uint _headerIdCounter = 0;
    private readonly Lock _headerIdLock = new();

    private readonly TimeSpan SendTimeOut = TimeSpan.FromSeconds(10);

    public string RobotId { get; }
    public RobotData Data { get; }
    public bool IsOnline => Data.ConnectionState == ConnectionState.ONLINE;
    public bool IsReady => IsOnline && !IsRobotBusy();

    public RobotController(
        string robotId,
        IRobotConnectionsService robotConnectionsService,
        IConnectionConfig configManager,
        ITrafficControlService trafficControlService,
        IServiceScopeFactory serviceScopeFactory,
        Logger<RobotController> logger)
    {
        RobotId = robotId;
        _robotConnectionsService = robotConnectionsService;
        _configManager = configManager;
        _trafficControlService = trafficControlService;
        _serviceScopeFactory = serviceScopeFactory;
        _logger = logger;

        Data = new RobotData
        {
            RobotId = robotId,
            ConnectionState = ConnectionState.OFFLINE,
            LastUpdated = DateTime.UtcNow
        };
    }

    public async Task<MessageResult> MoveToNodeAsync(string nodeName, double? angle, CancellationToken cancellationToken = default)
    {
        if (string.IsNullOrEmpty(nodeName))
        {
            _logger.Warning($"Cannot move to node: nodeName is null or empty for robot {RobotId}");
            return new(false, $"Cannot move to node: nodeName is null or empty for robot {RobotId}");
        }

        await _methodLock.WaitAsync(cancellationToken);
        try
        {
            // 1. Check if robot is busy with an order
            if (IsRobotBusy())
            {
                _logger.Warning($"Cannot move to node: robot {RobotId} is busy with an order {Data.State?.OrderId}");
                return new(false, $"Robot {RobotId} is busy with an order");
            }

            // 2. Get robot's levelId (MapId)
            var levelId = await GetRobotLevelIdAsync();
            if (levelId == null)
            {
                _logger.Warning($"Cannot move to node: robot {RobotId} has no MapId assigned");
                return new(false, $"Robot {RobotId} has no MapId assigned");
            }

            // 3. Find goal node by NodeName
            var goalNode = await FindNodeByNameAsync(levelId.Value, nodeName);
            if (goalNode == null)
            {
                _logger.Warning($"Cannot move to node: node '{nodeName}' not found in level {levelId}");
                return new(false, $"Node '{nodeName}' not found");
            }

            // 4. Plan route and send order
            return await PlanRouteAndSendOrderAsync(goalNode.Id, angle, cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error in MoveToNodeAsync for robot {RobotId}: {ex.Message}");
            return new(false, $"Error in MoveToNodeAsync for robot {RobotId}: {ex.Message}");
        }
        finally
        {
            _methodLock.Release();
        }
    }

    /// <summary>
    /// Move robot to a node by NodeId (string)
    /// </summary>
    public async Task<MessageResult> MoveToNodeByIdAsync(string nodeId, double? angle, CancellationToken cancellationToken = default)
    {
        if (string.IsNullOrEmpty(nodeId))
        {
            _logger.Warning($"Cannot move to node: nodeId is null or empty for robot {RobotId}");
            return new(false, $"Cannot move to node: nodeId is null or empty for robot {RobotId}");
        }

        await _methodLock.WaitAsync(cancellationToken);
        try
        {
            // 1. Check if robot is busy with an order
            if (IsRobotBusy())
            {
                _logger.Warning($"Cannot move to node: robot {RobotId} is busy with an order  {Data.State?.OrderId}");
                return new(false, $"Robot {RobotId} is busy with an order");
            }

            // 2. Get robot's levelId (MapId)
            var levelId = await GetRobotLevelIdAsync();
            if (levelId == null)
            {
                _logger.Warning($"Cannot move to node: robot {RobotId} has no MapId assigned");
                return new(false, $"Robot {RobotId} has no MapId assigned");
            }

            // 3. Find goal node by NodeId (string)
            var goalNode = await FindNodeByNodeIdAsync(levelId.Value, nodeId);
            if (goalNode == null)
            {
                _logger.Warning($"Cannot move to node: node with NodeId '{nodeId}' not found in level {levelId}");
                return new(false, $"Node with NodeId '{nodeId}' not found");
            }

            // 4. Plan route and send order
            return await PlanRouteAndSendOrderAsync(goalNode.Id, angle, cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error in MoveToNodeByIdAsync for robot {RobotId}: {ex.Message}");
            return new(false, $"Error in MoveToNodeByIdAsync for robot {RobotId}: {ex.Message}");
        }
        finally
        {
            _methodLock.Release();
        }
    }

    /// <summary>
    /// Move robot to a node by NodeId (Guid)
    /// </summary>
    public async Task<MessageResult> MoveToNodeByGuidAsync(Guid nodeId, double? angle, CancellationToken cancellationToken = default)
    {
        if (nodeId == Guid.Empty)
        {
            _logger.Warning($"Cannot move to node: nodeId is empty for robot {RobotId}");
            return new(false, $"Cannot move to node: nodeId is empty for robot {RobotId}");
        }

        await _methodLock.WaitAsync(cancellationToken);
        try
        {
            // 1. Check if robot is busy with an order
            if (IsRobotBusy())
            {
                _logger.Warning($"Cannot move to node: robot {RobotId} is busy with an order  {Data.State?.OrderId}");
                return new(false, $"Robot {RobotId} is busy with an order");
            }

            // 2. Verify node exists
            using var scope = _serviceScopeFactory.CreateScope();
            var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();
            var goalNode = await nodeService.GetByIdAsync(nodeId);
            if (goalNode == null)
            {
                _logger.Warning($"Cannot move to node: node with Id '{nodeId}' not found");
                return new(false, $"Node with Id '{nodeId}' not found");
            }

            // 3. Plan route and send order
            return await PlanRouteAndSendOrderAsync(nodeId, angle, cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error in MoveToNodeByGuidAsync for robot {RobotId}: {ex.Message}");
            return new(false, $"Error in MoveToNodeByGuidAsync for robot {RobotId}: {ex.Message}");
        }
        finally
        {
            _methodLock.Release();
        }
    }

    public Task<MessageResult> MoveToStationAsync(string nodeName, StationAction action, CancellationToken cancellationToken = default)
    {
        throw new NotImplementedException();
    }

    public async Task<MessageResult> SendInstantActionAsync(Action action, CancellationToken cancellationToken = default)
    {
        if (action == null)
        {
            _logger.Warning($"Cannot send instant action: action is null for robot {RobotId}");
            return new(false, $"Cannot send instant action: action is null for robot {RobotId}");
        }

        await _methodLock.WaitAsync(cancellationToken);
        try
        {
            var instantActions = new InstantActionsMsg
            {
                Actions = [action]
            };

            FillVDA5050Header(instantActions);
            var publish = await _robotConnectionsService.PublishInstantActionsAsync(RobotId, instantActions, cancellationToken);
            return new(publish);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error sending instant action to robot {RobotId}: {ex.Message}");
            return new(false, $"Error sending instant action to robot {RobotId}: {ex.Message}");
        }
        finally
        {
            _methodLock.Release();
        }
    }

    public async Task<MessageResult> SendOrderAsync(OrderMsg order, CancellationToken cancellationToken = default)
    {
        if (order == null)
        {
            _logger.Warning($"Cannot send order: order message is null for robot {RobotId}");
            return new(false, $"Cannot send order: order message is null for robot {RobotId}");
        }

        await _publishLock.WaitAsync(cancellationToken);
        try
        {
            // Fill header if not already set
            if (order.HeaderId == 0)
            {
                FillVDA5050Header(order);
            }
            else
            {
                // Ensure SerialNumber matches
                order.SerialNumber = RobotId;
            }

            var published = await _robotConnectionsService.PublishOrderAsync(RobotId, order, cancellationToken);

            // Update order in RobotData when successfully published
            if (published)
            {
                CancellationTokenSource cancelSend = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
                cancelSend.CancelAfter(SendTimeOut);
                while (true)
                {
                    if (cancelSend.IsCancellationRequested) return new(false, "Publish order failed timout");
                    if (Data.State is not null)
                    {
                        if (Data.State.OrderId == order.OrderId && IsRobotBusy() && Data.State.OrderUpdateId == order.OrderUpdateId)
                        {
                            Data.LastUpdated = DateTime.UtcNow;
                            Data.Order = order;
                            return new(true);
                        }
                    }
                }
            }

            return new(false, "Publish order failed");
        }
        catch (Exception ex)
        {
            _logger.Error($"Error sending order to robot {RobotId}: {ex.Message}");
            return new(false, $"Error sending order to robot {RobotId}");
        }
        finally
        {
            _publishLock.Release();
        }
    }

    public async Task<MessageResult> SendInstantActionsAsync(InstantActionsMsg instantActions, CancellationToken cancellationToken = default)
    {
        if (instantActions == null)
        {
            _logger.Warning($"Cannot send instant actions: instantActions message is null for robot {RobotId}");
            return new(false, $"Cannot send instant actions: instantActions message is null for robot {RobotId}");
        }

        if (instantActions.Actions == null || instantActions.Actions.Length == 0)
        {
            _logger.Warning($"Cannot send instant actions: actions array is null or empty for robot {RobotId}");
            return new(false, $"Cannot send instant actions: actions array is null or empty for robot {RobotId}");
        }

        await _publishLock.WaitAsync(cancellationToken);
        try
        {
            // Fill header if not already set
            if (instantActions.HeaderId == 0)
            {
                FillVDA5050Header(instantActions);
            }
            else
            {
                // Ensure SerialNumber matches
                instantActions.SerialNumber = RobotId;
            }

            var publish = await _robotConnectionsService.PublishInstantActionsAsync(RobotId, instantActions, cancellationToken);
            return new(publish);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error sending instant actions to robot {RobotId}: {ex.Message}");
            return new(false, $"Error sending instant actions to robot {RobotId}");
        }
        finally
        {
            _publishLock.Release();
        }
    }

    public async Task<MessageResult> RequestFactsheetAsync(CancellationToken cancellationToken = default)
    {
        await _methodLock.WaitAsync(cancellationToken);
        try
        {
            var action = new Action
            {
                ActionType = ActionType.FACTSHEET_REQUEST.ToJsonString(),
                ActionId = Guid.NewGuid().ToString(),
                BlockingType = BlockingType.NONE
            };

            return await SendInstantActionAsync(action, cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error requesting factsheet from robot {RobotId}: {ex.Message}");
            return new(false, $"Error requesting factsheet from robot {RobotId}");
        }
        finally
        {
            _methodLock.Release();
        }
    }

    public async Task<MessageResult> RequestStateAsync(CancellationToken cancellationToken = default)
    {
        await _methodLock.WaitAsync(cancellationToken);
        try
        {
            var action = new Action
            {
                ActionType = ActionType.STATE_REQUEST.ToJsonString(),
                ActionId = Guid.NewGuid().ToString(),
                BlockingType = BlockingType.NONE
            };

            return await SendInstantActionAsync(action, cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error requesting state from robot {RobotId}: {ex.Message}");
            return new(false, $"Error requesting state from robot {RobotId}");
        }
        finally
        {
            _methodLock.Release();
        }
    }

    public async Task<MessageResult> CancelOrderAsync(CancellationToken cancellationToken = default)
    {
        try
        {
            var action = new Action
            {
                ActionType = ActionType.CANCEL_ORDER.ToJsonString(),
                ActionId = Guid.NewGuid().ToString(),
                BlockingType = BlockingType.NONE,
                ActionDescription = "Cancel current order"
            };

            var result = await SendInstantActionAsync(action, cancellationToken);
            if (result.IsSuccess && Data.State != null)
            {
                // Clear order state immediately so a new order can be accepted without waiting
                // for the robot to report empty NodeStates/EdgeStates (avoids "robot is busy" after cancel).
                Data.State.NodeStates = [];
                Data.State.EdgeStates = [];
                Data.State.OrderId = string.Empty;
                Data.State.OrderUpdateId = 0;
                Data.OrderClearedByCancelAt = DateTime.UtcNow;
                _logger.Info($"Robot {RobotId}: cleared order state after cancel (ready for new order)");
            }
            return result;
        }
        catch (Exception ex)
        {
            _logger.Error($"Error canceling order for robot {RobotId}: {ex.Message}");
            return new(false, $"Error canceling order for robot {RobotId}");
        }
    }

    /// <summary>
    /// Check if robot is busy with an order
    /// </summary>
    private bool IsRobotBusy()
    {
        // Robot is busy if order status is Sent or Accepted
        return Data.State?.NodeStates.Length > 0 || Data.State?.EdgeStates.Length > 0;
    }

    /// <summary>
    /// Get robot's levelId (MapId) from database
    /// </summary>
    private async Task<Guid?> GetRobotLevelIdAsync()
    {
        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>
    /// Find node by NodeName in a level
    /// </summary>
    private async Task<RobotNet10.MapManager.Data.Node?> FindNodeByNameAsync(Guid levelId, string nodeName)
    {
        try
        {
            using var scope = _serviceScopeFactory.CreateScope();
            var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();

            // Get all nodes in level
            var nodes = await nodeService.GetNodesByLevelAsync(levelId);

            // Find by NodeName (case-insensitive)
            var node = nodes.FirstOrDefault(n =>
                !string.IsNullOrEmpty(n.NodeName) &&
                n.NodeName.Equals(nodeName, StringComparison.OrdinalIgnoreCase));

            return node;
        }
        catch (Exception ex)
        {
            _logger.Error($"Error finding node by name '{nodeName}' in level {levelId}: {ex.Message}");
            return null;
        }
    }

    /// <summary>
    /// Find node by NodeId (string) in a level
    /// </summary>
    private async Task<RobotNet10.MapManager.Data.Node?> FindNodeByNodeIdAsync(Guid levelId, string nodeId)
    {
        try
        {
            using var scope = _serviceScopeFactory.CreateScope();
            var nodeService = scope.ServiceProvider.GetRequiredService<INodeService>();

            // Get all nodes in level
            var nodes = await nodeService.GetNodesByLevelAsync(levelId);

            // Find by NodeId (case-insensitive)
            var node = nodes.FirstOrDefault(n =>
                n.NodeId.Equals(nodeId, StringComparison.OrdinalIgnoreCase));

            return node;
        }
        catch (Exception ex)
        {
            _logger.Error($"Error finding node by NodeId '{nodeId}' in level {levelId}: {ex.Message}");
            return null;
        }
    }

    /// <summary>
    /// Plan route and send order to robot
    /// </summary>
    private async Task<MessageResult> PlanRouteAndSendOrderAsync(Guid goalNodeId, double? angle, CancellationToken cancellationToken)
    {
        try
        {
            // Check if robot has state (required for route planning)
            if (Data.State == null || Data.State.AgvPosition == null)
            {
                _logger.Warning($"Cannot plan route: robot {RobotId} has no state or position");
                return new(false, $"Robot {RobotId} has no state or position");
            }

            // Cancel current order if exists
            if (IsRobotBusy())
            {
                return new(false, $"The robot is working on order {Data.State.OrderId}");
            }

            // Plan route using TrafficControlService from current position
            // Get current position from State.AgvPosition
            var currentX = Data.State.AgvPosition.X;
            var currentY = Data.State.AgvPosition.Y;
            var currentThetaRadians = Data.State.AgvPosition.Theta; // radians
            var currentThetaDegrees = currentThetaRadians * 180.0 / Math.PI; // convert to degrees

            // Plan route from current position to goal node
            RobotRoute? route = await _trafficControlService.PlanRouteFromPositionACSTrafficAsync(
                    RobotId,
                    currentX,
                    currentY,
                    currentThetaDegrees,
                    goalNodeId,
                    angle, // goalAngle in degrees
                    null, // startDirection
                    null, // finalDirection
                    cancellationToken);

            if (route == null)
            {
                _logger.Warning($"Cannot plan route to node {goalNodeId} for robot {RobotId}");
                return new(false, $"Failed to plan route to node {goalNodeId}");
            }
            return new(true);
        }
        catch (Exception ex)
        {
            _logger.Error($"Error planning route and sending order for robot {RobotId}: {ex.Message}");
            return new(false, $"Error planning route: {ex.Message}");
        }
    }

    private uint GetNextHeaderId()
    {
        lock (_headerIdLock)
        {
            _headerIdCounter++;
            if (_headerIdCounter == 0) // Handle overflow
            {
                _headerIdCounter = 1;
            }
            return _headerIdCounter;
        }
    }

    private void FillVDA5050Header(OrderMsg msg)
    {
        var config = _configManager.GetVDA5050Config();
        msg.HeaderId = GetNextHeaderId();
        msg.Timestamp = DateTime.UtcNow;
        msg.Version = config.Version;
        msg.Manufacturer = config.Manufacturer;
        msg.SerialNumber = RobotId;
    }

    private void FillVDA5050Header(InstantActionsMsg msg)
    {
        var config = _configManager.GetVDA5050Config();
        msg.HeaderId = 1;
        msg.Timestamp = DateTime.UtcNow;
        msg.Version = config.Version;
        msg.Manufacturer = config.Manufacturer;
        msg.SerialNumber = RobotId;
    }

    public void Dispose()
    {
        // Cleanup resources if needed
        _methodLock?.Dispose();
        // RobotData will be cleaned up by GC
        _logger.Debug($"RobotController disposed for robot {RobotId}");
        GC.SuppressFinalize(this);
    }
}