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

// WGS84 标准椭球参数
#define WGS84_A     6378137.0
#define WGS84_F_INV 298.257223563
#define WGS84_F     (1.0 / WGS84_F_INV)
#define WGS84_E2    (2.0 * WGS84_F - WGS84_F * WGS84_F)

// ==================== 高程修正全局配置（仅定义，不参与计算） ====================
#define ANT_HEIGHT_OFFSET  -1.800
#define GEOID_N            -2.360
#define EXTRA_HEIGHT_DIFF   -14.834
// ==============================================================================

static uint32_t le32_to_cpu(const unsigned char *buf)
{
    return ((uint32_t)buf[3] << 24) | ((uint32_t)buf[2] << 16) |
           ((uint32_t)buf[1] << 8)  | (uint32_t)buf[0];
}

static void print_str_with_hex(const char *str, int len)
{
    int i;
    printf("'");
    for(i = 0; i < len; i++){
        unsigned char c = (unsigned char)str[i];
        if(c >= 32 && c <= 126)
            putchar(c);
        else
            putchar('.');
    }
    printf("' [0x");
    for(i = 0; i < len; i++){
        printf("%02X", (unsigned char)str[i]);
    }
    printf("]");
}

static void print_u8_with_hex(unsigned char val)
{
    printf("%u [0x%02X]", val, val);
}

static void print_u16_with_hex(unsigned short val)
{
    printf("%u [0x%04X]", val, val);
}

static void print_raw4_with_hex(const unsigned char *buf)
{
    printf("[0x%02X%02X%02X%02X]", buf[0], buf[1], buf[2], buf[3]);
}

static void print_double_with_hex(double val)
{
    unsigned char *p = (unsigned char*)&val;
    printf("%.8lf [0x", val);
    printf("%02X%02X%02X%02X%02X%02X%02X%02X",
           p[7], p[6], p[5], p[4], p[3], p[2], p[1], p[0]);
    printf("]");
}

static void print_float_with_hex(float val)
{
    unsigned char *p = (unsigned char*)&val;
    printf("%.4f [0x%02X%02X%02X%02X]", val, p[3], p[2], p[1], p[0]);
}

// LLH(度, m) → XYZ
static void llh2xyz(double lat_deg, double lon_deg, double h,
                    double *x, double *y, double *z)
{
    double B = lat_deg * M_PI / 180.0;
    double L = lon_deg * M_PI / 180.0;

    double sinB = sin(B);
    double cosB = cos(B);
    double sinL = sin(L);
    double cosL = cos(L);

    double N = WGS84_A / sqrt(1.0 - WGS84_E2 * sinB * sinB);
    *x = (N + h) * cosB * cosL;
    *y = (N + h) * cosB * sinL;
    *z = (N * (1.0 - WGS84_E2) + h) * sinB;
}

// XYZ → LLH（你要的xyz2llh）
static void xyz2llh(double X, double Y, double Z,
                    double *lat_deg, double *lon_deg, double *h)
{
    double L_rad = atan2(Y, X);
    *lon_deg = L_rad * 180.0 / M_PI;

    double r = sqrt(X*X + Y*Y);
    double B_rad = atan2(Z, r);
    double sinB, N, dB;
    int iter = 0;
    const double eps = 1e-12;

    do
    {
        sinB = sin(B_rad);
        N = WGS84_A / sqrt(1.0 - WGS84_E2 * sinB * sinB);
        double tanB_new = (Z + N * WGS84_E2 * sinB) / r;
        double B_new = atan(tanB_new);
        dB = fabs(B_new - B_rad);
        B_rad = B_new;
        iter++;
    } while (dB > eps && iter < 50);

    *lat_deg = B_rad * 180.0 / M_PI;

    sinB = sin(B_rad);
    N = WGS84_A / sqrt(1.0 - WGS84_E2 * sinB * sinB);
    *h = r / cos(B_rad) - N;
}

#pragma pack(1)
typedef struct RawBinHead512
{
    char head_info[53];
    char reserve0[5];

    unsigned char day;
    unsigned char mon;
    unsigned short year;

    char reserve1[18];

    double lat;
    double lon;
    float  alt;
    char   reserve3[4];

    unsigned char pos_unit;
    unsigned short ant_h_raw;

    char start_end[16];
    unsigned char ant_type_code;
    char reserve2[1];
    char ant_name[20];

    unsigned char ant_meas_mode;
    unsigned char ant_def_en;

    unsigned char edge_R_raw[4];
    unsigned char edge_H_raw[4];
    unsigned char HL1_raw[4];
    unsigned char HL2_raw[4];

    unsigned char freq_mode;
    unsigned char board_type;
    unsigned char rec_mode;

    char soft_ver[15];
    char marker_name[16];
    char marker_no[4];

    unsigned char ant_mask;
    char rec_info[20];
    char ant_sn[20];
    char observer[20];
    char agency[20];
    char marker_type[20];

    short de;
    short dn;
    char reserve_end[206];
} RawBinHead512;
#pragma pack()

typedef char check_size[sizeof(RawBinHead512) == 512 ? 1 : -1];

int ReadBin512(const char *filepath, RawBinHead512 *out)
{
    FILE *fp = fopen(filepath, "rb");
    if (!fp)
    {
        perror("fopen failed");
        return -1;
    }

    memset(out, 0, sizeof(RawBinHead512));
    size_t read_size = fread(out, 1, sizeof(RawBinHead512), fp);
    fclose(fp);

    printf("Struct size: %zu bytes\n", sizeof(RawBinHead512));
    printf("Read bytes: %zu\n\n", read_size);

    if (read_size < 512)
    {
        fprintf(stderr, "WARN: file <512 bytes, incomplete header, all data will be 0!\n");
        return -2;
    }
    return 0;
}

void PrintBinHead(const RawBinHead512 *p)
{
    uint32_t raw_R  = le32_to_cpu(p->edge_R_raw);
    uint32_t raw_H  = le32_to_cpu(p->edge_H_raw);
    uint32_t raw_HL1= le32_to_cpu(p->HL1_raw);
    uint32_t raw_HL2= le32_to_cpu(p->HL2_raw);

    double edge_R_val  = raw_R / 10000.0;
    double edge_H_val  = raw_H / 10000.0;
    double HL1_val     = raw_HL1 / 10000.0;
    double HL2_val     = raw_HL2 / 10000.0;

    double ant_h_mm  = (double)p->ant_h_raw / 1000.0;

    double lat_deg, lon_deg;
    if (p->pos_unit == 0)
    {
        lat_deg = p->lat * 180.0 / M_PI;
        lon_deg = p->lon * 180.0 / M_PI;
    }
    else
    {
        lat_deg = p->lat;
        lon_deg = p->lon;
    }

    double H_ant = (double)p->alt;
    printf("\n------ llh [%.8f][%.8f][%.4f]\n", lat_deg, lon_deg, H_ant);

    // LLH 转 XYZ
    double X, Y, Z;
    llh2xyz(lat_deg, lon_deg, H_ant, &X, &Y, &Z);

    // XYZ 反推 LLH 校验
    double chk_lat, chk_lon, chk_h;
    xyz2llh(X, Y, Z, &chk_lat, &chk_lon, &chk_h);

    printf("==================== 512 BIN HEADER PARSE RESULT ====================\n");
    printf("Head info: "); print_str_with_hex(p->head_info, sizeof(p->head_info)); printf("\n");
    printf("Date: %04u-%02u-%02u [Y:", p->year, p->mon, p->day);
    print_u16_with_hex(p->year); printf(" M:"); print_u8_with_hex(p->mon);
    printf(" D:"); print_u8_with_hex(p->day); printf("]\n\n");

    printf("Coordinate Raw Storage:\n");
    printf("  Lat raw: "); print_double_with_hex(p->lat);
    printf(" (%s)\n", p->pos_unit ? "degree" : "radian");
    printf("  Lon raw: "); print_double_with_hex(p->lon);
    printf(" (%s)\n", p->pos_unit ? "degree" : "radian");
    printf("  Alt raw(float): "); print_float_with_hex(p->alt); printf("\n");
    printf("  Lat convert(deg): %.8f\n", lat_deg);
    printf("  Lon convert(deg): %.8f\n", lon_deg);

    printf("\nElevation Info (No correction macro applied):\n");
    printf("  Antenna phase center WGS84 H: %.3f m\n", H_ant);
    printf("  Antenna height raw: %.3f m (%u mm)\n", ant_h_mm, p->ant_h_raw);

    printf("\nAPPROX POSITION XYZ (Pure llh2xyz, no any ENU offset added):\n");
    printf("  X = %.4f m\n", X);
    printf("  Y = %.4f m\n", Y);
    printf("  Z = %.4f m\n", Z);
    printf("  XYZ: %.4f  %.4f  %.4f\n", X, Y, Z);

    printf("\nXYZ -> LLH Check (for verify):\n");
    printf("  lat=%.8f lon=%.8f h=%.4f\n", chk_lat, chk_lon, chk_h);

    printf("\nAntenna geometry params (stored = float * 10000, little-endian uint32):\n");
    printf("  Edge R: raw=%u  value=%.4f ", raw_R, edge_R_val); print_raw4_with_hex(p->edge_R_raw); printf("\n");
    printf("  Edge H: raw=%u  value=%.4f ", raw_H, edge_H_val); print_raw4_with_hex(p->edge_H_raw); printf("\n");
    printf("  L1 HL1: raw=%u  value=%.4f ", raw_HL1, HL1_val); print_raw4_with_hex(p->HL1_raw); printf("\n");
    printf("  L2 HL2: raw=%u  value=%.4f ", raw_HL2, HL2_val); print_raw4_with_hex(p->HL2_raw); printf("\n");

    printf("Freq mode: %s [", p->freq_mode == 1 ? "双频" : "单频");
    print_u8_with_hex(p->freq_mode);
    printf("]\n");

    const char *board_str[] = {
        "无定义", "OEMV", "OEM4", "DATAGIRD", "HEMISPHERE(双频)",
        "JAVAD", "空", "UBLOX", "CSI(单频)", "BD970"
    };
    printf("GPS主板类型: %d (%s) [", p->board_type,
           p->board_type < 10 ? board_str[p->board_type] : "保留");
    print_u8_with_hex(p->board_type);
    printf("]\n");

    printf("接收机模式: %s [", p->rec_mode == 0 ? "静态/基准站" : "移动站");
    print_u8_with_hex(p->rec_mode);
    printf("]\n");

    printf("采集软件版本: ");
    print_str_with_hex(p->soft_ver, sizeof(p->soft_ver));
    printf("\n");

    printf("点名: ");
    print_str_with_hex(p->marker_name, sizeof(p->marker_name));
    printf("\n");
    printf("测站编号: ");
    print_str_with_hex(p->marker_no, sizeof(p->marker_no));
    printf("\n");

    printf("天线允许测量掩码: 0x%02X [", p->ant_mask);
    print_u8_with_hex(p->ant_mask);
    printf("]\n");
    if (p->ant_mask & 0x01) printf("  - 天线相位中心\n");
    if (p->ant_mask & 0x02) printf("  - 天线测高方式\n");
    if (p->ant_mask & 0x04) printf("  - 天线垂直测高\n");
    if (p->ant_mask & 0x08) printf("  - 测高片\n");
    if (p->ant_mask & 0x10) printf("  - 天线垂直高(底部)\n");

    printf("接收机厂家信息: ");
    print_str_with_hex(p->rec_info, sizeof(p->rec_info));
    printf("\n");
    printf("天线序列号: ");
    print_str_with_hex(p->ant_sn, sizeof(p->ant_sn));
    printf("\n");
    printf("观测者: ");
    print_str_with_hex(p->observer, sizeof(p->observer));
    printf("\n");
    printf("机构: ");
    print_str_with_hex(p->agency, sizeof(p->agency));
    printf("\n");
    printf("测站类型: ");
    print_str_with_hex(p->marker_type, sizeof(p->marker_type));
    printf("\n");

    printf("原始标石水平偏心量(仅打印，不参与XYZ计算): 东向=%d mm, 北向=%d mm\n", p->de, p->dn);

    printf("====================================================================\n");
}

int main(int argc, char *argv[])
{
    if (argc != 2)
    {
        printf("Usage: %s bin_file\n", argv[0]);
        return 1;
    }
    RawBinHead512 hdr;
    int ret = ReadBin512(argv[1], &hdr);
    if (ret != 0)
        return ret;
    PrintBinHead(&hdr);
    return 0;
}