"""
地理坐标与参考系转换工具。

提供以下能力：
- 解析 GGA/自定义格式的原点配置
- WGS-84 下的 LLA ↔ ECEF ↔ ENU 互转
- 将 ENU 坐标反算为经纬度
"""

from __future__ import annotations

import dataclasses
import math
from typing import Optional, Tuple

WGS84_A = 6378137.0  # 半长轴
WGS84_F = 1 / 298.257223563
WGS84_E2 = WGS84_F * (2 - WGS84_F)
WGS84_B = WGS84_A * (1 - WGS84_F)
WGS84_EP2 = (WGS84_A ** 2 - WGS84_B ** 2) / (WGS84_B ** 2)


def dm_to_decimal(dm_value: float) -> float:
    """度分转十进制度。"""
    degrees = int(dm_value // 100)
    minutes = dm_value - degrees * 100
    return degrees + minutes / 60.0


@dataclasses.dataclass(frozen=True)
class Origin:
    """ENU 参考原点。"""

    latitude_deg: float
    longitude_deg: float
    altitude_m: float

    def __post_init__(self):
        object.__setattr__(self, "latitude_rad", math.radians(self.latitude_deg))
        object.__setattr__(self, "longitude_rad", math.radians(self.longitude_deg))
        object.__setattr__(self, "ecef", geo_to_ecef(self.latitude_deg, self.longitude_deg, self.altitude_m))


def parse_origin(origin_geo: str) -> Origin:
    """
    解析原点描述:
        1) 纯 GGA 报文: "$GNGGA,..."
        2) 简写: "3949.8890,N,11616.7555,E[,alt]"
    """
    text = (origin_geo or "").strip()
    if not text:
        raise ValueError("原点字符串不能为空")

    if text.startswith("$") and "GGA" in text.upper():
        parts = text.split(",")
        if len(parts) < 10:
            raise ValueError("GGA 报文字段数量不足")
        lat_dm = float(parts[2])
        lat_dir = parts[3].upper()
        lon_dm = float(parts[4])
        lon_dir = parts[5].upper()
        alt = float(parts[9])

        lat = dm_to_decimal(lat_dm)
        lon = dm_to_decimal(lon_dm)
        if lat_dir == "S":
            lat = -lat
        if lon_dir == "W":
            lon = -lon
        return Origin(lat, lon, alt)

    # 兼容 ",lat,N,lon,E" 或 "lat,N,lon,E"
    text = text.lstrip(",")
    parts = [p.strip() for p in text.split(",") if p.strip()]
    if len(parts) not in (4, 5):
        raise ValueError("原点格式应为 'lat,N,lon,E[,alt]' 或 GGA 报文")
    lat_dm = float(parts[0])
    lat_dir = parts[1].upper()
    lon_dm = float(parts[2])
    lon_dir = parts[3].upper()
    alt = float(parts[4]) if len(parts) == 5 else 0.0

    lat = dm_to_decimal(lat_dm)
    lon = dm_to_decimal(lon_dm)
    if lat_dir == "S":
        lat = -lat
    if lon_dir == "W":
        lon = -lon
    return Origin(lat, lon, alt)


def geo_to_ecef(lat_deg: float, lon_deg: float, alt_m: float) -> Tuple[float, float, float]:
    """经纬度 → ECEF。"""
    lat_rad = math.radians(lat_deg)
    lon_rad = math.radians(lon_deg)
    sin_lat = math.sin(lat_rad)
    cos_lat = math.cos(lat_rad)
    sin_lon = math.sin(lon_rad)
    cos_lon = math.cos(lon_rad)

    N = WGS84_A / math.sqrt(1 - WGS84_E2 * sin_lat * sin_lat)
    x = (N + alt_m) * cos_lat * cos_lon
    y = (N + alt_m) * cos_lat * sin_lon
    z = (N * (1 - WGS84_E2) + alt_m) * sin_lat
    return x, y, z


def ecef_to_enu(dx: float, dy: float, dz: float, lat0_rad: float, lon0_rad: float) -> Tuple[float, float, float]:
    """ECEF 差值 → ENU。"""
    sin_lat = math.sin(lat0_rad)
    cos_lat = math.cos(lat0_rad)
    sin_lon = math.sin(lon0_rad)
    cos_lon = math.cos(lon0_rad)

    east = -sin_lon * dx + cos_lon * dy
    north = -sin_lat * cos_lon * dx - sin_lat * sin_lon * dy + cos_lat * dz
    up = cos_lat * cos_lon * dx + cos_lat * sin_lon * dy + sin_lat * dz
    return east, north, up


def enu_to_ecef(east: float, north: float, up: float, lat0_rad: float, lon0_rad: float) -> Tuple[float, float, float]:
    """ENU → ECEF 差值。"""
    sin_lat = math.sin(lat0_rad)
    cos_lat = math.cos(lat0_rad)
    sin_lon = math.sin(lon0_rad)
    cos_lon = math.cos(lon0_rad)

    dx = -sin_lon * east - sin_lat * cos_lon * north + cos_lat * cos_lon * up
    dy = cos_lon * east - sin_lat * sin_lon * north + cos_lat * sin_lon * up
    dz = cos_lat * north + sin_lat * up
    return dx, dy, dz


def ecef_to_lla(x: float, y: float, z: float) -> Tuple[float, float, float]:
    """ECEF → LLA。"""
    p = math.hypot(x, y)
    theta = math.atan2(WGS84_A * z, WGS84_B * p)
    sin_theta = math.sin(theta)
    cos_theta = math.cos(theta)

    lon = math.atan2(y, x)
    lat = math.atan2(
        z + WGS84_EP2 * WGS84_B * sin_theta ** 3,
        p - WGS84_E2 * WGS84_A * cos_theta ** 3,
    )

    N = WGS84_A / math.sqrt(1 - WGS84_E2 * math.sin(lat) ** 2)
    alt = p / math.cos(lat) - N
    return math.degrees(lat), math.degrees(lon), alt


def enu_to_lla(east: float, north: float, up: float, origin: Origin) -> Tuple[float, float, float]:
    """将 ENU 坐标转换为实时经纬度。"""
    dx, dy, dz = enu_to_ecef(east, north, up, origin.latitude_rad, origin.longitude_rad)
    x = origin.ecef[0] + dx
    y = origin.ecef[1] + dy
    z = origin.ecef[2] + dz
    return ecef_to_lla(x, y, z)


def heading_math_to_nav(heading_rad: float) -> float:
    """
    将数学坐标系 (东为0°, CCW为正) 的航向角转换为导航坐标系 (北为0°, 顺时针为正)。
    """
    heading_deg = math.degrees(heading_rad)
    nav = (90.0 - heading_deg) % 360.0
    if nav < 0:
        nav += 360.0
    return nav