using RobotNet10.CANOpen.Enums;

namespace RobotNet10.CANOpen.Models;

/// <summary>
/// PDO Mapping entry (đại diện cho 1 object trong PDO)
/// </summary>
public readonly struct PdoMapping(ushort index, byte subIndex, byte bitLength)
{
    public ushort Index { get; init; } = index;
    public byte SubIndex { get; init; } = subIndex;
    public byte BitLength { get; init; } = bitLength;

    /// <summary>
    /// Convert sang mapping value format (theo DS301)
    /// Format: [Index:16][SubIndex:8][BitLength:8]
    /// </summary>
    public uint ToMappingValue()
    {
        return ((uint)Index << 16) | ((uint)SubIndex << 8) | BitLength;
    }
    
    /// <summary>
    /// Parse từ mapping value
    /// </summary>
    public static PdoMapping FromMappingValue(uint mappingValue)
    {
        ushort index = (ushort)((mappingValue >> 16) & 0xFFFF);
        byte subIndex = (byte)((mappingValue >> 8) & 0xFF);
        byte bitLength = (byte)(mappingValue & 0xFF);
        
        return new PdoMapping(index, subIndex, bitLength);
    }
}

/// <summary>
/// PDO Configuration (TPDO hoặc RPDO)
/// </summary>
public class PdoConfiguration
{
    private readonly List<PdoMapping> _mappings = [];
    
    public PdoConfiguration() 
    {
    }
    
    public PdoConfiguration(byte pdoNumber, uint cobId, PdoTransmissionType transmissionType = PdoTransmissionType.Asynchronous)
    {
        if (pdoNumber < 1 || pdoNumber > 4)
            throw new ArgumentException("PDO number must be between 1 and 4", nameof(pdoNumber));
        
        PdoNumber = pdoNumber;
        CobId = cobId;
        TransmissionType = transmissionType;
    }
    
    /// <summary>
    /// PDO number (1-4 cho TPDO, 1-4 cho RPDO)
    /// </summary>
    public byte PdoNumber { get; set; }
    
    /// <summary>
    /// COB-ID của PDO
    /// </summary>
    public uint CobId { get; set; }
    
    /// <summary>
    /// Transmission type
    /// </summary>
    public PdoTransmissionType TransmissionType { get; set; }
    
    /// <summary>
    /// Inhibit time (x100 microseconds)
    /// </summary>
    public ushort InhibitTime { get; set; }
    
    /// <summary>
    /// Event timer (milliseconds)
    /// </summary>
    public ushort EventTimer { get; set; }
    
    /// <summary>
    /// Danh sách các mapped objects (read-only)
    /// </summary>
    public IReadOnlyList<PdoMapping> Mappings => _mappings.AsReadOnly();
    
    /// <summary>
    /// Check if PDO is valid (bit 31 của COB-ID = 0)
    /// </summary>
    public bool IsValid => (CobId & 0x80000000) == 0;
    
    /// <summary>
    /// Check if RTR is allowed (bit 30 của COB-ID = 0)
    /// </summary>
    public bool RtrAllowed => (CobId & 0x40000000) == 0;
    
    /// <summary>
    /// Total bits mapped in this PDO
    /// </summary>
    public int TotalMappedBits => _mappings.Sum(m => m.BitLength);
    
    /// <summary>
    /// Check if mapping is valid (không vượt quá 64 bits)
    /// </summary>
    public bool IsMappingValid => TotalMappedBits <= 64;
    
    /// <summary>
    /// Add mapping với validation
    /// </summary>
    public void AddMapping(PdoMapping mapping)
    {
        if (mapping.BitLength == 0 || mapping.BitLength > 64)
            throw new ArgumentException("BitLength must be between 1 and 64", nameof(mapping));
        
        if (TotalMappedBits + mapping.BitLength > 64)
            throw new InvalidOperationException($"Adding this mapping would exceed 64 bits limit. Current: {TotalMappedBits}, Adding: {mapping.BitLength}");
        
        _mappings.Add(mapping);
    }
    
    /// <summary>
    /// Remove mapping
    /// </summary>
    public bool RemoveMapping(PdoMapping mapping)
    {
        return _mappings.Remove(mapping);
    }
    
    /// <summary>
    /// Clear all mappings
    /// </summary>
    public void ClearMappings()
    {
        _mappings.Clear();
    }
    
    /// <summary>
    /// Validate entire configuration
    /// </summary>
    public ValidationResult ValidateConfiguration()
    {
        var errors = new List<string>();
        
        if (PdoNumber < 1 || PdoNumber > 4)
            errors.Add("PDO number must be between 1 and 4");
        
        if (!IsValid)
            errors.Add("PDO is disabled (bit 31 of COB-ID is set)");
        
        if (!IsMappingValid)
            errors.Add($"Total mapped bits ({TotalMappedBits}) exceeds 64 bits limit");
        
        if (_mappings.Count == 0)
            errors.Add("No mappings configured");
        
        // Validate individual mappings
        for (int i = 0; i < _mappings.Count; i++)
        {
            var mapping = _mappings[i];
            if (mapping.Index == 0)
                errors.Add($"Mapping {i}: Invalid index (0)");
            
            if (mapping.BitLength == 0)
                errors.Add($"Mapping {i}: Invalid bit length (0)");
        }
        
        return new ValidationResult(errors.Count == 0, errors);
    }
}

/// <summary>
/// PDO Data (received or to-send)
/// </summary>
public readonly struct PdoData(byte pdoNumber, uint cobId, byte[] data, DateTime timestamp)
{
    public byte PdoNumber { get; init; } = pdoNumber;
    public uint CobId { get; init; } = cobId;
    public byte[] Data { get; init; } = data;
    public DateTime Timestamp { get; init; } = timestamp;

    /// <summary>
    /// Extract một value từ PDO data theo mapping
    /// </summary>
    public T ExtractValue<T>(PdoMapping mapping) where T : struct
    {
        if (mapping.BitLength == 0 || mapping.BitLength > 64)
            throw new ArgumentException("BitLength must be 1-64");
        
        int bitOffset = 0;
        foreach (var _ in Enumerable.Range(0, mapping.SubIndex))
        {
            // Tính bit offset dựa trên các mappings trước đó
            // (simplified - trong thực tế cần biết tất cả mappings)
        }
        
        return ExtractValue<T>(bitOffset, mapping.BitLength);
    }
    
    /// <summary>
    /// Extract value tại bit offset cụ thể
    /// </summary>
    public T ExtractValue<T>(int bitOffset, int bitLength) where T : struct
    {
        ulong value = 0;
        int byteOffset = bitOffset / 8;
        int bitInByteOffset = bitOffset % 8;
        int bitsRead = 0;
        
        while (bitsRead < bitLength && byteOffset < Data.Length)
        {
            int bitsToRead = Math.Min(8 - bitInByteOffset, bitLength - bitsRead);
            byte mask = (byte)((1 << bitsToRead) - 1);
            byte byteValue = (byte)((Data[byteOffset] >> bitInByteOffset) & mask);
            
            value |= (ulong)byteValue << bitsRead;
            
            bitsRead += bitsToRead;
            byteOffset++;
            bitInByteOffset = 0;
        }
        
        // Convert to target type with sign extension if needed
        return ConvertValue<T>(value, bitLength);
    }
    
    private static T ConvertValue<T>(ulong value, int bitLength) where T : struct
    {
        var type = typeof(T);
        
        if (type == typeof(bool))
            return (T)(object)(value != 0);
        
        if (type == typeof(byte))
            return (T)(object)(byte)value;
        
        if (type == typeof(sbyte))
        {
            // Sign extension
            if (bitLength < 8 && (value & (1UL << (bitLength - 1))) != 0)
                value |= (ulong)((1L << bitLength) - 1) << bitLength;
            return (T)(object)(sbyte)value;
        }
        
        if (type == typeof(ushort))
            return (T)(object)(ushort)value;
        
        if (type == typeof(short))
        {
            if (bitLength < 16 && (value & (1UL << (bitLength - 1))) != 0)
                value |= (ulong)((1L << bitLength) - 1) << bitLength;
            return (T)(object)(short)value;
        }
        
        if (type == typeof(uint))
            return (T)(object)(uint)value;
        
        if (type == typeof(int))
        {
            if (bitLength < 32 && (value & (1UL << (bitLength - 1))) != 0)
                value |= (ulong)((1L << bitLength) - 1) << bitLength;
            return (T)(object)(int)value;
        }
        
        if (type == typeof(ulong))
            return (T)(object)value;
        
        if (type == typeof(long))
        {
            if (bitLength < 64 && (value & (1UL << (bitLength - 1))) != 0)
                value |= ulong.MaxValue << bitLength;
            return (T)(object)(long)value;
        }
        
        throw new NotSupportedException($"Type {type.Name} is not supported");
    }
}