using System;
using System.IO.Ports;
using System.Text;

namespace ImuReader
{
    public class IMUFrame
    {
        public float Gx, Gy, Gz;
        public float Ax, Ay, Az;
        public float Mx, My, Mz;
        public float Roll, Pitch, Yaw;
        public float Temp, Counter;
    }

    public class AHRSFrame
    {
        public float Roll, Pitch, Yaw;
        public float Q0, Q1, Q2, Q3;
        public float GbX, GbY, GbZ;
        public float Counter;
    }

    public class INSGPSFrame
    {
        public double Lat, Lon, Alt;
        public float Vx, Vy, Vz;
        public float Roll, Pitch, Yaw;
        public int Status;
        public float Counter;
    }

    internal class Program
    {
        // Frame định nghĩa
        const byte FRAME_HEAD = 0xFC;
        const byte FRAME_END = 0xFD;

        // Loại gói
        const byte TYPE_IMU = 0x40;
        const byte TYPE_AHRS = 0x41;
        const byte TYPE_INSGPS = 0x42;
        const byte TYPE_GROUND = 0xF0;

        // Chiều dài payload
        const int IMU_LEN = 56;    // 0x38
        const int AHRS_LEN = 48;   // 0x30
        const int INSGPS_LEN = 84; // 0x54

        static bool if_debug_ = true;
        static bool first_sn_ = false;
        static int read_sn_ = -1;
        static int sn_lost_ = 0;

        static void Main(string[] args)
        {
            Console.OutputEncoding = Encoding.UTF8;

            SerialPort serial = new SerialPort()
            {
                PortName = "COM5",    // chỉnh lại cho đúng cổng
                BaudRate = 921600,
                ReadTimeout = 2000,
                Parity = Parity.None,
                StopBits = StopBits.One,
                DataBits = 8,
            };

            try
            {
                serial.Open();
                Console.WriteLine("[INFO] Serial port opened.");
            }
            catch (Exception ex)
            {
                Console.WriteLine("[ERROR] Cannot open port: " + ex.Message);
                return;
            }

            while (serial.IsOpen)
            {
                try
                {
                    // B1: Tìm header (đồng bộ)
                    int headInt = serial.ReadByte();
                    if (headInt < 0) { Console.WriteLine("[WARN] ReadByte returned -1"); continue; }
                    byte head = (byte)headInt;
                    if (head != FRAME_HEAD)
                    {
                        if (if_debug_) Console.WriteLine($"[DEBUG] Bỏ byte: 0x{head:X2}");
                        continue;
                    }

                    // B2: đọc datatype, length
                    int dt = serial.ReadByte();
                    if (dt < 0) { Console.WriteLine("[WARN] datatype timeout"); continue; }
                    byte datatype = (byte)dt;

                    int ln = serial.ReadByte();
                    if (ln < 0) { Console.WriteLine("[WARN] length timeout"); continue; }
                    byte length = (byte)ln;

                    if (if_debug_) Console.WriteLine($"[DEBUG] Datatype: 0x{datatype:X2}, Length: {length}");

                    // Kiểm tra loại hợp lệ
                    if (datatype != TYPE_IMU && datatype != TYPE_AHRS && datatype != TYPE_INSGPS && datatype != TYPE_GROUND)
                    {
                        Console.WriteLine("[WARN] datatype error");
                        continue;
                    }

                    // Kiểm tra độ dài hợp lệ cho từng type
                    if (datatype == TYPE_IMU && length != IMU_LEN) { Console.WriteLine($"[ERROR] IMU length error expected {IMU_LEN} got {length}"); continue; }
                    if (datatype == TYPE_AHRS && length != AHRS_LEN) { Console.WriteLine($"[ERROR] AHRS length error expected {AHRS_LEN} got {length}"); continue; }
                    if (datatype == TYPE_INSGPS && length != INSGPS_LEN) { Console.WriteLine($"[ERROR] INSGPS length error expected {INSGPS_LEN} got {length}"); continue; }

                    // Nếu là GROUND hoặc code 0x50 -> bỏ qua frame này (như code gốc)
                    if (datatype == TYPE_GROUND || length == 0x50)
                    {
                        // read sn
                        int gsn = serial.ReadByte();
                        if (gsn < 0) { Console.WriteLine("[WARN] ground sn timeout"); continue; }
                        byte groundSn = (byte)gsn;

                        // update sn lost similar to C++ logic
                        if (!first_sn_)
                        {
                            read_sn_ = (groundSn - 1) & 0xFF;
                            first_sn_ = true;
                        }
                        int expected = (read_sn_ + 1) & 0xFF;
                        if (groundSn != expected)
                        {
                            int lost = groundSn > expected ? groundSn - expected : 256 - (expected - groundSn);
                            sn_lost_ += lost;
                            Console.WriteLine($"[WARN] ground SN lost {lost}, total lost {sn_lost_}");
                        }
                        read_sn_ = groundSn;

                        // đọc phần còn lại và bỏ qua
                        int bytesToRead = (length + 4);
                        if (bytesToRead > 500) bytesToRead = 500;
                        byte[] ground_ignore = new byte[bytesToRead];
                        int got = serial.Read(ground_ignore, 0, bytesToRead);
                        if (if_debug_) Console.WriteLine($"[DEBUG] Bỏ {got} byte ground");
                        continue;
                    }

                    // B3: đọc sn, crc8, crc16_h, crc16_l
                    int sn_b = serial.ReadByte();
                    int crc8_b = serial.ReadByte();
                    int crc16_h_b = serial.ReadByte();
                    int crc16_l_b = serial.ReadByte();
                    if (sn_b < 0 || crc8_b < 0 || crc16_h_b < 0 || crc16_l_b < 0)
                    {
                        Console.WriteLine("[WARN] header tail bytes timeout");
                        continue;
                    }

                    byte sn = (byte)sn_b;
                    byte crc8_recv = (byte)crc8_b;
                    byte crc16_h = (byte)crc16_h_b;
                    byte crc16_l = (byte)crc16_l_b;

                    // Tạo header array để check CRC8 (4 byte: head, datatype, length, sn)
                    byte[] header4 = new byte[4] { head, datatype, length, sn };
                    byte crc8_calc = CRC8_Table(header4, 4);
                    if (crc8_calc != crc8_recv)
                    {
                        Console.WriteLine($"[WARN] header_crc8 error calc=0x{crc8_calc:X2} recv=0x{crc8_recv:X2}");
                        continue;
                    }

                    // SN handling (check lost)
                    if (!first_sn_)
                    {
                        read_sn_ = (sn - 1) & 0xFF;
                        first_sn_ = true;
                    }
                    int expectedSn = (read_sn_ + 1) & 0xFF;
                    if (sn != expectedSn)
                    {
                        int lost = sn > expectedSn ? sn - expectedSn : 256 - (expectedSn - sn);
                        sn_lost_ += lost;
                        Console.WriteLine($"[WARN] SN lost {lost}, total lost {sn_lost_}");
                    }
                    read_sn_ = sn;

                    // B4: đọc payload + end
                    int payloadLen = length;
                    byte[] payload_plus_end = new byte[payloadLen + 1];
                    int readPayload = 0;
                    while (readPayload < payload_plus_end.Length)
                    {
                        int r = serial.Read(payload_plus_end, readPayload, payload_plus_end.Length - readPayload);
                        if (r <= 0) break;
                        readPayload += r;
                    }
                    if (readPayload != payload_plus_end.Length)
                    {
                        Console.WriteLine($"[WARN] Thiếu payload, đọc được {readPayload}/{payload_plus_end.Length}");
                        continue;
                    }

                    if (payload_plus_end[payloadLen] != FRAME_END)
                    {
                        Console.WriteLine($"[WARN] frame_end sai: 0x{payload_plus_end[payloadLen]:X2}");
                        continue;
                    }

                    // B5: check CRC16 (CRC16 trong header: high then low)
                    ushort crc16_recv = (ushort)((crc16_h << 8) | crc16_l);
                    ushort crc16_calc = CRC16_Table(payload_plus_end, payloadLen); // chỉ tính trên payload (không bao gồm end)
                    if (crc16_calc != crc16_recv)
                    {
                        Console.WriteLine($"[WARN] CRC16 mismatch calc=0x{crc16_calc:X4} recv=0x{crc16_recv:X4}");
                        continue;
                    }

                    // B6: parse theo loại
                    if (datatype == TYPE_IMU)
                    {
                        IMUFrame imu = ParseIMU(payload_plus_end);
                        PrintIMU(imu);
                    }
                    else if (datatype == TYPE_AHRS)
                    {
                        AHRSFrame ahrs = ParseAHRS(payload_plus_end);
                        PrintAHRS(ahrs);
                    }
                    else if (datatype == TYPE_INSGPS)
                    {
                        INSGPSFrame gps = ParseINSGPS(payload_plus_end);
                        PrintINSGPS(gps);
                    }
                }
                catch (TimeoutException)
                {
                    Console.WriteLine("[WARN] Timeout khi đọc serial.");
                }
                catch (Exception ex)
                {
                    Console.WriteLine($"[ERROR] {ex.Message}");
                }
            }
        }

        static IMUFrame ParseIMU(byte[] payloadPlusEnd)
        {
            IMUFrame imu = new IMUFrame();
            // payloadPlusEnd length = IMU_LEN + 1, but BitConverter offsets on payload start at 0
            imu.Gx = BitConverter.ToSingle(payloadPlusEnd, 0);
            imu.Gy = BitConverter.ToSingle(payloadPlusEnd, 4);
            imu.Gz = BitConverter.ToSingle(payloadPlusEnd, 8);

            imu.Ax = BitConverter.ToSingle(payloadPlusEnd, 12);
            imu.Ay = BitConverter.ToSingle(payloadPlusEnd, 16);
            imu.Az = BitConverter.ToSingle(payloadPlusEnd, 20);

            imu.Mx = BitConverter.ToSingle(payloadPlusEnd, 24);
            imu.My = BitConverter.ToSingle(payloadPlusEnd, 28);
            imu.Mz = BitConverter.ToSingle(payloadPlusEnd, 32);

            imu.Roll = BitConverter.ToSingle(payloadPlusEnd, 36);
            imu.Pitch = BitConverter.ToSingle(payloadPlusEnd, 40);
            imu.Yaw = BitConverter.ToSingle(payloadPlusEnd, 44);

            imu.Temp = BitConverter.ToSingle(payloadPlusEnd, 48);
            imu.Counter = BitConverter.ToSingle(payloadPlusEnd, 52);

            return imu;
        }

        static void PrintIMU(IMUFrame imu)
        {
            Console.WriteLine("=== IMU Data ===");
            Console.WriteLine($"Gyro : {imu.Gx:F3}, {imu.Gy:F3}, {imu.Gz:F3}");
            Console.WriteLine($"Accel: {imu.Ax:F3}, {imu.Ay:F3}, {imu.Az:F3}");
            Console.WriteLine($"Mag  : {imu.Mx:F3}, {imu.My:F3}, {imu.Mz:F3}");
            Console.WriteLine($"Euler: R={imu.Roll:F3}, P={imu.Pitch:F3}, Y={imu.Yaw:F3}");
            Console.WriteLine($"Temp : {imu.Temp:F3}, Counter={imu.Counter:F0}");
            Console.WriteLine("-------------------------------");
        }

        static AHRSFrame ParseAHRS(byte[] payloadPlusEnd)
        {
            AHRSFrame ahrs = new AHRSFrame();

            ahrs.Roll = BitConverter.ToSingle(payloadPlusEnd, 0);
            ahrs.Pitch = BitConverter.ToSingle(payloadPlusEnd, 4);
            ahrs.Yaw = BitConverter.ToSingle(payloadPlusEnd, 8);

            ahrs.Q0 = BitConverter.ToSingle(payloadPlusEnd, 12);
            ahrs.Q1 = BitConverter.ToSingle(payloadPlusEnd, 16);
            ahrs.Q2 = BitConverter.ToSingle(payloadPlusEnd, 20);
            ahrs.Q3 = BitConverter.ToSingle(payloadPlusEnd, 24);

            ahrs.GbX = BitConverter.ToSingle(payloadPlusEnd, 28);
            ahrs.GbY = BitConverter.ToSingle(payloadPlusEnd, 32);
            ahrs.GbZ = BitConverter.ToSingle(payloadPlusEnd, 36);

            ahrs.Counter = BitConverter.ToSingle(payloadPlusEnd, 40);

            return ahrs;
        }

        static void PrintAHRS(AHRSFrame ahrs)
        {
            Console.WriteLine("=== AHRS Data ===");
            Console.WriteLine($"Euler: R={ahrs.Roll:F3}, P={ahrs.Pitch:F3}, Y={ahrs.Yaw:F3}");
            Console.WriteLine($"Quat : {ahrs.Q0:F3}, {ahrs.Q1:F3}, {ahrs.Q2:F3}, {ahrs.Q3:F3}");
            Console.WriteLine($"GyroBias: {ahrs.GbX:F3}, {ahrs.GbY:F3}, {ahrs.GbZ:F3}");
            Console.WriteLine($"Counter: {ahrs.Counter:F0}");
            Console.WriteLine("-------------------------------");
        }

        static INSGPSFrame ParseINSGPS(byte[] payloadPlusEnd)
        {
            INSGPSFrame gps = new INSGPSFrame();

            gps.Lat = BitConverter.ToDouble(payloadPlusEnd, 0);
            gps.Lon = BitConverter.ToDouble(payloadPlusEnd, 8);
            gps.Alt = BitConverter.ToDouble(payloadPlusEnd, 16);

            gps.Vx = BitConverter.ToSingle(payloadPlusEnd, 24);
            gps.Vy = BitConverter.ToSingle(payloadPlusEnd, 28);
            gps.Vz = BitConverter.ToSingle(payloadPlusEnd, 32);

            gps.Roll = BitConverter.ToSingle(payloadPlusEnd, 36);
            gps.Pitch = BitConverter.ToSingle(payloadPlusEnd, 40);
            gps.Yaw = BitConverter.ToSingle(payloadPlusEnd, 44);

            gps.Status = BitConverter.ToInt32(payloadPlusEnd, 48);
            gps.Counter = BitConverter.ToSingle(payloadPlusEnd, 52);

            return gps;
        }

        static void PrintINSGPS(INSGPSFrame gps)
        {
            Console.WriteLine("=== INSGPS Data ===");
            Console.WriteLine($"Lat={gps.Lat:F6}, Lon={gps.Lon:F6}, Alt={gps.Alt:F2}");
            Console.WriteLine($"Vel: {gps.Vx:F3}, {gps.Vy:F3}, {gps.Vz:F3}");
            Console.WriteLine($"Euler: R={gps.Roll:F3}, P={gps.Pitch:F3}, Y={gps.Yaw:F3}");
            Console.WriteLine($"Status={gps.Status}, Counter={gps.Counter:F0}");
            Console.WriteLine("-------------------------------");
        }

        // ===== CRC tables & funcs (port từ C++ của bạn) =====
        static readonly byte[] CRC8Table = new byte[] {
            0,94,188,226,97,63,221,131,194,156,126,32,163,253,31,65,
            157,195,33,127,252,162,64,30,95,1,227,189,62,96,130,220,
            35,125,159,193,66,28,254,160,225,191,93,3,128,222,60,98,
            190,224,2,92,223,129,99,61,124,34,192,158,29,67,161,255,
            70,24,250,164,39,121,155,197,132,218,56,102,229,187,89,7,
            219,133,103,57,186,228,6,88,25,71,165,251,120,38,196,154,
            101,59,217,135,4,90,184,230,167,249,27,69,198,152,122,36,
            248,166,68,26,153,199,37,123,58,100,134,216,91,5,231,185,
            140,210,48,110,237,179,81,15,78,16,242,172,47,113,147,205,
            17,79,173,243,112,46,204,146,211,141,111,49,178,236,14,80,
            175,241,19,77,206,144,114,44,109,51,209,143,12,82,176,238,
            50,108,142,208,83,13,239,177,240,174,76,18,145,207,45,115,
            202,148,118,40,171,245,23,73,8,86,180,234,105,55,213,139,
            87,9,235,181,54,104,138,212,149,203,41,119,244,170,72,22,
            233,183,85,11,136,214,52,106,43,117,151,201,74,20,246,168,
            116,42,200,150,21,75,169,247,182,232,10,84,215,137,107,53};

        static readonly ushort[] CRC16Table = new ushort[] {
            0x0000,0x1021,0x2042,0x3063,0x4084,0x50A5,0x60C6,0x70E7,
            0x8108,0x9129,0xA14A,0xB16B,0xC18C,0xD1AD,0xE1CE,0xF1EF,
            0x1231,0x0210,0x3273,0x2252,0x52B5,0x4294,0x72F7,0x62D6,
            0x9339,0x8318,0xB37B,0xA35A,0xD3BD,0xC39C,0xF3FF,0xE3DE,
            0x2462,0x3443,0x0420,0x1401,0x64E6,0x74C7,0x44A4,0x5485,
            0xA56A,0xB54B,0x8528,0x9509,0xE5EE,0xF5CF,0xC5AC,0xD58D,
            0x3653,0x2672,0x1611,0x0630,0x76D7,0x66F6,0x5695,0x46B4,
            0xB75B,0xA77A,0x9719,0x8738,0xF7DF,0xE7FE,0xD79D,0xC7BC,
            0x48C4,0x58E5,0x6886,0x78A7,0x0840,0x1861,0x2802,0x3823,
            0xC9CC,0xD9ED,0xE98E,0xF9AF,0x8948,0x9969,0xA90A,0xB92B,
            0x5AF5,0x4AD4,0x7AB7,0x6A96,0x1A71,0x0A50,0x3A33,0x2A12,
            0xDBFD,0xCBDC,0xFBBF,0xEB9E,0x9B79,0x8B58,0xBB3B,0xAB1A,
            0x6CA6,0x7C87,0x4CE4,0x5CC5,0x2C22,0x3C03,0x0C60,0x1C41,
            0xEDAE,0xFD8F,0xCDEC,0xDDCD,0xAD2A,0xBD0B,0x8D68,0x9D49,
            0x7E97,0x6EB6,0x5ED5,0x4EF4,0x3E13,0x2E32,0x1E51,0x0E70,
            0xFF9F,0xEFBE,0xDFDD,0xCFFC,0xBF1B,0xAF3A,0x9F59,0x8F78,
            0x9188,0x81A9,0xB1CA,0xA1EB,0xD10C,0xC12D,0xF14E,0xE16F,
            0x1080,0x00A1,0x30C2,0x20E3,0x5004,0x4025,0x7046,0x6067,
            0x83B9,0x9398,0xA3FB,0xB3DA,0xC33D,0xD31C,0xE37F,0xF35E,
            0x02B1,0x1290,0x22F3,0x32D2,0x4235,0x5214,0x6277,0x7256,
            0xB5EA,0xA5CB,0x95A8,0x8589,0xF56E,0xE54F,0xD52C,0xC50D,
            0x34E2,0x24C3,0x14A0,0x0481,0x7466,0x6447,0x5424,0x4405,
            0xA7DB,0xB7FA,0x8799,0x97B8,0xE75F,0xF77E,0xC71D,0xD73C,
            0x26D3,0x36F2,0x0691,0x16B0,0x6657,0x7676,0x4615,0x5634,
            0xD94C,0xC96D,0xF90E,0xE92F,0x99C8,0x89E9,0xB98A,0xA9AB,
            0x5844,0x4865,0x7806,0x6827,0x18C0,0x08E1,0x3882,0x28A3,
            0xCB7D,0xDB5C,0xEB3F,0xFB1E,0x8BF9,0x9BD8,0xABBB,0xBB9A,
            0x4A75,0x5A54,0x6A37,0x7A16,0x0AF1,0x1AD0,0x2AB3,0x3A92,
            0xFD2E,0xED0F,0xDD6C,0xCD4D,0xBDAA,0xAD8B,0x9DE8,0x8DC9,
            0x7C26,0x6C07,0x5C64,0x4C45,0x3CA2,0x2C83,0x1CE0,0x0CC1,
            0xEF1F,0xFF3E,0xCF5D,0xDF7C,0xAF9B,0xBFBA,0x8FD9,0x9FF8,
            0x6E17,0x7E36,0x4E55,0x5E74,0x2E93,0x3EB2,0x0ED1,0x1EF0
        };

        static byte CRC8_Table(byte[] p, int count)
        {
            byte crc8 = 0;
            for (int i = 0; i < count; i++)
            {
                crc8 = CRC8Table[crc8 ^ p[i]];
            }
            return crc8;
        }

        static ushort CRC16_Table(byte[] p, int count)
        {
            ushort crc16 = 0;
            for (int i = 0; i < count; i++)
            {
                crc16 = (ushort)(CRC16Table[((crc16 >> 8) ^ p[i]) & 0xff] ^ (crc16 << 8));
            }
            return crc16;
        }
    }
}


//using ConsoleApp1;
//using System.IO.Ports;
//using System.Text;

//namespace ImuReader
//{
//    internal class Program
//    {
//        const byte FRAME_HEAD = 0xFC;
//        const byte FRAME_END = 0xFD;

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

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

//        static bool if_debug_;
//        static bool frist_sn_;
//        static byte read_sn_;
//        static int sn_lost_;

//        static void Main(string[] args)
//        {
//            Console.OutputEncoding = Encoding.UTF8;

//            SerialPort serial = new SerialPort()
//            {
//                PortName = "COM5",
//                BaudRate = 921600,
//                ReadTimeout = 2000,
//                Parity = Parity.None,
//                StopBits = StopBits.One,
//                DataBits = 8,
//            };

//            serial.Open();
//            Console.WriteLine("[INFO] Serial port opened.");

//            while (serial.IsOpen)
//            {
//                byte head = (byte)serial.ReadByte();
//                if (head != FRAME_HEAD) continue;

//                byte datatype = (byte)serial.ReadByte();
//                byte length = (byte)serial.ReadByte();

//                if (datatype != TYPE_IMU && datatype != TYPE_AHRS && datatype != TYPE_INSGPS && datatype != TYPE_GROUND)
//                    continue;

//                // Bỏ qua ground packet
//                if (datatype == TYPE_GROUND || datatype == 0x50)
//                {
//                    byte[] ground_ignore = new byte[length + 4];
//                    serial.Read(ground_ignore, 0, ground_ignore.Length);
//                    continue;
//                }

//                byte sn_s = (byte)serial.ReadByte();
//                byte crc8_s = (byte)serial.ReadByte();
//                byte crc16_H_s = (byte)serial.ReadByte();
//                byte crc16_L_s = (byte)serial.ReadByte();

//                // Đọc payload + FRAME_END
//                byte[] frame_data = new byte[length + 1];
//                int read_count = serial.Read(frame_data, 0, frame_data.Length);

//                if (frame_data[length] != FRAME_END)
//                {
//                    Console.WriteLine($"[WARN] frame_end error, type=0x{datatype:X2}");
//                    continue;
//                }

//                // In dữ liệu thô ra hex
//                byte[] full_frame = new byte[7 + frame_data.Length];
//                full_frame[0] = head;
//                full_frame[1] = datatype;
//                full_frame[2] = length;
//                full_frame[3] = sn_s;
//                full_frame[4] = crc8_s;
//                full_frame[5] = crc16_H_s;
//                full_frame[6] = crc16_L_s;
//                Buffer.BlockCopy(frame_data, 0, full_frame, 7, frame_data.Length);

//                Console.WriteLine($"[RAW] {BitConverter.ToString(full_frame)}");
//            }
//        }
//    }
//}