using System;
using System.Collections.Generic;
using System.IO.Ports;
using System.Threading;

namespace RobotNet10.RobotApp.Drivers.WheeltecIMU
{
    public class WheeltecReader : IDisposable
    {
        // Buffer nho de tich luy du lieu cho den khi co du mot frame hoan chinh
        // Frame lon nhat: INSGPS = 8 + 84 = 92 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
        public event EventHandler? DataReceived;

        // 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 uint Time_stamp { 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; }

        public double Temp { get; private set; }

        public double Rollspeed { get; private set; }
        public double Pitchspeed { get; private set; }
        public double Yawspeed { get; private set; }

        const byte FRAME_HEAD = 0xFC;
        const byte FRAME_END = 0xFD;

        // Loai goi
        const byte TYPE_IMU = 0x40;
        const byte TYPE_AHRS = 0x41;
        const byte TYPE_INSGPS = 0x42;
        const byte TYPE_GROUND = 0xF0;

        // Chieu dai payload
        const byte IMU_LEN = 0x38;    // 56
        const byte AHRS_LEN = 0x30;   // 48
        const byte INSGPS_LEN = 0x54; // 84

        // Dictionary de map datatype -> expectedLength
        private static readonly Dictionary<byte, byte> DataTypeLengthMap = new()
        {
            { TYPE_IMU, IMU_LEN },
            { TYPE_AHRS, AHRS_LEN },
            { TYPE_INSGPS, INSGPS_LEN }
        };

        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;

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

        public WheeltecReader(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;
            public double Temp;
        }

        /// <summary>
        /// Lay snapshot cua tat ca du lieu hien tai mot cach thread-safe
        /// Dam bao tat ca cac gia tri deu tu cung mot thoi diem
        /// </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,
                Temp = Temp
            };
        }

        public void Connect()
        {
            // Reset frame buffer khi ket noi moi
            _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();
        }

        /// <summary>
        /// Khoi dong reading thread voi priority cao de doc du lieu tu SerialPort
        /// </summary>
        private void StartReadingThread()
        {
            if (_readingThread != null && _readingThread.IsAlive)
                return;

            _shouldRead = true;

            // Tao moi CancellationTokenSource cho thread moi
            _readingCts?.Dispose();
            _readingCts = new CancellationTokenSource();

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

            _readingThread.Start();
        }

        /// <summary>
        /// Dung reading thread
        /// </summary>
        private void StopReadingThread()
        {
            _shouldRead = false;

            _readingCts?.Cancel();

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

            // Dispose CancellationTokenSource sau khi thread da dung
            _readingCts?.Dispose();
            _readingCts = null;
        }

        /// <summary>
        /// Reading thread loop - outer loop xu ly reconnect, inner loop doc du lieu
        /// Moi exception tu SerialPort deu duoc bat de tranh crash thread va tu dong
        /// reconnect sau RECONNECT_BACKOFF_MS
        /// </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} [WheeltecReader] Connected to {_portName}");
                    }

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

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

            SafeCloseSerial();
            Thread.EndThreadAffinity();
        }

        /// <summary>
        /// Inner loop doc du lieu tu serial port. Thoat khi port dong hoac co exception
        /// de outer loop xu ly reconnect
        /// </summary>
        private void InnerReadLoop(byte[] readBuffer, CancellationToken cancellationToken)
        {
            while (_shouldRead && !cancellationToken.IsCancellationRequested
                   && serial != null && serial.IsOpen)
            {
                int bytesToRead = serial.BytesToRead;
                if (bytesToRead <= 0)
                {
                    if (DateTime.UtcNow.Ticks - _lastFrameTicks > DATA_TIMEOUT_TICKS)
                    {
                        throw new TimeoutException($"No IMU frame for > {DATA_TIMEOUT_TICKS / TimeSpan.TicksPerSecond}s");
                    }
                    Thread.Sleep(1);
                    continue;
                }

                int bytesRead = serial.Read(readBuffer, 0, Math.Min(bytesToRead, readBuffer.Length));
                if (bytesRead <= 0) continue;

                ProcessIncomingData(readBuffer, bytesRead);
            }
        }

        /// <summary>
        /// Dong va dispose SerialPort an toan, set serial=null de lan sau tao moi.
        /// SerialPort sau IOException thuong khong Open() lai duoc tren Linux nen phai tao moi.
        /// </summary>
        private void SafeCloseSerial()
        {
            if (serial == null) return;

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

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

            serial = null;
        }

        /// <summary>
        /// Xu ly du lieu moi nhan duoc tu serial port
        /// Them vao frame buffer va tim, 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)
                {
                    RemoveIncompleteFrameAtStart();
                    availableSpace = FRAME_BUFFER_SIZE - _frameBufferLength;

                    if (availableSpace == 0)
                    {
                        Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Buffer still full after removing incomplete 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);

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

                if (headIndex > 0)
                {
                    int remainingBytes = _frameBufferLength - headIndex;
                    Array.Copy(_frameBuffer, headIndex, _frameBuffer, 0, remainingBytes);
                    _frameBufferLength = remainingBytes;
                    continue;
                }

                if (_frameBufferLength < 8)
                {
                    break;
                }

                byte datatype = _frameBuffer[1];
                byte payloadLength = _frameBuffer[2];

                if (!DataTypeLengthMap.TryGetValue(datatype, out byte expectedLength))
                {
                    int remainingBytes = _frameBufferLength - 1;
                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
                    _frameBufferLength = remainingBytes;
                    continue;
                }

                if (payloadLength != expectedLength)
                {
                    int remainingBytes = _frameBufferLength - 1;
                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
                    _frameBufferLength = remainingBytes;
                    continue;
                }

                int totalFrameLength = 8 + payloadLength;
                if (_frameBufferLength < totalFrameLength)
                {
                    break;
                }

                if (_frameBuffer[7 + payloadLength] != FRAME_END)
                {
                    int remainingBytes = _frameBufferLength - 1;
                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingBytes);
                    _frameBufferLength = remainingBytes;
                    continue;
                }

                byte[] frame = new byte[totalFrameLength];
                Array.Copy(_frameBuffer, 0, frame, 0, totalFrameLength);

                int remainingAfterFrame = _frameBufferLength - totalFrameLength;
                if (remainingAfterFrame > 0)
                {
                    Array.Copy(_frameBuffer, totalFrameLength, _frameBuffer, 0, remainingAfterFrame);
                }
                _frameBufferLength = remainingAfterFrame;

                try
                {
                    if (ParseFrame(frame))
                    {
                        _lastFrameTicks = DateTime.UtcNow.Ticks;
                        try
                        {
                            DataReceived?.Invoke(this, EventArgs.Empty);
                        }
                        catch (Exception ex)
                        {
                            Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] DataReceived subscriber error: {ex.Message}");
                        }
                    }
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"{DateTime.Now:HH:mm:ss.ffffff} [WheeltecReader] Error parsing frame: {ex.Message}");
                }
            }
        }

        /// <summary>
        /// Loai bo frame thieu o dau buffer va tim frame head tiep theo
        /// </summary>
        private void RemoveIncompleteFrameAtStart()
        {
            if (_frameBufferLength == 0)
                return;

            ReadOnlySpan<byte> bufferSpan = new(_frameBuffer, 0, _frameBufferLength);
            int headIndex = bufferSpan.IndexOf(FRAME_HEAD);

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

            if (headIndex == 0)
            {
                if (_frameBufferLength < 8)
                    return;

                byte datatype = _frameBuffer[1];
                if (!DataTypeLengthMap.TryGetValue(datatype, out byte expectedLength))
                {
                    int remainingAfterSkip = _frameBufferLength - 1;
                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingAfterSkip);
                    _frameBufferLength = remainingAfterSkip;
                    return;
                }

                byte payloadLength = _frameBuffer[2];
                if (payloadLength != expectedLength)
                {
                    int remainingAfterSkip = _frameBufferLength - 1;
                    Array.Copy(_frameBuffer, 1, _frameBuffer, 0, remainingAfterSkip);
                    _frameBufferLength = remainingAfterSkip;
                    return;
                }

                int totalFrameLength = 8 + payloadLength;
                if (_frameBufferLength < totalFrameLength)
                    return;

                return;
            }

            int remainingAfterHead = _frameBufferLength - headIndex;
            Array.Copy(_frameBuffer, headIndex, _frameBuffer, 0, remainingAfterHead);
            _frameBufferLength = remainingAfterHead;
        }

        public void Disconnect()
        {
            StopReadingThread();

            SafeCloseSerial();

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

        private void DecodeIMU(byte[] payload)
        {
            Gx = BitConverter.ToSingle(payload, 0);
            Gy = BitConverter.ToSingle(payload, 4);
            Gz = BitConverter.ToSingle(payload, 8);
            AccX = BitConverter.ToSingle(payload, 12);
            AccY = BitConverter.ToSingle(payload, 16);
            AccZ = BitConverter.ToSingle(payload, 20);
            MagX = BitConverter.ToSingle(payload, 24);
            MagY = BitConverter.ToSingle(payload, 28);
            MagZ = BitConverter.ToSingle(payload, 32);
            Temp = BitConverter.ToSingle(payload, 36);
            Time_stamp = BitConverter.ToUInt32(payload, 40);
            Volatile.Write(ref _memoryBarrier, 0);
        }

        private void DecodeAHRS(byte[] payload)
        {
            double rollspeed = BitConverter.ToSingle(payload, 0);
            double pitchspeed = BitConverter.ToSingle(payload, 4);
            double yawspeed = BitConverter.ToSingle(payload, 8);

            double roll = BitConverter.ToSingle(payload, 12);
            double pitch = BitConverter.ToSingle(payload, 16);
            double yaw = BitConverter.ToSingle(payload, 20);

            Rollspeed = rollspeed;
            Pitchspeed = pitchspeed;
            Yawspeed = yawspeed;
            Roll = roll;
            Pitch = pitch;
            Yaw = yaw;
            Volatile.Write(ref _memoryBarrier, 0);
        }

        private void DecodeINSGPS(byte[] payload)
        {
            double latitude = BitConverter.ToDouble(payload, 0);
            double longitude = BitConverter.ToDouble(payload, 8);
            double altitude = BitConverter.ToSingle(payload, 16);

            double vn = BitConverter.ToSingle(payload, 20);
            double ve = BitConverter.ToSingle(payload, 24);
            double vd = BitConverter.ToSingle(payload, 28);

            double roll = BitConverter.ToSingle(payload, 32);
            double pitch = BitConverter.ToSingle(payload, 36);
            double yaw = BitConverter.ToSingle(payload, 40);

            double qw = BitConverter.ToSingle(payload, 44);
            double qx = BitConverter.ToSingle(payload, 48);
            double qy = BitConverter.ToSingle(payload, 52);
            double qz = BitConverter.ToSingle(payload, 56);
        }

        /// <summary>
        /// Parse mot frame hoan chinh tu buffer
        /// </summary>
        private bool ParseFrame(byte[] frame)
        {
            if (frame.Length < 8)
                return false;

            byte head = frame[0];
            if (head != FRAME_HEAD)
                return false;

            byte datatype = frame[1];
            byte length = frame[2];
            byte sn = frame[3];
            byte crc8 = frame[4];
            byte crc16_h = frame[5];
            byte crc16_l = frame[6];
            ushort head_crc16 = (ushort)(crc16_l + (crc16_h << 8));

            Span<byte> header = [head, datatype, length, sn];
            byte crc8_calc = CRCTable.CRC8_Table(header);
            if (crc8_calc != crc8)
            {
                throw new Exception($"CRC8 header error: recv={crc8:X2}, calc={crc8_calc:X2}");
            }

            if (frame[7 + length] != FRAME_END)
            {
                throw new Exception($"Frame end error: {BitConverter.ToString(frame)}");
            }

            ReadOnlySpan<byte> payload = frame.AsSpan(7, length);

            ushort crc16_calc = CRCTable.CRC16_Table(payload);
            if (crc16_calc != head_crc16)
            {
                throw new Exception($"CRC16 payload error: recv={head_crc16:X4}, calc={crc16_calc:X4}");
            }

            switch (datatype)
            {
                case TYPE_AHRS:
                    DecodeAHRS([..payload]);
                    break;
                case TYPE_IMU:
                    DecodeIMU([..payload]);
                    break;
                case TYPE_INSGPS:
                    DecodeINSGPS([..payload]);
                    break;
            }

            return true;
        }

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

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