using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.RobotApp.Interfaces;
using RobotNet10.Shared;
using RobotNet10.Shared.Sensor;
using System.Diagnostics;

namespace RobotNet10.RobotApp.Drivers.YNZDH;

[Device(DeviceType.RfHandle, "YNZDH", "YNZDH_RfHandle", "1.0.0",
    Description = "YNZDH RF Handle (minimal but full simulation properties)")]
public class YNZDH_RfHandle : DeviceBase, IRfHandle
{
    private readonly string _port;
    private readonly int _baud;
    private readonly ILogger<YNZDH_RfHandle> _logger;

    private ModbusRtuClient? _modbus;
    
    // High-priority polling thread for real-time data acquisition
    private Thread? _pollingThread;
    private CancellationTokenSource? _pollingCts;
    private volatile bool _shouldPoll = false;

    private readonly Lock _lock = new();

    public event Action? Updated;

    public YNZDH_RfHandle(string deviceId, string deviceName, IConfigurationSection cfg, IServiceProvider serviceProvider)
        : base(deviceId, deviceName, DeviceType.RfHandle)
    {
        _port = cfg.GetValue<string>("Port") ?? throw new Exception("Port is required");
        _baud = cfg.GetValue<int?>("BaudRate") ?? throw new Exception("BaudRate is required");

        var loggerFactory = serviceProvider.GetRequiredService<ILoggerFactory>();
        _logger = loggerFactory.CreateLogger<YNZDH_RfHandle>();

        AutoReconnectEnabled = true;
        ReconnectDelayMs = 2000;
        MaxReconnectAttempts = 0;

        // Khởi tạo PropertyDescriptions → DeviceBase validation pass
        UpdateProperties();
    }

    // ====================== STATES ===========================

    public DateTime LastUpdateTime { get; private set; }

    public int Heartbeat { get; private set; }
    public bool RemoteReady { get; private set; }
    public bool EStop { get; private set; }

    public bool LiftUp { get; private set; }
    public bool LiftDown { get; private set; }
    public bool RotateLeft { get; private set; }
    public bool RotateRight { get; private set; }

    public bool ModeSelect { get; private set; }
    public bool Enable { get; private set; }

    public int Speed { get; private set; }          // 0–100
    public double Linear { get; private set; }       // -1 → +1
    public double Angular { get; private set; }      // -1 → +1

    public RFMode Mode { get; private set; } = RFMode.None;

    private Joy? _cachedJoyState;

    // IRfHandle Implementation
    public Joy? CurrentJoyState
    {
        get { lock (_lock) { return _cachedJoyState; } }
    }

    // ====================== SIM PROPERTIES ====================

    protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
    {
        return
        [
            new("Heartbeat", "Heartbeat"),
            new("RemoteReady", "Remote Ready"),
            new("EStop", "Emergency Stop"),

            new("LiftUp", "Lift Up"),
            new("LiftDown", "Lift Down"),
            new("RotateLeft", "Rotate Left"),
            new("RotateRight", "Rotate Right"),

            new("ModeSelect", "Mode Select"),
            new("Enable", "Enable"),

            new("Speed", "Speed"),
            new("Mode", "Mode"),

            new("LastUpdate", "Last Update Time")
        ];
    }

    private void UpdateProperties()
    {
        SetProperty("Heartbeat", Heartbeat.ToString());
        SetProperty("RemoteReady", RemoteReady.ToString());
        SetProperty("EStop", EStop.ToString());

        SetProperty("LiftUp", LiftUp.ToString());
        SetProperty("LiftDown", LiftDown.ToString());
        SetProperty("RotateLeft", RotateLeft.ToString());
        SetProperty("RotateRight", RotateRight.ToString());

        SetProperty("ModeSelect", ModeSelect.ToString());
        SetProperty("Enable", Enable.ToString());

        SetProperty("Speed", Speed.ToString());
        SetProperty("Mode", Mode.ToString());

        SetProperty("LastUpdate", LastUpdateTime == default
            ? "Never"
            : LastUpdateTime.ToString("yyyy-MM-dd HH:mm:ss"));
    }

    // ====================== DEVICEBASE ========================

    protected override Task OnInitializeAsync(CancellationToken cancellationToken)
    {
        try
        {
            //_modbus = new ModbusRtuClient(_port, _baud, logger: _logger);
            _modbus = new ModbusRtuClient(_port, _baud);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Modbus init failed");
            LastError = ex;
            OnErrorOccurred(ex, "Modbus init failed");
        }

        return Task.CompletedTask;
    }

    protected override Task OnConnectAsync(CancellationToken cancellationToken)
    {
        StartPolling();
        return Task.CompletedTask;
    }

    protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
        StopPolling();
        return Task.CompletedTask;
    }

    protected override void Dispose(bool disposing)
    {
        if (disposing)
        {
            // Stop polling thread before disposing base class
            StopPolling();
        }
        base.Dispose(disposing);
    }

    protected override Task OnResetAsync(CancellationToken cancellationToken)
    {
        StopPolling();
        StartPolling();
        return Task.CompletedTask;
    }

    protected override Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
        => Task.FromResult(_modbus != null);

    // ===================== POLLING LOOP ======================

    /// <summary>
    /// Start high-priority polling thread for real-time data acquisition at 10Hz
    /// </summary>
    private void StartPolling()
    {
        StopPolling(); // Đảm bảo không có thread nào đang chạy

        _shouldPoll = true;
        
        // Tạo mới CancellationTokenSource cho thread mới
        _pollingCts?.Dispose();
        _pollingCts = new CancellationTokenSource();
        
        _pollingThread = new Thread(() => PollingThreadLoop(_pollingCts.Token))
        {
            Name = $"YNZDH-RfHandle-Polling-{DeviceId}",
            IsBackground = false, // Không phải background thread để đảm bảo chạy liên tục
            Priority = ThreadPriority.Highest // Priority cao để đảm bảo real-time polling
        };
        
        _pollingThread.Start();
        _logger.LogDebug("YNZDH_RfHandle: Started high-priority polling thread at 10Hz for device {DeviceId}", DeviceId);
    }

    /// <summary>
    /// Stop polling thread gracefully
    /// </summary>
    private void StopPolling()
    {
        _shouldPoll = false;
        
        _pollingCts?.Cancel();
        
        if (_pollingThread != null)
        {
            if (!_pollingThread.Join(1000)) // Đợi tối đa 1 giây
            {
                _logger.LogWarning("YNZDH_RfHandle: Polling thread did not stop gracefully for device {DeviceId}", DeviceId);
            }
            _pollingThread = null;
        }
        
        // Dispose CancellationTokenSource sau khi thread đã dừng
        _pollingCts?.Dispose();
        _pollingCts = null;
    }

    /// <summary>
    /// High-priority polling thread loop - runs at 10Hz (100ms interval)
    /// Uses Stopwatch for high-precision timing to ensure accurate 10Hz polling rate
    /// </summary>
    private void PollingThreadLoop(CancellationToken cancellationToken)
    {
        var modbusClient = _modbus;
        if (modbusClient == null)
        {
            _logger.LogError("YNZDH_RfHandle: Modbus client is null");
            return;
        }

        Thread.BeginThreadAffinity();
        try
        {
            const int pollingIntervalMs = 100; // 10Hz = 100ms
            var intervalTicks = pollingIntervalMs * TimeSpan.TicksPerMillisecond;
            var stopwatch = Stopwatch.StartNew();
            var nextPollTime = stopwatch.ElapsedTicks + intervalTicks;
            var spinWait = new SpinWait();
            long currentTicks = 0;

            while (_shouldPoll && !cancellationToken.IsCancellationRequested)
            {
                currentTicks = stopwatch.ElapsedTicks;

                // Check if it's time to poll
                if (currentTicks >= nextPollTime)
                {
                    try
                    {
                        ushort[] regs = modbusClient.ReadHoldingRegisters(1, 1, 4);
                        DecodeRegisters(regs);
                    }
                    catch (Exception ex)
                    {
                        LastError = ex;
                        OnErrorOccurred(ex, "Polling error");
                        ResetToDefaultValues();
                    }

                    // Calculate next poll time
                    nextPollTime = currentTicks + intervalTicks;
                }

                // SpinWait for precise timing (10 spins then reset)
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.SpinOnce();
                spinWait.Reset();
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "YNZDH_RfHandle: Error in polling thread loop for device {DeviceId}", DeviceId);
            LastError = ex;
            OnErrorOccurred(ex, "Polling thread error");
        }
        finally
        {
            Thread.EndThreadAffinity();
        }
    }

    // ===================== DECODE ============================

    private void ResetToDefaultValues()
    {
        lock (_lock)
        {
            Heartbeat = 0;
            RemoteReady = false;
            EStop = false;

            LiftUp = false;
            LiftDown = false;
            RotateLeft = false;
            RotateRight = false;

            ModeSelect = false;
            Enable = false;

            Speed = 0;
            Linear = 0.0;
            Angular = 0.0;
            Mode = RFMode.None;

            _cachedJoyState = null;

            UpdateProperties();
        }

        Updated?.Invoke();
    }

    private void DecodeRegisters(ushort[] regs)
    {
        byte d0 = (byte)(regs[0] >> 8); // Word0H
        byte d1 = (byte)(regs[0]);      // Word0L
        byte d2 = (byte)(regs[1] >> 8); // Word1H
        byte d3 = (byte)(regs[1]);      // Word1L
        byte d6 = (byte)(regs[3] >> 8); // Word3H (JOY FB)
        byte d7 = (byte)(regs[3]);      // Word3L (JOY LR)
        lock (_lock)
        {
            // ===== System =====
            Heartbeat = (d0 >> 4) & 0x0F;
            RemoteReady = (d0 & 0x04) != 0;
            RemoteReady = !RemoteReady;
            EStop = (d0 & 0x01) != 0;

            // ===== Buttons =====
            Enable = (d1 & 0x80) != 0;
            ModeSelect = (d1 & 0x40) != 0;

            LiftUp = (d1 & 0x01) != 0;
            LiftDown = (d1 & 0x02) != 0;
            RotateLeft = (d1 & 0x04) != 0;
            RotateRight = (d1 & 0x08) != 0;

            // ===== Speed =====
            Speed = Math.Clamp((int)d3, 0, 100);
            // ===== Mode =====
            Mode = DecodeMode(d2);

            // ===== Safety =====
            if (!RemoteReady || !Enable || EStop)
            {
                Linear = 0;
                Angular = 0;
            }
            else
            {
                // ===== Joystick ANALOG =====
                Linear = (d6 - 127f) / 127f;
                Angular = (127f - d7) / 127f;
            }

            LastUpdateTime = DateTime.UtcNow;

            // Update cached JoyState
            _cachedJoyState = CreateJoyStateFromCache();
            
            UpdateProperties();
        }

        Updated?.Invoke();
    }

    private static RFMode DecodeMode(byte d2) =>
        (d2 & 0x0F) switch
        {
            0x00 => RFMode.Default,
            0x01 => RFMode.Maintenance,
            0x02 => RFMode.Override,
            _ => RFMode.None
        };

    // ===================== IRfHandle Implementation ===========

    public Task<Joy> ReadJoyStateAsync(CancellationToken cancellationToken = default)
    {
        lock (_lock)
        {
            if (_cachedJoyState.HasValue)
            {
                return Task.FromResult(_cachedJoyState.Value);
            }
            
            return Task.FromResult(CreateJoyStateFromCache());
        }
    }

    private Joy CreateJoyStateFromCache()
    {
        return new Joy
        {
            Header = new Header
            {
                Stamp = LastUpdateTime == default ? DateTime.UtcNow : LastUpdateTime,
                FrameId = "rfhandle_frame"
            },
            Axes =
            [
                Linear,                 // Axis 0: Forward / Backward
                Angular,                // Axis 1: Left / Right
                Speed / 100f            // Axis 2: Speed
            ],
            Buttons =
            [
                LiftUp ? 1 : 0,
                LiftDown ? 1 : 0,
                RotateLeft ? 1 : 0,
                RotateRight ? 1 : 0,
                ModeSelect ? 1 : 0,
                Enable ? 1 : 0,
                EStop ? 1 : 0
            ]
        };
    }

}