"""Bbox + takeoff-origin helpers (AZ-489).

Implements FAC-INV-3 (horizontal-distance buffer via ENU round-trip) and
FAC-INV-4 (takeoff origin is ``waypoints[0]``, no rounding).
"""

from __future__ import annotations

import math

import numpy as np

from gps_denied_onboard._types.geo import BoundingBox, LatLonAlt
from gps_denied_onboard.components.c12_operator_tooling.flights_api.errors import (
    EmptyWaypointsError,
)
from gps_denied_onboard.components.c12_operator_tooling.flights_api.interface import (
    FlightDto,
    WaypointDto,
)
from gps_denied_onboard.helpers.wgs_converter import WgsConverter

__all__ = ["bbox_from_waypoints", "takeoff_origin_from_flight"]


def bbox_from_waypoints(
    waypoints: tuple[WaypointDto, ...],
    *,
    buffer_m: float = 1000.0,
) -> BoundingBox:
    """Envelope ``waypoints`` lat/lon and inflate by ``buffer_m`` horizontal metres.

    The buffer is applied in local-ENU around the envelope centre so the
    inflation is a true horizontal distance at the flight's latitude. Naive
    ``min_lat - buffer / 111000`` style buffering is intentionally NOT used
    — it under-inflates the east/west extent at high latitudes.

    Raises:
        EmptyWaypointsError: ``waypoints`` is empty.
        ValueError: ``buffer_m`` is negative or non-finite.
    """
    if not waypoints:
        raise EmptyWaypointsError(
            "FlightDto.waypoints is empty; re-plan the mission in the Mission Planner UI"
        )
    if not math.isfinite(buffer_m) or buffer_m < 0.0:
        raise ValueError(f"buffer_m must be a non-negative finite number; got {buffer_m!r}")

    min_lat = min(wp.lat_deg for wp in waypoints)
    max_lat = max(wp.lat_deg for wp in waypoints)
    min_lon = min(wp.lon_deg for wp in waypoints)
    max_lon = max(wp.lon_deg for wp in waypoints)

    origin = LatLonAlt(
        lat_deg=(min_lat + max_lat) / 2.0,
        lon_deg=(min_lon + max_lon) / 2.0,
        alt_m=0.0,
    )
    sw = LatLonAlt(lat_deg=min_lat, lon_deg=min_lon, alt_m=0.0)
    ne = LatLonAlt(lat_deg=max_lat, lon_deg=max_lon, alt_m=0.0)

    sw_enu = WgsConverter.latlonalt_to_local_enu(origin, sw)
    ne_enu = WgsConverter.latlonalt_to_local_enu(origin, ne)

    sw_inflated_enu = np.array(
        [sw_enu[0] - buffer_m, sw_enu[1] - buffer_m, 0.0], dtype=np.float64
    )
    ne_inflated_enu = np.array(
        [ne_enu[0] + buffer_m, ne_enu[1] + buffer_m, 0.0], dtype=np.float64
    )

    sw_inflated = WgsConverter.local_enu_to_latlonalt(origin, sw_inflated_enu)
    ne_inflated = WgsConverter.local_enu_to_latlonalt(origin, ne_inflated_enu)

    return BoundingBox(
        min_lat_deg=sw_inflated.lat_deg,
        min_lon_deg=sw_inflated.lon_deg,
        max_lat_deg=ne_inflated.lat_deg,
        max_lon_deg=ne_inflated.lon_deg,
    )


def takeoff_origin_from_flight(flight: FlightDto) -> LatLonAlt:
    """Return ``waypoints[0]`` as a :class:`LatLonAlt` — no rounding, no projection.

    Raises:
        EmptyWaypointsError: ``flight.waypoints`` is empty (should not happen
            on a parser-validated DTO; defensive check).
    """
    if not flight.waypoints:
        raise EmptyWaypointsError(
            "FlightDto.waypoints is empty; re-plan the mission in the Mission Planner UI"
        )
    first = flight.waypoints[0]
    return LatLonAlt(lat_deg=first.lat_deg, lon_deg=first.lon_deg, alt_m=first.alt_m)
