#define _USE_MATH_DEFINES
#include <stdio.h>
#include <math.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)

// BLH(度) → 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 → BLH(度)
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;
}

int main(void)
{
    // 官方给出XYZ
    // double X0 = -2003490.2688;
    // double Y0 = 5411732.0806;
    // double Z0 =  2707878.1432;

    //-2003444.2579  5411900.5609  2707528.6741
    double X0 = -2003444.2579;
    double Y0 = 5411900.5609;
    double Z0 =  2707528.6741;
    
    double lat, lon, H;
    // 第一步：XYZ反推BLH
    xyz2llh(X0, Y0, Z0, &lat, &lon, &H);

    printf("==== 1. XYZ反算 BLH ====\n");
    printf("输入 X = %.4f\n", X0);
    printf("输入 Y = %.4f\n", Y0);
    printf("输入 Z = %.4f\n", Z0);
    printf("------------------------\n");
    printf("纬度 lat = %.8f °\n", lat);
    printf("经度 lon = %.8f °\n", lon);
    printf("大地高 H = %.4f m\n", H);

    // 第二步：用反算出的BLH正向转回XYZ，校验闭环
    double X1, Y1, Z1;
    llh2xyz(lat, lon, H, &X1, &Y1, &Z1);

    printf("\n==== 2. BLH正向转回XYZ 校验 ====\n");
    printf("X还原 = %.4f  差值: %.6f\n", X1, X1 - X0);
    printf("Y还原 = %.4f  差值: %.6f\n", Y1, Y1 - Y0);
    printf("Z还原 = %.4f  差值: %.6f\n", Z1, Z1 - Z0);

    return 0;
}