/**
******************************************************************************
* @file           : studio_proj_c.c
* @author         : wyj
* @brief          : 高斯投影 C 语言版
* @attention      : None
* @date           : 2025/5/9
******************************************************************************
*/

#include "studio_proj_c.h"

void lonlat_to_gauss(const double central, const double lon, const double lat, double *gx, double *gy)
{
    // 将纬度和经度转换为弧度
    double lamb = DEG2RAD_C(lat);
    double phi = DEG2RAD_C(lon);

    // 将中央子午线转换为弧度
    double centralMeridianRad = DEG2RAD_C(central);

    // 计算高斯-克吕格投影公式中的参数
    double N = WGS84_A_C / sqrt(1 - WGS84_E2_C * sin(lamb) * sin(lamb));
    double T = tan(lamb) * tan(lamb);
    double C = WGS84_E2_C * cos(lamb) * cos(lamb) / (1 - WGS84_E2_C);
    double A = (phi - centralMeridianRad) * cos(lamb);

    // 计算子午线弧长
    double M = WGS84_A_C * ((1 - WGS84_E2_C / 4.0 - 3.0 * WGS84_E2_C * WGS84_E2_C / 64.0 - 5.0 * WGS84_E2_C * WGS84_E2_C * WGS84_E2_C / 256.0) * lamb -
                            (3.0 * WGS84_E2_C / 8.0 + 3.0 * WGS84_E2_C * WGS84_E2_C / 32.0 + 45.0 * WGS84_E2_C * WGS84_E2_C * WGS84_E2_C / 1024.0) * sin(2.0 * lamb) +
                            (15.0 * WGS84_E2_C * WGS84_E2_C / 256.0 + 45.0 * WGS84_E2_C * WGS84_E2_C * WGS84_E2_C / 1024.0) * sin(4.0 * lamb) - (35.0 * WGS84_E2_C * WGS84_E2_C * WGS84_E2_C / 3072.0) * sin(6.0 * lamb));

    // 计算 X, Y 坐标
    *gy = M + N * tan(lamb) * (A * A / 2.0 + (5.0 - T + 9.0 * C + 4.0 * C * C) * A * A * A * A / 24.0 + (61.0 - 58.0 * T + T * T + 600.0 * C - 330.0 * WGS84_E2_C) * A * A * A * A * A * A / 720.0);
    *gx = N * (A + (1.0 - T + C) * A * A * A / 6.0 + (5.0 - 18.0 * T + T * T + 72.0 * C - 58.0 * WGS84_E2_C) * A * A * A * A * A / 120.0) + 500000.0;  // 中央子午线偏移+500000.0
}

void gauss_to_lonlat(const double central, const double gx, const double gy, double *lon, double *lat)
{
    // 将高斯平面坐标转换为经纬度坐标
    double Y = gx;
    double X = gy;

    // 去除中央经线的偏移量
    Y -= 500000;
    // 计算第一偏心率的平方根倒数
    double e1 = (1 - sqrt(1 - WGS84_E2_C)) / (1 + sqrt(1 - WGS84_E2_C));
    // 计算子午线弧长
    double M = X;
    // 计算子午线曲率半径
    double mu = M / (WGS84_A_C * (1 - WGS84_E2_C / 4.0 - 3 * WGS84_E2_C * WGS84_E2_C / 64.0 - 5 * WGS84_E2_C * WGS84_E2_C * WGS84_E2_C / 256.0));
    // 计算纬度
    double phi1 = mu + (3 * e1 / 2 - 27 * e1 * e1 * e1 / 32) * sin(2 * mu) + (21 * e1 * e1 / 16 - 55 * e1 * e1 * e1 * e1 / 32) * sin(4 * mu) + (151 * e1 * e1 * e1 / 96) * sin(6 * mu) + (1097 * e1 * e1 * e1 * e1 / 512) * sin(8 * mu);

    // 计算子午线曲率半径
    double C1 = WGS84_E2_C * cos(phi1) * cos(phi1) / (1 - WGS84_E2_C);
    // 计算纬度曲率半径
    double T1 = tan(phi1) * tan(phi1);
    // 计算卯酉圈曲率半径
    double N1 = WGS84_A_C / sqrt(1 - WGS84_E2_C * sin(phi1) * sin(phi1));
    // 计算子午线弧长
    double R1 = WGS84_A_C * (1 - WGS84_E2_C) / pow(1 - WGS84_E2_C * sin(phi1) * sin(phi1), 1.5);
    // 计算子午线弧长
    double D = Y / N1;

    // 经纬度计算
    double phi = phi1 - (N1 * tan(phi1) / R1) * (D * D / 2 - (5 + 3 * T1 + 10 * C1 - 4 * C1 * C1 - 9 * e1) * D * D * D * D / 24 + (61 + 90 * T1 + 298 * C1 + 45 * T1 * T1 - 252 * e1 - 3 * C1 * C1) * D * D * D * D * D * D / 720);

    double lambda = DEG2RAD_C(central) + (D - (1 + 2 * T1 + C1) * D * D * D / 6 + (5 - 2 * C1 + 28 * T1 - 3 * C1 * C1 + 8 * e1 + 24 * T1 * T1) * D * D * D * D * D / 120) / cos(phi1);

    // 转换为度
    *lat = RAD2DEG_C(phi);
    *lon = RAD2DEG_C(lambda);
}

void lonlat_to_mercator(const double lon, const double lat, double *mctx, double *mcty)
{
    // 经度转换：phi = lon * (M_PI / 180.0)
    double phi = DEG2RAD_C(lon);

    // 纬度转换：lamb = lat * (M_PI / 180.0)
    double lamb = DEG2RAD_C(lat);

    // 墨卡托投影公式：mctx = WGS84_A_C * phi
    *mctx = WGS84_A_C * phi;

    // 墨卡托投影公式：mcty = WGS84_A_C * log(tan(M_PI / 4 + lamb / 2))
    *mcty = WGS84_A_C * log(tan(M_PI / 4 + lamb / 2));
}

void mercator_to_lonlat(const double mctx, const double mcty, double *lon, double *lat)
{
    // 经度转换：mctx / WGS84_A_C
    *lon = RAD2DEG_C(mctx / WGS84_A_C);

    // 纬度转换：使用逆墨卡托公式
    *lat = RAD2DEG_C(2 * atan(exp(mcty / WGS84_A_C)) - M_PI / 2);
}

// 将高斯投影坐标转换为墨卡托投影坐标
void gauss_to_mercator(const double central, const double gx, const double gy, double *mctx, double *mcty)
{
    // 定义经度和纬度变量
    double lon;
    double lat;
    // 将高斯投影坐标转换为经纬度坐标
    gauss_to_lonlat(central, gx, gy, &lon, &lat);
    // 将经纬度坐标转换为墨卡托投影坐标
    lonlat_to_mercator(lon, lat, mctx, mcty);
}

// 将墨卡托坐标转换为高斯坐标
void mercator_to_gauss(const double central, const double mctx, const double mcty, double *gx, double *gy)
{
    // 定义经纬度变量
    double lon;
    double lat;
    // 将墨卡托坐标转换为经纬度
    mercator_to_lonlat(mctx, mcty, &lon, &lat);
    // 将经纬度转换为高斯坐标
    lonlat_to_gauss(central, lon, lat, gx, gy);
}

void lonlat_to_ecef(const double lon, const double lat, const double height, double* x, double* y, double* z)
{
    // 将经纬度转换为弧度
    double lamb = DEG2RAD_C(lat);
    double phi = DEG2RAD_C(lon);

    // 计算N (曲率半径)
    double N = WGS84_A_C / sqrt(1 - WGS84_E2_C * sin(lamb) * sin(lamb));

    // 计算XYZ坐标
    *x = (N + height) * cos(lamb) * cos(phi);
    *y = (N + height) * cos(lamb) * sin(phi);
    *z = (WGS84_B_C * WGS84_B_C / (WGS84_A_C * WGS84_A_C) * N + height) * sin(lamb);
}

void ecef_to_lonlat(const double x, const double y, const double z, double* lon, double* lat, double* height)
{
    // 计算经度
    *lon = atan2(y, x);

    // 计算初始纬度估计
    double p = sqrt(x * x + y * y);
    double theta = atan2(z, p * (1 - WGS84_F_C));
    *lat = atan2(z + WGS84_E2_C * WGS84_B_C * pow(sin(theta), 3), p - WGS84_E2_C * WGS84_B_C * pow(cos(theta), 3));

    // 迭代计算纬度，直到收敛
    double previousLatitude;
    do
    {
        previousLatitude = *lat;
        double N = WGS84_A_C / sqrt(1 - WGS84_E2_C * sin(*lat) * sin(*lat));
        *height = p / cos(*lat) - N;
        *lat = atan2(z + WGS84_E2_C * N * sin(*lat), p);
    } while (fabs(*lat - previousLatitude) > 1e-12);  // 收敛条件

    // 将纬度和经度转换为度
    *lat = RAD2DEG_C(*lat);
    *lon = RAD2DEG_C(*lon);
}