#define _USE_MATH_DEFINES
#include <stdio.h>
#include <math.h>

// ===================== 各类椭球参数 =====================
#define WGS84_A     6378137.0
#define WGS84_FINV  298.257223563

#define CGCS2000_A  6378137.0
#define CGCS2000_FINV 298.257222101

#define XA80_A      6378140.0
#define XA80_FINV   298.257

#define BJ54_A      6378245.0
#define BJ54_FINV   298.3

#define GRS80_A     6378137.0
#define GRS80_FINV  298.257222101

#define WGS72_A     6378135.0
#define WGS72_FINV  298.26

/**
 * LLH(纬度°，经度°，大地高m) → XYZ地固直角坐标
 * a:椭球长半轴 finv:1/f
 */
static void llh2xyz(double lat_deg, double lon_deg, double h,
                    double a, double finv,
                    double *x, double *y, double *z)
{
    double f = 1.0 / finv;
    double e2 = 2.0 * f - f * f;

    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 = a / sqrt(1.0 - e2 * sinB * sinB);
    *x = (N + h) * cosB * cosL;
    *y = (N + h) * cosB * sinL;
    *z = (N * (1.0 - e2) + h) * sinB;
}

/**
 * XYZ → LLH(纬度°，经度°，大地高m)
 * a:椭球长半轴 finv:1/f
 */
static void xyz2llh(double X, double Y, double Z,
                     double a, double finv,
                     double *lat_deg, double *lon_deg, double *h)
{
    double f = 1.0 / finv;
    double e2 = 2.0 * f - f * f;

    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 = a / sqrt(1.0 - e2 * sinB * sinB);
        double tanB_new = (Z + N * 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 = a / sqrt(1.0 - e2 * sinB * sinB);
    *h = r / cos(B_rad) - N;
}

// 批量对比输出单个椭球结果
void calc_one_ellipsoid(const char *name, double a, double finv, double X0, double Y0, double Z0)
{
    double lat, lon, H;
    xyz2llh(X0, Y0, Z0, a, finv, &lat, &lon, &H);

    double X1, Y1, Z1;
    llh2xyz(lat, lon, H, a, finv, &X1, &Y1, &Z1);

    printf("==================== %s ====================\n", name);
    printf("A=%.1f 1/f=%.8f\n", a, finv);
    printf("lat=%.8f ° lon=%.8f ° H=%.4f m\n", lat, lon, H);
    printf("闭环差值 X=%.6f Y=%.6f Z=%.6f\n\n", X1-X0, Y1-Y0, Z1-Z0);
}

int main(void)
{
    // 你官方给出的XYZ坐标
    // double X0 = -2003444.2579;
    // double Y0 = 5411900.5609;
    // double Z0 =  2707528.6741;
    //-2005215.8123  5411960.1839  2706121.1543
    double X0 = -2005215.8123;
    double Y0 = 5411960.1839;
    double Z0 =  2706121.1543;

    // 依次计算所有椭球，方便对比差异
    calc_one_ellipsoid("WGS84",      WGS84_A,    WGS84_FINV,    X0,Y0,Z0);
    calc_one_ellipsoid("CGCS2000",   CGCS2000_A, CGCS2000_FINV, X0,Y0,Z0);
    calc_one_ellipsoid("GRS80",      GRS80_A,    GRS80_FINV,    X0,Y0,Z0);
    calc_one_ellipsoid("西安80 XA80",XA80_A,     XA80_FINV,     X0,Y0,Z0);
    calc_one_ellipsoid("北京54 BJ54",BJ54_A,     BJ54_FINV,     X0,Y0,Z0);
    calc_one_ellipsoid("WGS72",      WGS72_A,    WGS72_FINV,    X0,Y0,Z0);

    return 0;
}