"""
地理坐标工具集 —— 球面计算 + 矢量分解
"""
import math
from typing import Tuple

from uas.model.state import FlightPhase

EARTH_RADIUS = 6_371_000.0
DEG2RAD = math.pi / 180.0
RAD2DEG = 180.0 / math.pi


# def determine_phase1(alt_start, alt_end, vs):
#     if alt_start < 500 and alt_end < 500:
#         return FlightPhase.TAKEOFF if vs > 0.5 else FlightPhase.LAND
#     if alt_start < 500: return FlightPhase.TAKEOFF
#     if alt_end < 500: return FlightPhase.APPROACH
#     if vs > 1.0: return FlightPhase.CLIMB
#     if vs < -1.0: return FlightPhase.DESCENT
#     return FlightPhase.CRUISE
#
# def determine_phase2(altitude, vs, airspeed):
#     """判断飞行阶段（同 v1.0）"""
#     if altitude < 50.0:
#         return FlightPhase.LAND
#     if vs > 2.0:
#         return FlightPhase.CLIMB
#     if vs < -2.0:
#         if altitude < 3000.0 and airspeed < 103.0:
#             return FlightPhase.APPROACH
#         return FlightPhase.DESCENT
#     if altitude < 3000.0:
#         return FlightPhase.APPROACH
#     return FlightPhase.CRUISE

def determine_phase(alt: float, vs: float,
                     min_alt: float, max_alt: float):
    if alt <= min_alt + 50:
        return (FlightPhase.TAKEOFF if vs > 0.5
                else FlightPhase.GROUND)
    if alt >= max_alt - 200:
        return FlightPhase.CRUISE
    if vs > 2.0: return FlightPhase.CLIMB
    if vs < -2.0: return FlightPhase.DESCENT
    return FlightPhase.CRUISE

class GeoUtils:
    @staticmethod
    def offset_position(lat: float, lon: float,
                        bearing_deg: float, distance_m: float
                        ) -> Tuple[float, float]:
        """球面偏移：从 (lat,lon) 沿 bearing 移动 distance_m"""
        if distance_m == 0:
            return lat, lon
        lat_r = lat * DEG2RAD
        lon_r = lon * DEG2RAD
        brg_r = bearing_deg * DEG2RAD
        delta = distance_m / EARTH_RADIUS

        new_lat_r = math.asin(
            math.sin(lat_r) * math.cos(delta) +
            math.cos(lat_r) * math.sin(delta) * math.cos(brg_r)
        )
        new_lon_r = lon_r + math.atan2(
            math.sin(brg_r) * math.sin(delta) * math.cos(lat_r),
            math.cos(delta) - math.sin(lat_r) * math.sin(new_lat_r)
        )
        return new_lat_r * RAD2DEG, (new_lon_r * RAD2DEG + 540) % 360 - 180

    @staticmethod
    def haversine(lat1: float, lon1: float,
                  lat2: float, lon2: float) -> float:
        """大圆距离（米）"""
        dlat = (lat2 - lat1) * DEG2RAD
        dlon = (lon2 - lon1) * DEG2RAD
        a = (math.sin(dlat / 2) ** 2 +
             math.cos(lat1 * DEG2RAD) * math.cos(lat2 * DEG2RAD) *
             math.sin(dlon / 2) ** 2)
        return 2 * EARTH_RADIUS * math.asin(math.sqrt(max(0, min(1, a))))

    @staticmethod
    def bearing(lat1: float, lon1: float,
                lat2: float, lon2: float) -> float:
        """初始方位角（度，真北顺时针）"""
        lat1_r, lat2_r = lat1 * DEG2RAD, lat2 * DEG2RAD
        dlon_r = (lon2 - lon1) * DEG2RAD
        x = math.sin(dlon_r) * math.cos(lat2_r)
        y = (math.cos(lat1_r) * math.sin(lat2_r) -
             math.sin(lat1_r) * math.cos(lat2_r) * math.cos(dlon_r))
        return (math.atan2(x, y) * RAD2DEG) % 360.0

    @staticmethod
    def normalize_heading(hdg: float) -> float:
        return hdg % 360.0

    @staticmethod
    def heading_diff(h1: float, h2: float) -> float:
        """h2 - h1 的最短路径差值，范围 (-180, 180]"""
        return ((h2 - h1 + 540) % 360) - 180

    @staticmethod
    def wind_to_components(wind_dir_deg: float, wind_speed_ms: float
                           ) -> Tuple[float, float]:
        """
        风向（气象风向：风从该方向吹来）→ 北向/东向分量（m/s）
        北向分量为正表示向北，东向分量为正表示向东
        """
        # 气象风向转数学方向：风从 wind_dir 吹来，速度向量方向为 wind_dir + 180
        math_dir = (wind_dir_deg + 180.0) % 360.0
        rad = math_dir * DEG2RAD
        v_north = wind_speed_ms * math.cos(rad)
        v_east  = wind_speed_ms * math.sin(rad)
        return v_north, v_east

    @staticmethod
    def components_to_heading_speed(v_north: float, v_east: float
                                    ) -> Tuple[float, float]:
        """北向/东向速度分量 → 航向（度）+ 合速度（m/s）"""
        speed = math.sqrt(v_north ** 2 + v_east ** 2)
        heading = (math.atan2(v_east, v_north) * RAD2DEG) % 360.0
        return heading, speed