using System;
using RobotNet10.CANOpen.Models;
using RobotNet10.CANOpen.CiA402.Models;

namespace RobotNet10.CANOpen.CiA402.Strategies;

/// <summary>
/// Interface cho PDO multiplexing strategy
/// </summary>
public interface IPdoMultiplexingStrategy : IDisposable
{
    /// <summary>
    /// Configure PDO mappings for the strategy
    /// </summary>
    void ConfigurePdoMappings(byte nodeId, Action<PdoConfiguration> configureRpdo, Action<PdoConfiguration> configureTpdo);
    
    /// <summary>
    /// Handle incoming PDO data
    /// </summary>
    void HandlePdoData(byte pdoNumber, byte[] data, Action<int, byte[]> motorDataHandler);
    
    /// <summary>
    /// Send control data for specific motor
    /// </summary>
    void SendMotorControl(int motorIndex, byte[] controlData, Action<byte, byte[]> sendRpdo);
    
    /// <summary>
    /// Number of motors supported
    /// </summary>
    int MotorCount { get; }
}

/// <summary>
/// Time-multiplexed strategy - xoay vòng giữa các motors theo thời gian
/// </summary>
public class TimeMultiplexedStrategy : IPdoMultiplexingStrategy
{
    private int _currentMotorIndex = 0;
    private readonly Timer _rotationTimer;
    private readonly int _rotationIntervalMs;
    
    public int MotorCount { get; }
    
    public TimeMultiplexedStrategy(int motorCount, int rotationIntervalMs = 100)
    {
        MotorCount = motorCount;
        _rotationIntervalMs = rotationIntervalMs;
        _rotationTimer = new Timer(RotateMotor, null, rotationIntervalMs, rotationIntervalMs);
    }
    
    private void RotateMotor(object? state)
    {
        _currentMotorIndex = (_currentMotorIndex + 1) % MotorCount;
    }
    
    public void ConfigurePdoMappings(byte nodeId, Action<PdoConfiguration> configureRpdo, Action<PdoConfiguration> configureTpdo)
    {
        // RPDO1-3: Individual Controlwords
        for (byte i = 1; i <= Math.Min(MotorCount, 3); i++)
        {
            var rpdo = new PdoConfiguration(i, (uint)(0x200 + (i-1) * 0x100 + nodeId));
            // Motor controlword mapping would be added by caller
            configureRpdo(rpdo);
        }
        
        // RPDO4: Multiplexed position control
        var rpdo4 = new PdoConfiguration(4, (uint)(0x500 + nodeId));
        // Motor ID + Position data
        configureRpdo(rpdo4);
        
        // TPDO1-3: Individual Status + Position
        for (byte i = 1; i <= Math.Min(MotorCount, 3); i++)
        {
            var tpdo = new PdoConfiguration(i, (uint)(0x180 + (i-1) * 0x100 + nodeId));
            // Statusword + Position mapping would be added by caller
            configureTpdo(tpdo);
        }
        
        // TPDO4: Multiplexed velocity + torque (rotates between motors)
        var tpdo4 = new PdoConfiguration(4, (uint)(0x480 + nodeId));
        // Motor ID + Velocity + Torque
        configureTpdo(tpdo4);
    }
    
    public void HandlePdoData(byte pdoNumber, byte[] data, Action<int, byte[]> motorDataHandler)
    {
        if (pdoNumber <= 3)
        {
            // Direct mapping: PDO1 -> Motor 0, PDO2 -> Motor 1, PDO3 -> Motor 2
            int motorIndex = pdoNumber - 1;
            if (motorIndex < MotorCount)
            {
                motorDataHandler(motorIndex, data);
            }
        }
        else if (pdoNumber == 4)
        {
            // Multiplexed data - current motor được xác định bởi timer
            motorDataHandler(_currentMotorIndex, data);
        }
    }
    
    public void SendMotorControl(int motorIndex, byte[] controlData, Action<byte, byte[]> sendRpdo)
    {
        if (motorIndex < 3)
        {
            // Send via dedicated RPDO (1-3)
            sendRpdo((byte)(motorIndex + 1), controlData);
        }
        else
        {
            // Send via multiplexed RPDO4 với motor ID prefix
            var multiplexedData = new byte[controlData.Length + 1];
            multiplexedData[0] = (byte)motorIndex; // Motor ID
            controlData.CopyTo(multiplexedData, 1);
            
            sendRpdo(4, multiplexedData);
        }
    }
    
    public void Dispose()
    {
        _rotationTimer?.Dispose();
        GC.SuppressFinalize(this);
    }
}

/// <summary>
/// Priority-based strategy - motors có priority khác nhau
/// </summary>
public class PriorityBasedStrategy : IPdoMultiplexingStrategy
{
    private readonly int[] _motorPriorities;
    private readonly Dictionary<int, DateTime> _lastUpdateTimes;
    private readonly int _highPriorityIntervalMs;
    private readonly int _lowPriorityIntervalMs;
    
    public int MotorCount { get; }
    
    public PriorityBasedStrategy(int[] motorPriorities, int highPriorityIntervalMs = 10, int lowPriorityIntervalMs = 100)
    {
        _motorPriorities = motorPriorities;
        MotorCount = motorPriorities.Length;
        _highPriorityIntervalMs = highPriorityIntervalMs;
        _lowPriorityIntervalMs = lowPriorityIntervalMs;
        _lastUpdateTimes = new Dictionary<int, DateTime>();
    }
    
    public void ConfigurePdoMappings(byte nodeId, Action<PdoConfiguration> configureRpdo, Action<PdoConfiguration> configureTpdo)
    {
        // High priority motors get dedicated PDOs
        // Low priority motors share multiplexed PDOs
        
        var highPriorityMotors = _motorPriorities
            .Select((priority, index) => new { Priority = priority, Index = index })
            .Where(x => x.Priority >= 8) // High priority threshold
            .Take(3) // Max 3 dedicated PDOs
            .ToList();
        
        // Configure dedicated PDOs for high priority motors
        for (int i = 0; i < highPriorityMotors.Count; i++)
        {
            var rpdo = new PdoConfiguration((byte)(i + 1), (uint)(0x200 + i * 0x100 + nodeId));
            configureRpdo(rpdo);
            
            var tpdo = new PdoConfiguration((byte)(i + 1), (uint)(0x180 + i * 0x100 + nodeId));
            configureTpdo(tpdo);
        }
        
        // Remaining PDO for low priority multiplexing
        if (highPriorityMotors.Count < 4)
        {
            var rpdo4 = new PdoConfiguration(4, (uint)(0x500 + nodeId));
            configureRpdo(rpdo4);
            
            var tpdo4 = new PdoConfiguration(4, (uint)(0x480 + nodeId));
            configureTpdo(tpdo4);
        }
    }
    
    public void HandlePdoData(byte pdoNumber, byte[] data, Action<int, byte[]> motorDataHandler)
    {
        if (pdoNumber <= 3)
        {
            // Dedicated PDO mapping
            var highPriorityMotors = _motorPriorities
                .Select((priority, index) => new { Priority = priority, Index = index })
                .Where(x => x.Priority >= 8)
                .Take(3)
                .ToList();
            
            if (pdoNumber - 1 < highPriorityMotors.Count)
            {
                var motorIndex = highPriorityMotors[pdoNumber - 1].Index;
                motorDataHandler(motorIndex, data);
            }
        }
        else if (pdoNumber == 4 && data.Length > 0)
        {
            // Multiplexed PDO - first byte is motor ID
            int motorIndex = data[0];
            if (motorIndex < MotorCount)
            {
                var motorData = data.Skip(1).ToArray();
                motorDataHandler(motorIndex, motorData);
            }
        }
    }
    
    public void SendMotorControl(int motorIndex, byte[] controlData, Action<byte, byte[]> sendRpdo)
    {
        var highPriorityMotors = _motorPriorities
            .Select((priority, index) => new { Priority = priority, Index = index })
            .Where(x => x.Priority >= 8)
            .Take(3)
            .ToList();
        
        // Check if motor has dedicated PDO
        var dedicatedPdoIndex = highPriorityMotors.FindIndex(x => x.Index == motorIndex);
        
        if (dedicatedPdoIndex >= 0)
        {
            // Send via dedicated PDO
            sendRpdo((byte)(dedicatedPdoIndex + 1), controlData);
        }
        else
        {
            // Send via multiplexed PDO4
            var multiplexedData = new byte[controlData.Length + 1];
            multiplexedData[0] = (byte)motorIndex;
            controlData.CopyTo(multiplexedData, 1);
            
            sendRpdo(4, multiplexedData);
        }
    }
    
    public void Dispose()
    {
        // PriorityBasedStrategy doesn't use any disposable resources
        GC.SuppressFinalize(this);
    }
}

/// <summary>
/// Adaptive strategy - tự động điều chỉnh dựa trên activity
/// </summary>
public class AdaptiveStrategy : IPdoMultiplexingStrategy
{
    private readonly Dictionary<int, MotorActivity> _motorActivities;
    private readonly Timer _adaptiveTimer;
    
    public int MotorCount { get; }
    
    private class MotorActivity
    {
        public int UpdateCount { get; set; }
        public DateTime LastUpdate { get; set; }
        public bool IsActive => DateTime.UtcNow - LastUpdate < TimeSpan.FromSeconds(5);
        public double ActivityScore => IsActive ? UpdateCount / Math.Max(1, (DateTime.UtcNow - LastUpdate).TotalSeconds) : 0;
    }
    
    public AdaptiveStrategy(int motorCount)
    {
        MotorCount = motorCount;
        _motorActivities = Enumerable.Range(0, motorCount)
            .ToDictionary(i => i, i => new MotorActivity { LastUpdate = DateTime.UtcNow });
        
        _adaptiveTimer = new Timer(AdaptConfiguration, null, TimeSpan.FromSeconds(10), TimeSpan.FromSeconds(10));
    }
    
    private void AdaptConfiguration(object? state)
    {
        // Periodically reconfigure based on motor activity
        var activeMotors = _motorActivities
            .Where(kv => kv.Value.IsActive)
            .OrderByDescending(kv => kv.Value.ActivityScore)
            .Take(3)
            .Select(kv => kv.Key)
            .ToList();
        
        // Logic to reconfigure PDO mappings would go here
        // For simplicity, just update activity scores
        foreach (var activity in _motorActivities.Values)
        {
            activity.UpdateCount = Math.Max(0, activity.UpdateCount - 1); // Decay
        }
    }
    
    public void ConfigurePdoMappings(byte nodeId, Action<PdoConfiguration> configureRpdo, Action<PdoConfiguration> configureTpdo)
    {
        // Similar to time-multiplexed but can be reconfigured
        for (byte i = 1; i <= 4; i++)
        {
            var rpdo = new PdoConfiguration(i, (uint)(0x200 + (i-1) * 0x100 + nodeId));
            configureRpdo(rpdo);
            
            var tpdo = new PdoConfiguration(i, (uint)(0x180 + (i-1) * 0x100 + nodeId));
            configureTpdo(tpdo);
        }
    }
    
    public void HandlePdoData(byte pdoNumber, byte[] data, Action<int, byte[]> motorDataHandler)
    {
        // Determine motor index based on current active configuration
        int motorIndex = (pdoNumber - 1) % MotorCount;
        
        // Update activity tracking
        if (_motorActivities.ContainsKey(motorIndex))
        {
            var activity = _motorActivities[motorIndex];
            activity.UpdateCount++;
            activity.LastUpdate = DateTime.UtcNow;
        }
        
        motorDataHandler(motorIndex, data);
    }
    
    public void SendMotorControl(int motorIndex, byte[] controlData, Action<byte, byte[]> sendRpdo)
    {
        // Update activity
        if (_motorActivities.ContainsKey(motorIndex))
        {
            var activity = _motorActivities[motorIndex];
            activity.UpdateCount++;
            activity.LastUpdate = DateTime.UtcNow;
        }
        
        // Use cyclic assignment for now
        byte pdoNumber = (byte)((motorIndex % 4) + 1);
        sendRpdo(pdoNumber, controlData);
    }
    
    public void Dispose()
    {
        _adaptiveTimer?.Dispose();
        GC.SuppressFinalize(this);
    }
}