#define _USE_MATH_DEFINES
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <math.h>
#include <stdint.h>
#include <stdbool.h>

#pragma pack(push, 1)
typedef struct {
    uint8_t Sync[3];
    uint8_t HeaderLength;
    uint16_t MessageID;
    uint8_t MessageType;
    uint8_t Reserved1;
    uint16_t MessageLength;
    uint16_t Reserved2;
    uint8_t IdleTime;
    uint8_t TimeStatus;
    uint16_t GpsWeek;
    uint32_t GpsMs;
    uint32_t Reserved3;
    uint16_t BdsGpsTimeOffset;
    uint16_t Reserved4;
} LogFramePrefix;
#pragma pack(pop)

static uint16_t le16(const uint8_t *p)
{
    return (uint16_t)p[0] | ((uint16_t)p[1] << 8);
}
static uint32_t le32(const uint8_t *p)
{
    return (uint32_t)p[0] | ((uint32_t)p[1] << 8)
           | ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
}

int parseLogFrame(const uint8_t *buf, int buf_len, LogFramePrefix *out, const uint8_t **msg_body_out)
{
    const int HDR_TOTAL = sizeof(LogFramePrefix);
    if (buf_len < HDR_TOTAL)
    {
        printf("[ERR] 帧数据长度不足31字节\n");
        return -1;
    }
    memcpy(out, buf, HDR_TOTAL);
    if (out->Sync[0] != 0xAA || out->Sync[1] != 0x44 || out->Sync[2] != 0x12)
    {
        printf("[ERR] 同步头AA4412不匹配\n");
        return -2;
    }
    uint8_t *raw = (uint8_t *)buf;
    out->MessageID        = le16(raw + 4);
    out->MessageLength    = le16(raw + 8);
    out->Reserved2        = le16(raw + 10);
    out->GpsWeek          = le16(raw + 14);
    out->GpsMs            = le32(raw + 16);
    out->Reserved3        = le32(raw + 20);
    out->BdsGpsTimeOffset = le16(raw + 24);
    out->Reserved4        = le16(raw + 26);
    *msg_body_out = buf + HDR_TOTAL;
    return 0;
}

void printFrameInfo(const LogFramePrefix *hdr, int body_len)
{
    printf("==================== 外层帧头部解析 ====================\n");
    printf("同步 Sync: 0x%02X,0x%02X,0x%02X\n", hdr->Sync[0], hdr->Sync[1], hdr->Sync[2]);
    printf("HeaderLength: 0x%02X (%d 字节头)\n", hdr->HeaderLength, hdr->HeaderLength);
    printf("MessageID: %u (0x%04X)\n", hdr->MessageID);
    const char *typeStr;
    if (hdr->MessageType == 0) typeStr = "二进制RANGECMP2";
    else if (hdr->MessageType == 1) typeStr = "ASCII";
    else typeStr = "简化ASCII";
    printf("MessageType: %u (%s)\n", hdr->MessageType, typeStr);
    printf("Reserved1: 0x%02X\n", hdr->Reserved1);
    printf("MessageLength: %u 字节(消息体总长)\n", hdr->MessageLength);
    printf("Reserved2: 0x%04X\n", hdr->Reserved2);
    printf("IdleTime: %d (空闲占比 %.1f%%)\n", hdr->IdleTime, hdr->IdleTime / 2.0);
    const char *timeState;
    if (hdr->TimeStatus == 160) timeState = "GPS时间精准";
    else if (hdr->TimeStatus == 20) timeState = "时间未知";
    else timeState = "未知状态";
    printf("TimeStatus: %d (%s)\n", hdr->TimeStatus, timeState);
    printf("GPS Week: %u 周\n", hdr->GpsWeek);
    printf("GPS Ms(本周毫秒): %u\n", hdr->GpsMs);
    printf("Reserved3: 0x%08X\n", hdr->Reserved3);
    printf("BDS-GPS时差(ms): %d\n", hdr->BdsGpsTimeOffset);
    printf("Reserved4: 0x%04X\n", hdr->Reserved4);
    printf("消息体实际可用长度: %d 字节\n", body_len);
    printf("========================================================\n\n");
}

// ===================== RANGECMP2 解析 =====================
static const double g_std_psr_table[16] = {
    0.02, 0.03, 0.045, 0.066, 0.099, 0.148, 0.22, 0.329,
    0.491, 0.732, 1.092, 1.629, 2.43, 3.625, 5.409, 999.0
};
static const double g_std_adr_table[16] = {
    0.00391, 0.00521, 0.00696, 0.00929, 0.01239, 0.01654, 0.02208, 0.02947,
    0.03933, 0.05249, 0.07006, 0.09350, 0.12480, 0.16656, 0.22230, 999.0
};
typedef struct { double num; double den; } DopplerScaleFactor;
static const DopplerScaleFactor g_doppler_scale[8][6] = {
    {{1.0,1.0},{154.0,120.0},{154.0,120.0},{154.0,115.0},{0,0},{0,0}},
    {{1.0,1.0},{9.0,7.0},{9.0,7.0},{0,0},{0,0},{0,0}},
    {{1.0,1.0},{154.0,115.0},{0,0},{0,0},{0,0},{0,0}},
    {{1.0,1.0},{154.0,115.0},{154.0,118.0},{154.0,116.5},{154.0,125.0},{154.0,125.0}},
    {{1.0,1.0},{154.0,120.0},{154.0,115.0},{154.0,125.0},{0,0},{0,0}},
    {{1.0,1.0},{0,0},{0,0},{0,0},{0,0},{0,0}},
    {{1526.0,1540.0},{1526.0,1180.0},{1526.0,1150.0},{1526.0,1240.0},{0,0},{0,0}},
    {{1.0,1.0},{0,0},{0,0},{0,0},{0,0},{0,0}}
};
typedef enum {
    SYS_GPS=0,SYS_GLO=1,SYS_SBAS=2,SYS_GAL=3,SYS_QZSS=4,SYS_LBAND=5,SYS_BDS=6,SYS_NAVIC=7
} SatSystemID;
typedef struct {
    uint8_t signal_type;        // 5bit编码，表9-63信号类型值：1=L1CA、4=L2Y、7=L5Q等
    bool phase_lock;            // 载波相位锁定标志 1=锁相，0失锁
    bool parity_known;          // 相位整周模糊度/奇偶校验已知标志
    bool code_lock;             // 伪距码跟踪锁定标志
    uint32_t locktime_ms;       // 该频点连续锁定时长，单位毫秒（从卫星头17bit锁定时间换算）
    uint8_t corr_type;          // 内部相关器类型，协议预留字段
    bool is_primary;            // 是否为主跟踪信号（基准频点）
    bool half_cycle_added;      // 是否补偿半周相位模糊
    uint8_t cno;                // 载噪比C/N0，原始5bit值 20~51 dB-Hz
    double std_psr;             // 伪距观测噪声标准差，查表9-60得到(m)，索引15时=999.0代表超限坏值
    double std_adr;             // 载波相位噪声标准差，查表9-61得到(m)，索引15时=999.0代表超限坏值
    double psr;                 // 最终换算完成的真实伪距(m) = psr_base + psr_diff/128
    double adr;                 // 最终真实载波相位(周) = psr_base + adr_diff/2048
    double doppler;             // 最终真实多普勒(Hz) = (doppler_base + dop_diff/256) * 缩放系数(表9-62)
} RangeSignalBlock;

typedef struct {
    uint8_t sv_chan;            // 接收机内部跟踪通道号
    uint8_t sv_id;              // 卫星PRN编号：G02→2、C14→14、E07→7
    uint8_t glo_ch_offset;      // GLONASS专用：频点偏移号，GPS/BDS/Galileo填0
    SatSystemID sys_id;         // 卫星系统枚举：0=GPS,1=GLO,2=SBAS,3=GAL,4=QZSS,6=BDS,7=NAVIC
    double psr_base;            // 卫星级29bit伪距基准值(m)，所有频点共用
    double doppler_base;        // 卫星级21bit多普勒基准值，所有频点共用
    uint8_t sig_block_cnt;      // 该卫星有效频点数量（4bit，1~6），控制循环读取几个SignalBlock
    RangeSignalBlock sigs[16];  // 存储该卫星全部频点观测，最多预留16路，实际只用sig_block_cnt个
} RangeSatRecord;
typedef struct {
    uint32_t data_byte_len;     // RANGECMP2负载数据总字节长度（二进制有效载荷长度）
    uint32_t crc32;             // 负载CRC32校验值，用于校验二进制帧是否损坏
    uint8_t sat_count;          // 本帧内总跟踪卫星数量（对应RINEX历元头数字，示例里是16）
    RangeSatRecord sats[240];   // 存储全部卫星数据，最多支持240颗卫星，实际有效sat_count颗
} RangeCmp2Result;

typedef struct {
    const uint8_t *buf;
    uint32_t byte_len;
    uint32_t bit_pos;
} BitStream;
static void bitstream_init(BitStream *bs, const uint8_t *buf, uint32_t len)
{
    bs->buf=buf; bs->byte_len=len; bs->bit_pos=0;
}
static uint64_t bitstream_read_bits(BitStream *bs, uint8_t n)
{
    uint64_t val=0;uint32_t i=0;
    while(i<n){
        uint32_t b_idx=bs->bit_pos/8;
        uint32_t b_off=bs->bit_pos%8;
        if(b_idx>=bs->byte_len) return 0;
        uint8_t bit=(bs->buf[b_idx]>>b_off)&1;
        val|=((uint64_t)bit)<<i;
        bs->bit_pos++;i++;
    }
    return val;
}
static int64_t bitstream_read_sbits(BitStream *bs, uint8_t n)
{
    uint64_t raw=bitstream_read_bits(bs,n);
    uint64_t mask=1ULL<<(n-1);
    if(raw&mask) return (int64_t)(raw|(~((1ULL<<n)-1)));
    return (int64_t)raw;
}
static int parse_single_sat_record(BitStream *bs, RangeSatRecord *out)
{
    memset(out,0,sizeof(RangeSatRecord));
    out->sv_chan = bitstream_read_bits(bs,8);
    out->sv_id = bitstream_read_bits(bs,8);
    out->glo_ch_offset = bitstream_read_bits(bs,4);
    out->sys_id = (SatSystemID)bitstream_read_bits(bs,5);
    bitstream_read_bits(bs,1);
    uint64_t psr_base_raw = bitstream_read_bits(bs,29);
    out->psr_base = (double)psr_base_raw;
    int64_t dop_base_raw = bitstream_read_sbits(bs,21);
    out->doppler_base = (double)dop_base_raw;
    out->sig_block_cnt = bitstream_read_bits(bs,4);
    if(out->sig_block_cnt>16) return -1;
    for(uint8_t i=0;i<out->sig_block_cnt;i++){
        RangeSignalBlock *sig=&out->sigs[i];
        memset(sig,0,sizeof(RangeSignalBlock));
        sig->signal_type = bitstream_read_bits(bs,5);
        sig->phase_lock = bitstream_read_bits(bs,1)!=0;
        sig->parity_known = bitstream_read_bits(bs,1)!=0;
        sig->code_lock = bitstream_read_bits(bs,1)!=0;
        sig->locktime_ms = bitstream_read_bits(bs,17);
        sig->corr_type = bitstream_read_bits(bs,4);
        sig->is_primary = bitstream_read_bits(bs,1)!=0;
        sig->half_cycle_added = bitstream_read_bits(bs,1)!=0;
        bitstream_read_bits(bs,1);
        uint8_t cno_raw = bitstream_read_bits(bs,5);
        sig->cno = 20 + cno_raw;
        uint8_t idx_psr = bitstream_read_bits(bs,4);
        sig->std_psr = g_std_psr_table[idx_psr];
        uint8_t idx_adr = bitstream_read_bits(bs,4);
        sig->std_adr = g_std_adr_table[idx_adr];
        uint16_t diff_psr = bitstream_read_bits(bs,14);
        sig->psr = out->psr_base + (double)diff_psr / 128.0;
        uint32_t diff_adr = bitstream_read_bits(bs,20);
        sig->adr = out->psr_base + (double)diff_adr / 2048.0;
        int16_t diff_dop = bitstream_read_sbits(bs,17);
        double dop_diff = (double)diff_dop / 256.0;
        const DopplerScaleFactor *sf = &g_doppler_scale[out->sys_id][sig->signal_type];
        double scale = sf->num / sf->den;
        sig->doppler = (out->doppler_base + dop_diff) * scale;
    }
    return 0;
}


int parse_range_cmp2(const uint8_t *msg_body, int body_len, RangeCmp2Result *out)
{
    memset(out,0,sizeof(RangeCmp2Result));
    if(body_len < 8) return -1;
    out->data_byte_len = body_len - 2;
    //if(out->data_byte_len > (uint32_t)(body_len-4)) return -2;
    const uint8_t *range_data = msg_body;
    const uint8_t *crc_buf = range_data + out->data_byte_len;
   // if((crc_buf+4) > (msg_body+body_len)) return -3;
    out->crc32 = le32(crc_buf);
    BitStream bs;
    bitstream_init(&bs, range_data, out->data_byte_len);
    while(bs.bit_pos < out->data_byte_len*8){
        if(out->sat_count>=240) break;
        int ret = parse_single_sat_record(&bs, &out->sats[out->sat_count]);
        if(ret!=0) break;
        out->sat_count++;
    }
    return 0;
}

// ===================== 关键修复：和你截图一模一样的RINEX打印 =====================
void print_rinex_obs(const RangeCmp2Result *cmp)
{
    // 固定RINEX3表头（和你给的文本完全一致）
    puts("530        0.0000        0.0000                  ANTENNA: DELTA H/E/N");
    puts("G   12 C1C L1C D1C S1C C2C L2C D2C S2C C5Q L5Q D5Q S5Q      SYS / # / OBS TYPES");
    puts("R    8 C1C L1C D1C S1C C2P L2P D2P S2P                      SYS / # / OBS TYPES");
    puts("C    8 C1I L1I D1I S1I C7I L7I D7I                      SYS / # / OBS TYPES");
    puts("S    4 C1C L1C D1C S1C                                      SYS / # / OBS TYPES");
    puts("E   12 C1C L1C D1C S1C C5Q L5Q D5Q S5Q C7Q L7Q D7Q S7Q      SYS / # / OBS TYPES");
    puts("DBHZ");
    puts("");
    // 历元时间（示例固定，可替换GPS周毫秒换算）
    printf("> 2026 06 11 12 10 58.0000000  0 %d       0.000000000000\n\n", cmp->sat_count);

    const char sys_char[]={'G','R','S','E','Q','X','C','N'};
    for(uint8_t i=0;i<cmp->sat_count;i++)
    {
        const RangeSatRecord *sat=&cmp->sats[i];
        // 打印卫星PRN开头
        printf("%c%02d", sys_char[sat->sys_id], sat->sv_id);
        // 逐信号输出，多信号自动接续，过长换行对齐原版
        for(uint8_t s=0;s<sat->sig_block_cnt;s++)
        {
            const RangeSignalBlock *sig=&sat->sigs[s];
            // 格式：伪距 载波 多普勒 信噪比，和你样例小数位匹配
            printf("  %.3f %d %.3f          %.0f", sig->psr, sig->locktime_ms/1000, sig->adr, sig->cno);
        }
        puts("");
    }
   // puts("\n> 2026 06 1");
}

int main(void)
{
    // 你提供的完整测试二进制帧
    uint8_t hex_sta[] = {
        0xAA,0x44,0x12,0x1C,0xF9,0x04,0x00,0x01,0x24,0x02,0xC0,0x34,
        0x59,0xA0,0x76,0x09,0x50,0xA8,0x36,0x17,0xEC,0xEC,0x19,0x00,0x00,0x00,0x12,0x00,
   
    0x20,0x02,0x00,0x00,0x01,0x1F,0x00,0xE8,0x8E,0x67,
    0x05,0x00,0x05,0x20,0xE1,0x8A,0x3E,0x00,0x88,0xEB,
    0x35,0x30,0x0C,0x00,0x5C,0x00,0x84,0x00,0x00,0x00,
    0xE5,0xF9,0x29,0x19,0x03,0x05,0x06,0x00,0x03,0x1C,
    0x00,0x10,0xEC,0x06,0x85,0xDA,0x01,0x30,0xE1,0xFF,
    0xFF,0x01,0xAD,0x69,0x5B,0xF8,0x19,0x00,0x6A,0x00,
    0xE4,0xFF,0xFF,0x01,0xAD,0xE9,0xD8,0x00,0xC0,0x82,
    0x94,0x00,0xE7,0xFF,0xFF,0x01,0xB6,0x44,0xB6,0xB8,
    0x1F,0x05,0x7F,0x00,0x08,0x02,0x00,0x4C,0xC8,0xEB,
    0x05,0xA5,0x00,0x10,0xE1,0xFF,0xFF,0x01,0x6E,0xE9,
    0x32,0xD0,0x36,0x00,0x54,0x00,0x12,0x90,0x20,0x94,
    0x01,0xDA,0x88,0xF3,0xFF,0x1F,0xE1,0xFF,0xFF,0x21,
    0x8E,0xA9,0x08,0xC8,0x98,0x00,0x83,0xFF,0x15,0x80,
    0x20,0x6C,0xFA,0xD8,0x88,0xFF,0xFF,0x1F,0xE1,0xFF,
    0xFF,0x21,0x2E,0x8B,0x06,0x60,0x2E,0x81,0xC5,0xFF,
    0x02,0x0F,0x17,0xF0,0x58,0x5A,0x85,0x3D,0x08,0x20,
    0xE1,0x56,0x36,0x00,0xA9,0x8B,0x66,0x10,0x1A,0x00,
    0x77,0x00,0x84,0x00,0x00,0x00,0x85,0x39,0x22,0x91,
    0x73,0x84,0xD7,0xFF,0x03,0x13,0x1A,0xA8,0x30,0x7E,
    0x85,0xFF,0xFC,0x2F,0xE1,0x4A,0x19,0x01,0xAA,0x6B,
    0xFC,0x28,0x32,0x00,0xE8,0xFF,0xE4,0xFF,0xFF,0x01,
    0x30,0x47,0x7B,0xF1,0xA8,0x83,0x1B,0x00,0x0C,0x12,
    0x14,0xD8,0x99,0x9D,0x85,0xF0,0xF9,0x2F,0xE1,0xFF,
    0xFF,0x01,0xF0,0xA7,0xAD,0x50,0x25,0x00,0xCB,0xFF,
    0xE4,0xFF,0xFF,0x01,0x2F,0xC9,0xCB,0x29,0x11,0x09,
    0xBB,0xFF,0x05,0x02,0x60,0xAC,0x01,0xDA,0x88,0xF3,
    0xFF,0x1F,0xE2,0xFF,0xFF,0x01,0x71,0xE7,0x0D,0xE0,
    0xA1,0x80,0xB5,0xFF,0x0B,0x0E,0x60,0x20,0xA7,0xE1,
    0x85,0xC0,0x04,0x10,0x81,0x00,0x00,0x00,0xA3,0xD9,
    0x04,0x98,0x07,0x80,0x7B,0x00,0x0D,0x1A,0x60,0xFC,
    0x0E,0xC7,0x05,0x3B,0x04,0x10,0xF4,0xFF,0xFF,0x01,
    0xAA,0xCB,0x00,0x88,0x72,0x02,0xA3,0x62,0x17,0x15,
    0x60,0x04,0xAE,0x85,0x05,0xB2,0x03,0x30,0x81,0x00,
    0x00,0x00,0xE5,0x59,0x7C,0x80,0x3D,0x02,0x11,0x00,
    0xED,0xBA,0x1D,0x00,0x28,0x6D,0x09,0x70,0x2B,0x00,
    0x3C,0x00,0xF4,0xFF,0xFF,0x01,0x8C,0xEB,0x25,0xE0,
    0x99,0x00,0xEC,0xFF,0x01,0x0F,0x50,0x54,0xBC,0xFD,
    0x85,0xA3,0xFB,0x3F,0xE1,0xFF,0xFF,0x01,0x8A,0x8B,
    0x96,0x98,0x22,0x80,0xC7,0xFF,0xE2,0xFF,0xFF,0x01,
    0x4D,0x49,0xF2,0xC8,0xE2,0x03,0xD3,0xFF,0xE3,0xFF,
    0xFF,0x01,0x2C,0x49,0xE3,0xC8,0xBA,0x82,0xF9,0xFF,
    0x06,0x07,0x50,0xB4,0xFB,0xA7,0x06,0x9C,0x02,0x30,
    0xE1,0x62,0x02,0x00,0x65,0x6D,0x1E,0x08,0x0F,0x80,
    0x74,0x00,0xE2,0xFF,0xFF,0x01,0x6B,0xCB,0xDA,0x28,
    0xE2,0x84,0x3E,0x00,0xE3,0xFF,0xFF,0x01,0xF2,0xE6,
    0xD0,0xB8,0xED,0x03,0x48,0x00,0x07,0x0D,0x50,0x3C,
    0x84,0x17,0x06,0x1F,0x00,0x30,0xE1,0x8E,0x03,0x00,
    0x68,0x8D,0x06,0x48,0x89,0x00,0x67,0x00,0xE2,0xFF,
    0xFF,0x01,0x52,0x67,0x53,0x21,0xB9,0x84,0xB3,0x00,
    0xE3,0xFF,0xFF,0x01,0xD2,0x26,0xF2,0x30,0x86,0x86,
    0xAA,0x00,0x09,0x08,0x50,0x3C,0xD0,0xE3,0x05,0x33,
    0x01,0x30,0x81,0x00,0x00,0x00,0x83,0x39,0x07,0xD0,
    0x18,0x80,0x5F,0x00,0xE2,0x72,0xB9,0x01,0x0C,0x89,
    0x41,0x48,0xE0,0x80,0x4E,0x00,0xE3,0x3A,0xEC,0x01,
    0x4D,0xE9,0x13,0x58,0xD4,0x00,0x43,0x00,0x13,0x46,
    0x23,0x45,
};
    int total_len = sizeof(hex_sta);

    LogFramePrefix frame;
    const uint8_t *msgBody = NULL;
    int ret = parseLogFrame(hex_sta, total_len, &frame, &msgBody);
    int body_len = total_len - sizeof(LogFramePrefix);
    if (ret != 0)
    {
        printf("帧头解析失败\n");
        return -1;
    }else{
        printf("帧头解析成功[%02x][%02x][%02x]\n", *msgBody, *(msgBody+body_len-1), *(msgBody + body_len - 2));
    }
    
    // printFrameInfo(&frame, body_len); // 注释掉，只输出RINEX，取消调试帧头打印
    if (frame.MessageID == 0x04F9)
    {
        RangeCmp2Result cmp_res;
        int r = parse_range_cmp2(msgBody, body_len, &cmp_res);
        if (r == 0)
        {
            // 核心：输出和你截图一模一样的RINEX观测文本
            print_rinex_obs(&cmp_res);
        }
        else printf("[ERR] RANGECMP2解析失败 %d\n", r);
    }
    return 0;
}