/*******************************************************************************
 * 文件名称 : geo_utils.c
 * 文件说明 : LLA/ECEF/ENU 坐标转换辅助函数
 * 创建日期 : 2025-11-22
 *******************************************************************************/

#include "geo_utils.h"

#include <math.h>

#define DEG2RAD               (0.017453292519943295769236907684886f)
#define WGS84_A               (6378137.0)
#define WGS84_E2              (6.69437999014e-3)

static void geo_lla_to_ecef(double lat_rad, double lon_rad, double alt_m, double ecef[3])
{
    double sin_lat = sin(lat_rad);
    double cos_lat = cos(lat_rad);
    double sin_lon = sin(lon_rad);
    double cos_lon = cos(lon_rad);
    double N = WGS84_A / sqrt(1.0 - WGS84_E2 * sin_lat * sin_lat);

    ecef[0] = (N + alt_m) * cos_lat * cos_lon;
    ecef[1] = (N + alt_m) * cos_lat * sin_lon;
    ecef[2] = (N * (1.0 - WGS84_E2) + alt_m) * sin_lat;
}

static void geo_ecef_to_enu(const ST_GeoOrigin *origin, const double ecef[3], float enu[3])
{
    double dx = ecef[0] - origin->ecef[0];
    double dy = ecef[1] - origin->ecef[1];
    double dz = ecef[2] - origin->ecef[2];

    float east = (float)(-origin->sin_lon * dx + origin->cos_lon * dy);
    float north = (float)(-origin->sin_lat * origin->cos_lon * dx
                        - origin->sin_lat * origin->sin_lon * dy
                        + origin->cos_lat * dz);
    float up = (float)(origin->cos_lat * origin->cos_lon * dx
                     + origin->cos_lat * origin->sin_lon * dy
                     + origin->sin_lat * dz);

    enu[0] = east;
    enu[1] = north;
    enu[2] = up;
}

/* 预先计算 ENU 原点，缓存经纬度 sin/cos 以及 ECEF 坐标 */
HIDO_VOID Geo_OriginInit(ST_GeoOrigin *_pstOrigin, double _latDeg, double _lonDeg, double _altM)
{
    if (_pstOrigin == HIDO_NULL)
    {
        return;
    }

    _pstOrigin->lat_deg = _latDeg;
    _pstOrigin->lon_deg = _lonDeg;
    _pstOrigin->alt_m = _altM;
    _pstOrigin->lat_rad = _latDeg * DEG2RAD;
    _pstOrigin->lon_rad = _lonDeg * DEG2RAD;
    _pstOrigin->sin_lat = sin(_pstOrigin->lat_rad);
    _pstOrigin->cos_lat = cos(_pstOrigin->lat_rad);
    _pstOrigin->sin_lon = sin(_pstOrigin->lon_rad);
    _pstOrigin->cos_lon = cos(_pstOrigin->lon_rad);

    geo_lla_to_ecef(_pstOrigin->lat_rad,
                    _pstOrigin->lon_rad,
                    _pstOrigin->alt_m,
                    _pstOrigin->ecef);
    _pstOrigin->initialized = HIDO_TRUE;
}

/* 将 GPRMI (经纬高) 转换为相对原点的 ENU 坐标 */
HIDO_VOID Geo_GprmiToENU(const ST_GPRMI *_pstGprmi, const ST_GeoOrigin *_pstOrigin, float _enu[3])
{
    if (_enu == HIDO_NULL)
    {
        return;
    }

    _enu[0] = 0.0f;
    _enu[1] = 0.0f;
    _enu[2] = 0.0f;

    if (_pstGprmi == HIDO_NULL || _pstOrigin == HIDO_NULL)
    {
        return;
    }

    if (_pstGprmi->m_bValid == HIDO_FALSE || _pstOrigin->initialized == HIDO_FALSE)
    {
        return;
    }

    double ecef[3];
    geo_lla_to_ecef(_pstGprmi->m_dLatitude * DEG2RAD,
                    _pstGprmi->m_dLongitude * DEG2RAD,
                    (double)_pstGprmi->m_fAltitude,
                    ecef);

    geo_ecef_to_enu(_pstOrigin, ecef, _enu);
}