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

// WGS84 椭球常数
#define A 6378137.0          // 长半轴 (米)
#define F 1.0 / 298.257223563 // 扁率
#define E2 (2 * F - F * F)    // 第一偏心率的平方

/**
 * 将经纬度(度)及高程(米)转换为 ECEF XYZ 坐标(米)
 * @param lat   纬度 (度, 北正南负)
 * @param lon   经度 (度, 东正西负)
 * @param h     高程 (米, 椭球高)
 * @param x     输出 X 坐标指针
 * @param y     输出 Y 坐标指针
 * @param z     输出 Z 坐标指针
 */
void latlonh_to_xyz(double lat, double lon, double h, double *x, double *y, double *z) {
    // 角度转弧度
    double rad_lat = lat * M_PI / 180.0;
    double rad_lon = lon * M_PI / 180.0;
    double sin_lat = sin(rad_lat);
    double cos_lat = cos(rad_lat);
    double sin_lon = sin(rad_lon);
    double cos_lon = cos(rad_lon);

    // 卯酉圈曲率半径 N
    double N = A / sqrt(1.0 - E2 * sin_lat * sin_lat);

    // 计算 XYZ
    *x = (N + h) * cos_lat * cos_lon;
    *y = (N + h) * cos_lat * sin_lon;
    *z = (N * (1.0 - E2) + h) * sin_lat;
}
void xyz_to_latlonh(double x, double y, double z, double *lat, double *lon, double *h) {
    *lon = atan2(y, x);
    double p = sqrt(x*x + y*y);
    double lat0 = atan2(z, p * (1.0 - E2));
    double N, lat1;
    do {
        N = A / sqrt(1.0 - E2 * sin(lat0) * sin(lat0));
        lat1 = atan2(z + N * E2 * sin(lat0), p);
        if (fabs(lat1 - lat0) < 1e-12) break;
        lat0 = lat1;
    } while (1);
    *lat = lat1 * 180.0 / M_PI;
    *h = p / cos(lat1) - N;
}





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

#define A 6378137.0
#define F (1.0 / 298.257223563)
#define E2 (2*F - F*F)

void _xyz_to_latlonh(double x, double y, double z, double *lat, double *lon, double *h) {
    *lon = atan2(y, x);  // 经度（弧度）
    double p = sqrt(x*x + y*y);
    double lat0 = atan2(z, p * (1.0 - E2));
    double N, lat1;
    do {
        N = A / sqrt(1.0 - E2 * sin(lat0) * sin(lat0));
        lat1 = atan2(z + N * E2 * sin(lat0), p);
        if (fabs(lat1 - lat0) < 1e-12) break;
        lat0 = lat1;
    } while (1);
    *lat = lat1 * 180.0 / M_PI;
    *h = p / cos(lat1) - N;
}

int main() {
    double x = -2329496.1096;
    double y =  4563772.4417;
    double z =  3785616.6626;
    double lat, lon, h;
    _xyz_to_latlonh(x, y, z, &lat, &lon, &h);
    printf("期望 XYZ 对应的参数:\n");
    printf("纬度 = %.6f°\n", lat);
    printf("经度 = %.6f°\n", lon);
    printf("椭球高 = %.3f 米\n", h);
    return 0;
}










//--------------------



// int main() {
//     // 示例：将某经纬度转换为 XYZ
//     // 注意：以下数据仅供参考，实际结果需与您的源数据匹配
//     double lat = 36.64157073;   // 纬度 (北京)
//     double lon = 117.04122411;  // 经度
//     double h = 97.834;         // 高程 (米)

//     double x, y, z;
//     latlonh_to_xyz(lat, lon, h, &x, &y, &z);

//     printf("经纬度 (%.4f°, %.4f°, %.2fm) 转换为 ECEF:\n", lat, lon, h);
//     printf("X = %.10f\nY = %.10f\nZ = %.10f\n", x, y, z);

//     // 您给出的参考 XYZ：
//     // -2329496.1096, 4563772.4417, 3785616.6626
//     // 可通过调整 lat/lon/h 使结果与之接近



//     double x0 = -2329496.1096, y0 = 4563772.4417, z0 = 3785616.6626;
// double lat0, lon0, h0;
// xyz_to_latlonh(x0, y0, z0, &lat0, &lon0, &h0);
// printf("期望XYZ对应的经纬度/高程: %.6f°, %.6f°, %.3f m\n", lat0, lon0, h0);
//     return 0;
// }