using RobotNet10.CANOpen;
using RobotNet10.CANOpen.Interfaces;
using System.Collections.Concurrent;

namespace RobotNet10.RobotApp.Drivers.Battery.Varta;
/// <summary>
/// Charger Simulator cho VARTA EasyBlade 59V
/// Máy tính đóng vai Charger, giao tiếp với pin thật qua USB-CAN adapter.
///
/// ── Thông số cố định (theo Technical Spec V1.8) ──────────────────────
///   Baud rate        : 250 kbit/s
///   Charger Node ID  : 100 (0x64)
///   Max voltage      : 58.8 V  (4 × 12V × 1.225 cells)
///   Max current      : 25 A
///   Heartbeat        : mỗi 1000 ms  →  COB-ID 0x764
///   RPDO1            : mỗi 200 ms   →  COB-ID 0x1E4
/// ─────────────────────────────────────────────────────────────────────
/// </summary>
public class Charger59V
{
    // ════════════════════════════════════════════════════════════════
    //  ⚙️  THÔNG SỐ HARDCODE – CHỈNH TẠI ĐÂY NẾU CẦN
    // ════════════════════════════════════════════════════════════════

    // Điện áp tối đa charger có thể cung cấp (V)
    // EasyBlade 59V: pin lithium 13S → max 54.6V, để an toàn dùng 58.8V
    private const double MAX_VOLTAGE_V   = 58.8;

    // Dòng tối đa charger có thể cung cấp (A)
    private const double MAX_CURRENT_A   = 25.0;

    // Điện áp thực đo được (báo lại pin trong RPDO1, byte 2-3)
    // Lúc chưa sạc thực thì đặt bằng Max hoặc giá trị đo thực của nguồn
    private const double ACTUAL_VOLTAGE_V = 54.0;

    // Dòng thực đo được (báo lại pin trong RPDO1, byte 0-1)
    private const double ACTUAL_CURRENT_A = 10.0;

    // COB-ID (không đổi theo spec)
    private const uint COB_HEARTBEAT = 0x764;   // gửi
    private const uint COB_RPDO1     = 0x1E4;   // gửi
    private const uint COB_SDO_TX    = 0x5E4;   // gửi (response về battery)
    private const uint COB_SDO_RX    = 0x664;   // nhận (request từ battery)
    private const uint COB_TPDO9     = 0x264;   // nhận (SoC, VReq, IReq)
    private const uint COB_TPDO8     = 0x49B;   // nhận (charge control status)

    // ── Giá trị raw (Q8 = ×256, Q4 = ×16) ───────────────────────────
    private static readonly ushort RAW_MAX_VOLTAGE = (ushort)(MAX_VOLTAGE_V * 256);
    private static readonly ushort RAW_MAX_CURRENT = (ushort)(MAX_CURRENT_A * 16);
    private static readonly ushort RAW_ACT_VOLTAGE = (ushort)(ACTUAL_VOLTAGE_V * 256);
    private static readonly ushort RAW_ACT_CURRENT = (ushort)(ACTUAL_CURRENT_A * 256);

    // ════════════════════════════════════════════════════════════════
    //  State
    // ════════════════════════════════════════════════════════════════
    private bool   _sdoInitDone      = false;
    private bool   _chargeActive     = false;  // true sau khi set Bit12
    private bool   _relayOpen        = true;   // true = relay mở, không có điện ra
    private bool   _batteryCharging  = false;  // true khi pin báo đang vào trạng thái sạc

    // Lưu lại giá trị SDO battery ghi vào charger
    private byte   _batteryStatus    = 0;   // Object 0x6000
    private byte   _chargeControl    = 0;   // Object 0x4200
    private ushort _voltageReqRaw    = 0;   // Object 0x2276
    private ushort _currentReqRaw    = 0;   // Object 0x6070

    private readonly ICanOpenManager _canOpenManager;
    private readonly string _canInterface;
    private ICanBus? _can;
    private readonly CancellationToken _ct;
    private readonly ConcurrentQueue<CanFrameReceivedEventArgs> _rxQueue = new();
    private readonly SemaphoreSlim _rxSignal = new(0);

    public Charger59V(ICanOpenManager canOpenManager, string canInterface, CancellationToken ct)
    {
        _canOpenManager = canOpenManager ?? throw new ArgumentNullException(nameof(canOpenManager));
        _canInterface = string.IsNullOrWhiteSpace(canInterface) ? "can0" : canInterface;
        _ct = ct;
    }

    // ════════════════════════════════════════════════════════════════
    public async Task RunAsync()
    {
        _can = await _canOpenManager.GetOrCreateCanBusAsync(_canInterface, _ct);
        _can.FrameReceived += OnFrameReceived;

        // Log($"Max Voltage : {MAX_VOLTAGE_V} V  (raw Q8 = {RAW_MAX_VOLTAGE})");
        // Log($"Max Current : {MAX_CURRENT_A} A  (raw Q4 = {RAW_MAX_CURRENT})");
        // Log($"Gửi Heartbeat 0x{COB_HEARTBEAT:X3} mỗi 1000ms...");
        // Log("Đang chờ pin kết nối...\n");

        try
        {
            // Chạy song song 3 vòng lặp
            await Task.WhenAll(
                HeartbeatLoopAsync(),   // gửi HB mỗi 1000ms
                ReceiveLoopAsync(),     // nhận SDO + TPDO từ pin
                Rpdo1LoopAsync()        // gửi RPDO1 sau khi SDO init xong
            );
        }
        finally
        {
            _can.FrameReceived -= OnFrameReceived;
            while (_rxQueue.TryDequeue(out _)) { }
            while (_rxSignal.Wait(0)) { }
        }
    }

    // ════════════════════════════════════════════════════════════════
    //  VÒNG LẶP 1 – Heartbeat (mỗi 1000ms)
    // ════════════════════════════════════════════════════════════════
    private async Task HeartbeatLoopAsync()
    {
        while (!_ct.IsCancellationRequested)
        {
            // NMT Heartbeat: 1 byte [0x05] = Operational state
            SendFrame(COB_HEARTBEAT, [0x05]);
            // Dim($"♥ HB → 0x{COB_HEARTBEAT:X3}");
            await Task.Delay(1000, _ct);
        }
    }

    // ════════════════════════════════════════════════════════════════
    //  VÒNG LẶP 2 – Nhận frame từ pin
    // ════════════════════════════════════════════════════════════════
    private async Task ReceiveLoopAsync()
    {
        while (!_ct.IsCancellationRequested)
        {
            if (TryReceive(out var frame))
            {
                switch (frame.CanId)
                {
                    case COB_SDO_RX:  HandleSdo(frame.Data);   break;
                    case COB_TPDO9:   HandleTpdo9(frame.Data); break;
                    case COB_TPDO8:   HandleTpdo8(frame.Data); break;
                }
            }
            else
            {
                await Task.Delay(1, _ct);   // yield CPU khi không có frame
            }
        }
    }

    // ════════════════════════════════════════════════════════════════
    //  VÒNG LẶP 3 – Gửi RPDO1 (mỗi 200ms, sau khi SDO init xong)
    // ════════════════════════════════════════════════════════════════
    private async Task Rpdo1LoopAsync()
    {
        // Chờ SDO init hoàn tất
        while (!_sdoInitDone && !_ct.IsCancellationRequested)
            await Task.Delay(50, _ct);

        if (_ct.IsCancellationRequested) return;

        // Delay 1s trước khi kích hoạt Bit12 (cho pin ổn định)
        LogOk("SDO init xong! Chờ 1s rồi bật Bit12...");
        await Task.Delay(1000, _ct);

        // Bật relay và charge mode
        _relayOpen    = false;
        _chargeActive = true;
        LogOk("==> Bit12 SET – Pin đang chuyển sang CHARGE MODE!");

        // Gửi RPDO1 mỗi 200ms
        while (!_ct.IsCancellationRequested)
        {
            SendRpdo1();
            await Task.Delay(200, _ct);
        }
    }

    // ════════════════════════════════════════════════════════════════
    //  GỬI RPDO1  (COB-ID 0x1E4)
    //
    //  Byte 0-1: Charging Current   [1/256 A, Q8]
    //  Byte 2-3: Charging Voltage   [1/256 V, Q8]
    //  Byte 4-5: Max avail Current  [1/16 A,  Q4]
    //  Byte 6-7: Extended Charger Status
    //            → Bit12 (0x1000) = kích hoạt charge mode
    // ════════════════════════════════════════════════════════════════
    private void SendRpdo1()
    {
        ushort extStatus = (_chargeActive && !_relayOpen)
                           ? (ushort)0x1000   // Bit12 set
                           : (ushort)0x0000;

        byte[] data =
        [
            (byte)(RAW_ACT_CURRENT & 0xFF), (byte)(RAW_ACT_CURRENT >> 8), // Byte 0-1
            (byte)(RAW_ACT_VOLTAGE & 0xFF), (byte)(RAW_ACT_VOLTAGE >> 8), // Byte 2-3
            (byte)(RAW_MAX_CURRENT & 0xFF), (byte)(RAW_MAX_CURRENT >> 8), // Byte 4-5
            (byte)(extStatus       & 0xFF), (byte)(extStatus       >> 8), // Byte 6-7
        ];

        SendFrame(COB_RPDO1, data);
        // Dim($"→ RPDO1 0x{COB_RPDO1:X3} [{string.Join(" ", data.Select(b => $"{b:X2}"))}] " +
            // $"ExtStat=0x{extStatus:X4}");
    }

    // ════════════════════════════════════════════════════════════════
    //  XỬ LÝ SDO REQUEST TỪ PIN  (COB-ID 0x664)
    // ════════════════════════════════════════════════════════════════
    private void HandleSdo(byte[] d)
    {
        if (d.Length < 8) return;
        byte   cmd   = d[0];
        ushort index = (ushort)(d[1] | (d[2] << 8));
        byte   sub   = d[3];

        switch (cmd)
        {
            // Pin GHI vào object của charger
            case 0x2F: // Write 1 byte
                OnWrite(index, sub, d[4], 0);
                break;
            case 0x2B: // Write 2 bytes
                OnWrite(index, sub, d[4], (ushort)(d[4] | (d[5] << 8)));
                break;
            case 0x23: // Write 4 bytes
                SdoWriteOk(index, sub); // phản hồi OK, bỏ qua giá trị
                break;

            // Pin ĐỌC object từ charger
            case 0x40: // Read request
                OnRead(index, sub);
                break;
        }
    }

    private void OnWrite(ushort index, byte sub, byte val8, ushort val16)
    {
        switch (index)
        {
            case 0x6000: // Battery Status
                _batteryStatus = val8;
                // Log($"  [SDO] 0x6000 Battery Status ← {val8}  " +
                //     (val8 == 1 ? "→ Relay CLOSED (power ON)" : "→ Relay OPEN (power OFF)"));
                _relayOpen = (val8 == 0);
                break;

            case 0x4200: // Charge Control
                _chargeControl = val8;
                // Log($"  [SDO] 0x4200 Charge Control ← {val8}  " +
                //     (val8 == 1 ? "→ Battery READY" : "→ Battery NOT ready"));
                // ChargeControl=0 → pin báo full/lỗi → tắt relay
                if (val8 == 0 && _sdoInitDone)
                {
                    LogWarn("ChargeControl=0 → TẮT RELAY (pin đầy hoặc lỗi)");
                    _relayOpen    = true;
                    _chargeActive = false;
                }
                break;

            case 0x2276: // Voltage Request
                _voltageReqRaw = val16;
                // Log($"  [SDO] 0x2276 Voltage Request ← {val16 / 256.0:F3} V");
                break;

            case 0x6070: // Current Request
                _currentReqRaw = val16;
                // Log($"  [SDO] 0x6070 Current Request ← {val16 / 16.0:F3} A");
                break;

            default:
                // Log($"  [SDO] Write idx=0x{index:X4}.{sub} val=0x{val16:X4}");
                break;
        }
        SdoWriteOk(index, sub);
        CheckInitComplete();
    }

    private void OnRead(ushort index, byte sub)
    {
        switch (index)
        {
            case 0x4208: // Max Charging Voltage
                SdoReadOk2(index, sub, RAW_MAX_VOLTAGE);
                // Log($"  [SDO] 0x4208 Max Voltage → {MAX_VOLTAGE_V} V  (raw=0x{RAW_MAX_VOLTAGE:X4})");
                break;

            case 0x4212: // Max Charging Current
                SdoReadOk2(index, sub, RAW_MAX_CURRENT);
                // Log($"  [SDO] 0x4212 Max Current → {MAX_CURRENT_A} A  (raw=0x{RAW_MAX_CURRENT:X4})");
                break;

            default:
                // Abort: object does not exist
                byte[] abort = [0x80,
                    (byte)(index & 0xFF), (byte)(index >> 8), sub,
                    0x00, 0x00, 0x02, 0x06];
                SendFrame(COB_SDO_TX, abort);
                break;
        }
        CheckInitComplete();
    }

    // ════════════════════════════════════════════════════════════════
    //  XỬ LÝ TPDO9  (COB-ID 0x264)  – Pin gửi mỗi 100ms
    //  Byte 0: ChargeControl  Byte 1: SoC
    //  Byte 3-4: Volt Request  Byte 5-6: Curr Request  Byte 7: BattStatus
    // ════════════════════════════════════════════════════════════════
    private void HandleTpdo9(byte[] d)
    {
        if (d.Length < 7) return;
        byte   cc   = d[0];
        byte   soc  = d[1];
        ushort vReq = (ushort)(d[3] | (d[4] << 8));
        ushort iReq = (ushort)(d[5] | (d[6] << 8));
        byte   bs   = d.Length > 7 ? d[7] : (byte)0;

        Console.ForegroundColor = ConsoleColor.Green;
        // Console.WriteLine(
        //     $"[{Now}] 📦 PIN   " +
        //     $"SoC={soc,3}%  " +
        //     $"VReq={vReq / 256.0,6:F2}V  " +
        //     $"IReq={iReq / 16.0,6:F2}A  " +
        //     $"ChargeCtrl={cc}  BattStat={bs}");
        // Console.ResetColor();

        // Pin gửi ChargeControl=0 → pin đầy hoặc có lỗi → dừng sạc
        if (cc == 0 && _sdoInitDone && _chargeActive)
        {
            LogWarn("ChargeControl=0 → PIN ĐẦY hoặc LỖI → Tắt relay!");
            _chargeActive = false;
            _relayOpen    = true;
        }
    }

    // ════════════════════════════════════════════════════════════════
    //  XỬ LÝ TPDO8  (COB-ID 0x49B) – Battery Charge Control Status
    // ════════════════════════════════════════════════════════════════
    private void HandleTpdo8(byte[] d)
    {
        if (d.Length < 2) return;
        ushort s = (ushort)(d[0] | (d[1] << 8));
        bool chargingNow = s == 0xC011 || s == 0xC033;

        if (chargingNow && !_batteryCharging)
        {
            _batteryCharging = true;
            LogOk($"✅ PIN ĐÃ VÀO TRẠNG THÁI SẠC (TPDO8=0x{s:X4})");
        }
        else if (!chargingNow && _batteryCharging)
        {
            _batteryCharging = false;
            LogWarn($"PIN THOÁT TRẠNG THÁI SẠC (TPDO8=0x{s:X4})");
        }

        string desc = s switch
        {
            0x0033 => "SDO init OK – chờ Bit12",
            0x4033 => "Standby – chờ Bit12",
            0xC011 => "⚡ CHARGING ACTIVE",
            0xC033 => "⚡ Charging (normal)",
            0xC000 => "Pin đầy – về standby",
            0xD000 => "Keep-power hết – SHUTDOWN",
            _      => $"bits={s:X4}"
        };
        Console.ForegroundColor = ConsoleColor.Cyan;
        // Console.WriteLine($"[{Now}] 📊 STATUS 0x{s:X4} → {desc}");
        Console.ResetColor();
    }

    // ════════════════════════════════════════════════════════════════
    //  Kiểm tra SDO init sequence đã đủ 4 bước chưa
    // ════════════════════════════════════════════════════════════════
    private void CheckInitComplete()
    {
        if (_sdoInitDone) return;
        if (_batteryStatus == 1
         && _chargeControl == 1
         && _voltageReqRaw >  0
         && _currentReqRaw >  0)
        {
            _sdoInitDone = true;
            // Console.ForegroundColor = ConsoleColor.Yellow;
            // Console.WriteLine($"\n[{Now}] ══════════════════════════════════════");
            // Console.WriteLine($"[{Now}]  ✅ SDO INITIALIZATION HOÀN TẤT!");
            // Console.WriteLine($"[{Now}]  BatteryStatus={_batteryStatus}  ChargeControl={_chargeControl}");
            // Console.WriteLine($"[{Now}]  VoltReq={_voltageReqRaw / 256.0:F3}V  CurrReq={_currentReqRaw / 16.0:F3}A");
            // Console.WriteLine($"[{Now}] ══════════════════════════════════════\n");
            // Console.ResetColor();
        }
    }

    // ════════════════════════════════════════════════════════════════
    //  SDO helpers
    // ════════════════════════════════════════════════════════════════
    private void SdoWriteOk(ushort index, byte sub)
    {
        byte[] d = [0x60, (byte)(index & 0xFF), (byte)(index >> 8), sub, 0, 0, 0, 0];
        SendFrame(COB_SDO_TX, d);
        // Dim($"← SDO OK   0x{COB_SDO_TX:X3} idx=0x{index:X4}");
    }

    private void SdoReadOk2(ushort index, byte sub, ushort value)
    {
        byte[] d = [0x4B,
                    (byte)(index & 0xFF), (byte)(index >> 8), sub,
                    (byte)(value & 0xFF), (byte)(value >> 8), 0, 0];
        SendFrame(COB_SDO_TX, d);
        // Dim($"← SDO RSP  0x{COB_SDO_TX:X3} idx=0x{index:X4} val=0x{value:X4}");
    }

    private void SendFrame(uint canId, byte[] data)
    {
        var bus = _can;
        if (bus is null || !bus.IsConnected)
        {
            return;
        }

        bus.SendFrameAsync(canId, data, _ct).GetAwaiter().GetResult();
    }

    private bool TryReceive(out CanFrameReceivedEventArgs frame)
    {
        if (_rxQueue.TryDequeue(out frame!))
        {
            return true;
        }

        try
        {
            if (!_rxSignal.Wait(10, _ct))
            {
                frame = null!;
                return false;
            }
        }
        catch (OperationCanceledException)
        {
            frame = null!;
            return false;
        }

        if (_rxQueue.TryDequeue(out frame!))
        {
            return true;
        }

        frame = null!;
        return false;
    }

    private void OnFrameReceived(object? sender, CanFrameReceivedEventArgs e)
    {
        _rxQueue.Enqueue(e);
        try
        {
            _rxSignal.Release();
        }
        catch (SemaphoreFullException)
        {
        }
    }

    // ════════════════════════════════════════════════════════════════
    //  Logging
    // ════════════════════════════════════════════════════════════════
    private static string Now => DateTime.Now.ToString("HH:mm:ss.fff");

    private static void Log(string msg)
        => Console.WriteLine($"[{Now}] {msg}");

    private static void LogOk(string msg)
    {
        Console.ForegroundColor = ConsoleColor.Yellow;
        Console.WriteLine($"[{Now}] {msg}");
        Console.ResetColor();
    }

    private static void LogWarn(string msg)
    {
        Console.ForegroundColor = ConsoleColor.Red;
        Console.WriteLine($"[{Now}] ⚠️  {msg}");
        Console.ResetColor();
    }

    private static void Dim(string msg)
    {
        Console.ForegroundColor = ConsoleColor.DarkGray;
        Console.WriteLine($"[{Now}] {msg}");
        Console.ResetColor();
    }
}