using Microsoft.Extensions.Logging;
using RobotNet10.CANOpen;
using RobotNet10.CANOpen.Interfaces;
using System.Collections.Concurrent;
using System.Runtime.InteropServices;

// using 
namespace RobotNet10.RobotApp.Drivers.Battery.Varta;

/// <summary>
/// Varta CAN client — đọc dữ liệu pin qua CANopen PDO.
/// Dùng SocketCAN transport có sẵn trong RobotNet10.CANOpen.
/// Protocol (CAN 11-bit):
///   0x19B -> Voltage, Current
///   0x281 -> FetTemp, CellTemp, ChargeReqVoltage, ChargeReqCurrent
///   0x381 -> NominalCapacity, FullCapacity, RemainingCapacity, SOC
///   0x481/0x581 -> Info, Warn, Error, ChargeCtrl
/// </summary>
public sealed class VartaCanClient : IDisposable
{
    private readonly ILogger _logger;
    private readonly ICanOpenManager _canOpenManager;
    private readonly string _canInterface;
    private readonly int _readTimeoutMs;
    private readonly Lock _stateLock = new();
    private readonly ConcurrentQueue<CanFrameReceivedEventArgs> _rxQueue = new();
    private readonly SemaphoreSlim _rxSignal = new(0);
    private ICanBus? _bus;
    private bool _disposed;
    private bool _isFaulted;

    public bool IsFaulted
    {
        get
        {
            lock (_stateLock)
            {
                return _isFaulted;
            }
        }
    }

    public VartaCanClient(ILogger logger, ICanOpenManager canOpenManager, string canInterface, int readTimeoutMs = 200)
    {
        _logger = logger;
        _canOpenManager = canOpenManager;
        _canInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
        _readTimeoutMs = Math.Max(10, readTimeoutMs);

        OpenBus();
    }


    private void OpenBus()
    {
        lock (_stateLock)
        {
            _isFaulted = false;
        }

        try
        {
            // Xóa bus cũ khỏi cache của CanOpenManager trước khi tạo lại,
            // tránh GetOrCreateCanBusAsync trả về bus đã chết do caching.
            _logger.LogInformation("[VartaCanClient] Removing old CAN bus from cache for {Iface}", _canInterface);
            _canOpenManager.RemoveCanBusAsync(_canInterface).GetAwaiter().GetResult();

            var bus = _canOpenManager.GetOrCreateCanBusAsync(_canInterface).GetAwaiter().GetResult();
            _logger.LogInformation("[VartaCanClient] CAN bus recreated for {Iface}, IsConnected={IsConnected}", _canInterface, bus.IsConnected);

            lock (_stateLock)
            {
                if (_bus != null)
                {
                    _bus.FrameReceived -= OnFrameReceived;
                }

                _bus = bus;
                _bus.FrameReceived += OnFrameReceived;
            }
        }
        catch (Exception ex)
        {
            lock (_stateLock)
            {
                _isFaulted = true;
            }
            // _logger.LogError(ex, "[VartaCanClient] Không thể mở SocketCAN trên interface {Iface}", _canInterface);
        }
    }

    public void ForceReconnect()
    {
        if (_disposed)
        {
            return;
        }

        _logger.LogInformation("[VartaCanClient] ForceReconnect start {Iface}", _canInterface);

        lock (_stateLock)
        {
            try
            {
                if (_bus != null)
                {
                    _bus.FrameReceived -= OnFrameReceived;
                }
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "[VartaCanClient] No data in {Iface}", _canInterface);
            }

            while (_rxQueue.TryDequeue(out _)) { }
            while (_rxSignal.Wait(0)) { }
        }

        OpenBus();
        _logger.LogInformation("[VartaCanClient] ForceReconnect Finished, IsFaulted={IsFaulted}", _isFaulted);
    }

    /// <summary>
    /// Đọc frame mới nhất từ queue receive và decode theo protocol Varta.
    /// Drain toàn bộ queue, chỉ giữ frame mới nhất cho mỗi CAN ID để tránh trễ dữ liệu.
    /// </summary>
    public Dictionary<string, double>? ReadResponse(int maxFrames = 30)
    {
        if (_disposed || IsFaulted || _bus == null || !_bus.IsConnected)
        {
            return null;
        }

        // Nếu queue rỗng, chờ frame mới đến
        if (_rxQueue.IsEmpty)
        {
            try
            {
                if (!_rxSignal.Wait(_readTimeoutMs))
                {
                    return null;
                    ForceReconnect();
                }
            }
            catch
            {
                return null;
            }
        }

        // Drain toàn bộ queue, chỉ giữ frame mới nhất cho mỗi CAN ID
        var latestFrames = new Dictionary<uint, CanFrameReceivedEventArgs>();
        while (_rxQueue.TryDequeue(out var frame))
        {
            latestFrames[frame.CanId] = frame;
            // Drain semaphore để khớp với số frame bị loại bỏ
            _rxSignal.Wait(0);
        }

        if (latestFrames.Count == 0)
        {
            return null;
        }

        var result = new Dictionary<string, double>(StringComparer.OrdinalIgnoreCase);
        foreach (var frame in latestFrames.Values)
        {
            DecodeFrame(frame, result);
        }

        return result.Count == 0 ? null : result;
    }

    private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
    {
        if (_disposed)
        {
            return;
        }

        _rxQueue.Enqueue(e);
        try
        {
            _rxSignal.Release();
        }
        catch (SemaphoreFullException)
        {
        }
    }

    private static void DecodeFrame(CanFrameReceivedEventArgs frame, Dictionary<string, double> result)
    {
        // uint canId = frame.CanId & 0x7FFu;
        var d = frame.Data;

        if (d == null)
        {
            return;
        }

        // var canBase = canId & 0x780u;

        switch (frame.CanId)
        {
            case 0x181: // TPDO1: 0x180 + NodeId
            {
                uint voltage = BitConverter.ToUInt32(d, 0);
                int current = BitConverter.ToInt32(d, 4);
                double volts = voltage / 1000.0;
                double amps = current / 1000.0;
                result["Voltage"] = Math.Round(volts, 1, MidpointRounding.ToZero);
                result["Current"] = Math.Round(amps, 1, MidpointRounding.ToZero);
                // Console.WriteLine($"[VartaCanClient] Received TPDO1: Voltage={volts} V, Current={amps} A, Timestamps: {DateTime.Now:HH:mm:ss.fff} s");
                break;
            }

            case 0x281: // TPDO2: 0x280 + NodeId
                result["FetTemp"] = BitConverter.ToInt16(d, 0) / 10.0;
                result["CellTemp"] = BitConverter.ToInt16(d, 2) / 10.0;
                result["ChargeReqVoltage"] = BitConverter.ToUInt16(d, 4) / 1000.0;
                result["ChargeReqCurrent"] = BitConverter.ToUInt16(d, 6) / 1000.0;
                // Console.WriteLine($"[VartaCanClient] Received TPDO2: FetTemp={result["FetTemp"]} °C, CellTemp={result["CellTemp"]} °C, ChargeReqVoltage={result["ChargeReqVoltage"]} V, ChargeReqCurrent={result["ChargeReqCurrent"]} A");
                break;

            case 0x381: // TPDO3: 0x380 + NodeId
            {
                var nominal = BitConverter.ToUInt16(d, 0);
                var full = BitConverter.ToUInt16(d, 2);
                var remaining = BitConverter.ToUInt16(d, 4);
                result["NominalCapacityMah"] = nominal;
                result["FullCapacityMah"] = full;
                result["RemainingCapacityMah"] = remaining;
                result["SOC"] = full == 0 ? 0 : remaining * 100.0 / full;
                result["SOH"] = nominal == 0 ? 0 : full * 100.0 / nominal;
                // Console.WriteLine($"[VartaCanClient] Received TPDO3: Nominal={nominal} mAh, Full={full} mAh, Remaining={remaining} mAh, SOC={result["SOC"]} %, SOH={result["SOH"]} %");
                break;
            }

            case 0x481: // TPDO4: 0x480 + NodeId

            case 0x581: // SDO response: 0x580 + NodeId (some firmware puts status words here)
                result["Info"] = BitConverter.ToUInt16(d, 0);
                result["Warn"] = BitConverter.ToUInt16(d, 2);
                result["Error"] = BitConverter.ToUInt16(d, 4);
                result["ChargeCtrl"] = BitConverter.ToUInt16(d, 6);
                // Console.WriteLine($"[VartaCanClient] Received TPDO4/SDO: Info={result["Info"]}, Warn={result["Warn"]}, Error={result["Error"]}, ChargeCtrl={result["ChargeCtrl"]}");
                break;

            case 0x264:
                result["ChargeControl"] = d[0];                                    // byte 0: uint8
                result["SOC"] = d[1];                                              // byte 1: uint8, %
                // byte 2: không sử dụng
                result["ChargeVoltageRequest"] = BitConverter.ToUInt16(d, 3) / 256.0;  // bytes 3-4: uint16, 1/256 V
                result["ChargeCurrentRequest"] = BitConverter.ToUInt16(d, 5) / 16.0;  // bytes 5-6: uint16, 1/16 A
                result["BatteryStatus"] = d[7];                                    // byte 7: uint8
                // Console.WriteLine($"[VartaCanClient] Received 0x264: ChargeControl={result["ChargeControl"]}, SOC={result["SOC"]} %, ChargeVoltageRequest={result["ChargeVoltageRequest"]:F4} V, ChargeCurrentRequest={result["ChargeCurrentRequest"]:F4} A, BatteryStatus={result["BatteryStatus"]}");
                break;
        }
    }

    public void Dispose()
    {
        lock (_stateLock)
        {
            if (_disposed)
            {
                return;
            }
            _disposed = true;
        }

        try
        {
            _bus?.FrameReceived -= OnFrameReceived;
        }
        catch
        {
        }
        finally
        {
            _rxSignal.Dispose();
            while (_rxQueue.TryDequeue(out _)) { }
        }
    }
}