using System;
using System.IO;
using System.IO.Ports;
using System.Threading;

namespace RobotNet10.RobotApp.Drivers.HfiA9IMU
{
    /// <summary>
    /// Doc du lieu tu IMU HandsFree HFI-A9 qua serial (CP210x USB-UART, 921600 baud).
    /// Giao thuc: frame = 0xAA 0x55 [LEN] [payload LEN bytes] [CRC16-Modbus lo, hi]
    ///   CRC tinh tren [LEN] + payload.
    ///   LEN 0x2C: 10 float LE tai payload offset 4:
    ///             [0]=timestamp, [1:4]=gyro (rad/s), [4:7]=accel (g), [7:10]=mag
    ///   LEN 0x14: 4 float LE tai payload offset 4: [1:4]=roll/pitch/yaw (do)
    /// </summary>
    public class HfiA9Reader : IDisposable
    {
        // Frame lon nhat: 0x2C + 5 = 49 bytes, dung 256 bytes de dam bao an toan
        private const int FRAME_BUFFER_SIZE = 256;
        private readonly byte[] _frameBuffer = new byte[FRAME_BUFFER_SIZE];
        private int _frameBufferLength = 0;

        // Non-volatile field de dung voi Volatile.Write lam memory barrier
        private int _memoryBarrier = 0;

        // Thread doc du lieu tu SerialPort voi priority cao
        private Thread? _readingThread;
        private volatile bool _shouldRead = false;
        private CancellationTokenSource? _readingCts;

        // Event de thong bao khi co du lieu moi duoc decode (fire sau frame Euler
        // de moi event tuong ung mot chu ky mau day du: gyro/accel/mag + euler)
        public event EventHandler? DataReceived;

        // Event bao trang thai ket noi thay doi (true = vua mo port, false = mat ket noi)
        // de device class bao cho DeviceBase cap nhat state machine / UI
        public event EventHandler<bool>? ConnectionStateChanged;

        // Properties - lock-free voi memory barriers (volatile khong ho tro double)
        public double Roll { get; private set; }
        public double Pitch { get; private set; }
        public double Yaw { get; private set; }

        public double Gx { get; private set; }
        public double Gy { get; private set; }
        public double Gz { get; private set; }

        public double AccX { get; private set; }
        public double AccY { get; private set; }
        public double AccZ { get; private set; }

        public double MagX { get; private set; }
        public double MagY { get; private set; }
        public double MagZ { get; private set; }

        const byte FRAME_HEAD_1 = 0xAA;
        const byte FRAME_HEAD_2 = 0x55;

        // LEN byte cua tung loai goi
        const byte LEN_IMU = 0x2C;   // 44: timestamp + gyro + accel + mag (10 float)
        const byte LEN_EULER = 0x14; // 20: timestamp + roll/pitch/yaw (4 float)

        // HFI-A9 xuat accel theo don vi g; nhan -9.8 theo quy uoc truc cua driver hang
        // (dung yen, Z huong len -> AccZ ~ +9.8 m/s², cung quy uoc voi Wheeltec N100)
        private const double ACCEL_SCALE = -9.8;
        private const double DEG_TO_RAD = Math.PI / 180.0;

        private SerialPort? serial;

        // Luu thong so port de tao lai SerialPort khi reconnect
        private readonly string _portName;
        private readonly int _baudRate;
        private readonly int _timeOut;

        // Thoi gian backoff giua cac lan thu reconnect (ms)
        private const int RECONNECT_BACKOFF_MS = 1000;

        // Watchdog: neu qua khoang thoi gian nay khong co frame hop le -> coi nhu mat ket noi
        // va trigger reconnect. Dung cho truong hop USB "chet mem" khong ne'm exception.
        private const long DATA_TIMEOUT_TICKS = 3 * TimeSpan.TicksPerSecond;
        private long _lastFrameTicks;

        // Kiem tra dinh ky file port con ton tai khong (rut USB -> /dev/ttyUSBx bien mat)
        // de phat hien disconnect nhanh hon watchdog
        private const long PORT_CHECK_INTERVAL_TICKS = TimeSpan.TicksPerSecond / 2;
        private long _lastPortCheckTicks;

        public bool IsConnected => serial != null && serial.IsOpen;

        /// <summary>
        /// Co frame hop le trong DATA_TIMEOUT gan nhat khong — dung de phan biet
        /// "port mo nhung khong co du lieu" (vd ModemManager dang giu port sau khi cam lai)
        /// </summary>
        public bool HasRecentData => DateTime.UtcNow.Ticks - Volatile.Read(ref _lastFrameTicks) < DATA_TIMEOUT_TICKS;

        public HfiA9Reader(string portName, int baudRate, int timeOut)
        {
            _portName = portName;
            _baudRate = baudRate;
            _timeOut = timeOut;

            serial = CreateSerialPort();
        }

        private SerialPort CreateSerialPort()
        {
            return new SerialPort()
            {
                PortName = _portName,
                BaudRate = _baudRate,
                ReadTimeout = _timeOut,
                Parity = Parity.None,
                StopBits = StopBits.One,
                DataBits = 8,
            };
        }

        /// <summary>
        /// Snapshot structure de lay tat ca du lieu cung luc mot cach thread-safe
        /// </summary>
        public struct DataSnapshot
        {
            public double AccX, AccY, AccZ;
            public double Gx, Gy, Gz;
            public double MagX, MagY, MagZ;
            public double Roll, Pitch, Yaw;
        }

        /// <summary>
        /// Lay snapshot cua tat ca du lieu hien tai mot cach thread-safe
        /// </summary>
        public DataSnapshot GetSnapshot()
        {
            Volatile.Read(ref _memoryBarrier);
            return new DataSnapshot
            {
                AccX = AccX,
                AccY = AccY,
                AccZ = AccZ,
                Gx = Gx,
                Gy = Gy,
                Gz = Gz,
                MagX = MagX,
                MagY = MagY,
                MagZ = MagZ,
                Roll = Roll,
                Pitch = Pitch,
                Yaw = Yaw,
            };
        }

        public void Connect()
        {
            _frameBufferLength = 0;
            Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);

            // Reading thread se tu mo port trong outer loop va tu reconnect khi mat ket noi
            StartReadingThread();
        }

        private void StartReadingThread()
        {
            if (_readingThread != null && _readingThread.IsAlive)
                return;

            _shouldRead = true;

            _readingCts?.Dispose();
            _readingCts = new CancellationTokenSource();

            _readingThread = new Thread(() => ReadingThreadLoop(_readingCts.Token))
            {
                Name = "HfiA9IMU-Reading",
                IsBackground = false,
                Priority = ThreadPriority.Highest
            };

            _readingThread.Start();
        }

        private void StopReadingThread()
        {
            _shouldRead = false;

            _readingCts?.Cancel();

            if (_readingThread != null)
            {
                if (!_readingThread.Join(1000))
                {
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Reading thread did not stop gracefully");
                }
                _readingThread = null;
            }

            _readingCts?.Dispose();
            _readingCts = null;
        }

        /// <summary>
        /// Reading thread loop - outer loop xu ly reconnect, inner loop doc du lieu
        /// </summary>
        private void ReadingThreadLoop(CancellationToken cancellationToken)
        {
            Thread.BeginThreadAffinity();
            byte[] readBuffer = new byte[256];

            while (_shouldRead && !cancellationToken.IsCancellationRequested)
            {
                try
                {
                    if (serial == null || !serial.IsOpen)
                    {
                        SafeCloseSerial();
                        serial = CreateSerialPort();
                        serial.Open();
                        _frameBufferLength = 0;
                        serial.DiscardInBuffer();
                        _lastFrameTicks = DateTime.UtcNow.Ticks;
                        Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Connected to {_portName}");
                        RaiseConnectionStateChanged(true);
                    }

                    InnerReadLoop(readBuffer, cancellationToken);
                }
                catch (OperationCanceledException)
                {
                    break;
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] IMU disconnected: {ex.Message}");
                    SafeCloseSerial();
                    RaiseConnectionStateChanged(false);
                }

                if (_shouldRead && !cancellationToken.IsCancellationRequested)
                {
                    cancellationToken.WaitHandle.WaitOne(RECONNECT_BACKOFF_MS);
                }
            }

            SafeCloseSerial();
            Thread.EndThreadAffinity();
        }

        private void InnerReadLoop(byte[] readBuffer, CancellationToken cancellationToken)
        {
            while (_shouldRead && !cancellationToken.IsCancellationRequested
                   && serial != null && serial.IsOpen)
            {
                // Watchdog phai kiem tra MOI vong lap: khi rut USB tren Linux,
                // BytesToRead co the van > 0 nhung Read() tra ve 0 (EOF) lien tuc,
                // neu chi kiem tra trong nhanh bytesToRead <= 0 se khong bao gio phat hien
                long nowTicks = DateTime.UtcNow.Ticks;
                if (nowTicks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
                {
                    throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s");
                }

                // Rut USB -> device file bien mat: phat hien nhanh hon watchdog
                if (nowTicks - _lastPortCheckTicks > PORT_CHECK_INTERVAL_TICKS)
                {
                    _lastPortCheckTicks = nowTicks;
                    if (!File.Exists(_portName))
                    {
                        throw new IOException($"Serial port {_portName} no longer exists (USB unplugged?)");
                    }
                }

                int bytesToRead = serial.BytesToRead;
                if (bytesToRead <= 0)
                {
                    Thread.Sleep(1);
                    continue;
                }

                int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
                if (bytesRead <= 0)
                {
                    Thread.Sleep(1);
                    continue;
                }

                ProcessIncomingData(readBuffer, bytesRead);
            }
        }

        private void SafeCloseSerial()
        {
            if (serial == null) return;

            try
            {
                serial.Close();
            }
            catch (Exception ex)
            {
                Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Error closing serial: {ex.Message}");
            }

            try { serial.Dispose(); }
            catch { }

            serial = null;
        }

        /// <summary>
        /// Them du lieu moi vao frame buffer va parse cac frame hoan chinh
        /// </summary>
        private void ProcessIncomingData(byte[] data, int length)
        {
            int dataOffset = 0;

            while (dataOffset < length)
            {
                ProcessCompleteFramesInBuffer();

                int availableSpace = FRAME_BUFFER_SIZE - _frameBufferLength;

                if (availableSpace == 0)
                {
                    // Buffer day ma khong co frame hop le -> du lieu rac, xoa het
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Buffer full without valid frame, clearing buffer");
                    _frameBufferLength = 0;
                    availableSpace = FRAME_BUFFER_SIZE;
                }

                int bytesToAdd = Math.Min(length - dataOffset, availableSpace);
                Array.Copy(data, dataOffset, _frameBuffer, _frameBufferLength, bytesToAdd);
                _frameBufferLength += bytesToAdd;
                dataOffset += bytesToAdd;

                ProcessCompleteFramesInBuffer();
            }
        }

        /// <summary>
        /// Xu ly tat ca cac frame hoan chinh trong buffer hien tai
        /// </summary>
        private void ProcessCompleteFramesInBuffer()
        {
            while (_frameBufferLength > 0)
            {
                ReadOnlySpan<byte> bufferSpan = new(_frameBuffer, 0, _frameBufferLength);
                int headIndex = bufferSpan.IndexOf(FRAME_HEAD_1);

                if (headIndex < 0)
                {
                    _frameBufferLength = 0;
                    break;
                }

                if (headIndex > 0)
                {
                    ShiftBuffer(headIndex);
                    continue;
                }

                // Can toi thieu 3 bytes: AA 55 LEN
                if (_frameBufferLength < 3)
                    break;

                if (_frameBuffer[1] != FRAME_HEAD_2)
                {
                    ShiftBuffer(1);
                    continue;
                }

                byte payloadLength = _frameBuffer[2];
                if (payloadLength != LEN_IMU && payloadLength != LEN_EULER)
                {
                    ShiftBuffer(1);
                    continue;
                }

                int totalFrameLength = payloadLength + 5;
                if (_frameBufferLength < totalFrameLength)
                    break;

                // CRC16-Modbus tren [LEN + payload], luu little-endian sau payload
                ushort crcCalc = Crc16Modbus(_frameBuffer.AsSpan(2, 1 + payloadLength));
                ushort crcRecv = (ushort)(_frameBuffer[3 + payloadLength] | (_frameBuffer[4 + payloadLength] << 8));
                if (crcCalc != crcRecv)
                {
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] CRC16 error: recv={crcRecv:X4}, calc={crcCalc:X4}");
                    ShiftBuffer(2);
                    continue;
                }

                try
                {
                    DecodeFrame(payloadLength, _frameBuffer.AsSpan(3, payloadLength));
                    _lastFrameTicks = DateTime.UtcNow.Ticks;
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] Error parsing frame: {ex.Message}");
                }

                ShiftBuffer(totalFrameLength);
            }
        }

        private void ShiftBuffer(int count)
        {
            int remaining = _frameBufferLength - count;
            if (remaining > 0)
            {
                Array.Copy(_frameBuffer, count, _frameBuffer, 0, remaining);
            }
            _frameBufferLength = Math.Max(0, remaining);
        }

        private void DecodeFrame(byte payloadLength, ReadOnlySpan<byte> payload)
        {
            if (payloadLength == LEN_IMU)
            {
                // float [0] la timestamp noi bo, bo qua
                Gx = BitConverter.ToSingle(payload.Slice(8, 4));
                Gy = BitConverter.ToSingle(payload.Slice(12, 4));
                Gz = BitConverter.ToSingle(payload.Slice(16, 4));
                AccX = BitConverter.ToSingle(payload.Slice(20, 4)) * ACCEL_SCALE;
                AccY = BitConverter.ToSingle(payload.Slice(24, 4)) * ACCEL_SCALE;
                AccZ = BitConverter.ToSingle(payload.Slice(28, 4)) * ACCEL_SCALE;
                MagX = BitConverter.ToSingle(payload.Slice(32, 4));
                MagY = BitConverter.ToSingle(payload.Slice(36, 4));
                MagZ = BitConverter.ToSingle(payload.Slice(40, 4));
                Volatile.Write(ref _memoryBarrier, 0);
            }
            else if (payloadLength == LEN_EULER)
            {
                Roll = BitConverter.ToSingle(payload.Slice(8, 4)) * DEG_TO_RAD;
                Pitch = BitConverter.ToSingle(payload.Slice(12, 4)) * DEG_TO_RAD;
                Yaw = BitConverter.ToSingle(payload.Slice(16, 4)) * DEG_TO_RAD;
                Volatile.Write(ref _memoryBarrier, 0);

                // Frame Euler den sau frame IMU trong moi chu ky -> fire event mot lan/chu ky
                try
                {
                    DataReceived?.Invoke(this, EventArgs.Empty);
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] DataReceived subscriber error: {ex.Message}");
                }
            }
        }

        private void RaiseConnectionStateChanged(bool connected)
        {
            try
            {
                ConnectionStateChanged?.Invoke(this, connected);
            }
            catch (Exception ex)
            {
                Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [HfiA9Reader] ConnectionStateChanged subscriber error: {ex.Message}");
            }
        }

        private static ushort Crc16Modbus(ReadOnlySpan<byte> data)
        {
            ushort crc = 0xFFFF;
            foreach (byte b in data)
            {
                crc ^= b;
                for (int i = 0; i < 8; i++)
                {
                    if ((crc & 1) != 0)
                        crc = (ushort)((crc >> 1) ^ 0xA001);
                    else
                        crc >>= 1;
                }
            }
            return crc;
        }

        public void Disconnect()
        {
            StopReadingThread();

            SafeCloseSerial();

            _frameBufferLength = 0;
            Array.Clear(_frameBuffer, 0, FRAME_BUFFER_SIZE);
        }

        public void Dispose()
        {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        protected virtual void Dispose(bool disposing)
        {
            if (disposing)
            {
                Disconnect();
            }
        }
    }
}