"""Coordinate Transformer (Component F13)."""

import math

import cv2
import numpy as np

from gps_denied.schemas import CameraParameters, GPSPoint

# ---------------------------------------------------------------------------
# Module-level helpers
# ---------------------------------------------------------------------------

def _build_intrinsic_matrix(cam: CameraParameters) -> np.ndarray:
    """Build 3x3 camera intrinsic matrix K from CameraParameters."""
    fx = cam.focal_length * cam.resolution_width / cam.sensor_width
    fy = cam.focal_length * cam.resolution_height / cam.sensor_height
    if cam.principal_point is not None:
        cx, cy = cam.principal_point
    else:
        cx = cam.resolution_width / 2.0
        cy = cam.resolution_height / 2.0
    return np.array([
        [fx,  0, cx],
        [ 0, fy, cy],
        [ 0,  0,  1],
    ], dtype=np.float64)


def _cam_to_body_rotation() -> np.ndarray:
    """Camera-to-body rotation for nadir-pointing camera.

    Camera frame: Z forward (optical axis), X right, Y down (OpenCV convention).
    Body frame: X forward (nose), Y right (starboard), Z down.
    Camera points nadir: camera Z-axis aligns with body -Z (downward).

    The rotation is Rx(180deg): flips Y and Z axes.
    R_cam_body = [[1, 0, 0], [0, -1, 0], [0, 0, -1]]
    """
    return np.array([
        [1,  0,  0],
        [0, -1,  0],
        [0,  0, -1],
    ], dtype=np.float64)


def _quat_to_rotation_matrix(q: np.ndarray) -> np.ndarray:
    """Convert [w, x, y, z] quaternion to 3x3 rotation matrix."""
    w, x, y, z = q
    return np.array([
        [1 - 2*(y*y + z*z), 2*(x*y - w*z),     2*(x*z + w*y)],
        [2*(x*y + w*z),     1 - 2*(x*x + z*z), 2*(y*z - w*x)],
        [2*(x*z - w*y),     2*(y*z + w*x),     1 - 2*(x*x + y*y)],
    ], dtype=np.float64)


class OriginNotSetError(Exception):
    """Raised when ENU origin is required but not set."""


class CoordinateTransformer:
    """Manages ENU origin and coordinate transformations."""

    def __init__(self):
        # flight_id -> GPSPoint (origin)
        self._origins: dict[str, GPSPoint] = {}

    def set_enu_origin(self, flight_id: str, origin_gps: GPSPoint) -> None:
        """Sets the ENU origin for a specific flight."""
        self._origins[flight_id] = origin_gps

    def get_enu_origin(self, flight_id: str) -> GPSPoint:
        """Returns the ENU origin for a specific flight."""
        if flight_id not in self._origins:
            raise OriginNotSetError(f"ENU Origin not set for flight_id: {flight_id}")
        return self._origins[flight_id]

    def gps_to_enu(self, flight_id: str, gps: GPSPoint) -> tuple[float, float, float]:
        """Converts GPS coordinates to ENU (East, North, Up) relative to flight origin."""
        origin = self.get_enu_origin(flight_id)

        delta_lat = gps.lat - origin.lat
        delta_lon = gps.lon - origin.lon

        # 111319.5 meters per degree at equator
        east = delta_lon * math.cos(math.radians(origin.lat)) * 111319.5
        north = delta_lat * 111319.5
        up = 0.0

        return (east, north, up)

    def enu_to_gps(self, flight_id: str, enu: tuple[float, float, float]) -> GPSPoint:
        """Converts ENU coordinates back to WGS84 GPS."""
        origin = self.get_enu_origin(flight_id)

        east, north, up = enu

        delta_lat = north / 111319.5
        delta_lon = east / (math.cos(math.radians(origin.lat)) * 111319.5)

        return GPSPoint(lat=origin.lat + delta_lat, lon=origin.lon + delta_lon)

    def pixel_to_gps(
        self,
        flight_id: str,
        pixel: tuple[float, float],
        frame_pose: dict,
        camera_params: CameraParameters,
        altitude: float,
        quaternion: np.ndarray | None = None,
    ) -> GPSPoint:
        """Unproject pixel to GPS via ray-ground intersection.

        Chain: pixel -> camera ray (K^-1) -> body (T_cam_body) -> ENU (quaternion) -> WGS84.

        Args:
            flight_id: Flight identifier for ENU origin lookup.
            pixel: (u, v) pixel coordinates.
            frame_pose: Dict with "position" key -> [east, north, up] in ENU meters.
            camera_params: Camera intrinsic parameters.
            altitude: UAV altitude above ground in meters (positive up).
            quaternion: [w, x, y, z] body-to-ENU rotation quaternion.
                        If None, assumes identity (level flight, north-facing).
        """
        # Step 1: Pixel -> camera ray
        K = _build_intrinsic_matrix(camera_params)
        K_inv = np.linalg.inv(K)
        pixel_h = np.array([pixel[0], pixel[1], 1.0])
        ray_cam = K_inv @ pixel_h  # direction in camera frame

        # Step 2: Camera -> body
        R_cam_body = _cam_to_body_rotation()
        ray_body = R_cam_body @ ray_cam

        # Step 3: Body -> ENU
        if quaternion is not None:
            R_body_enu = _quat_to_rotation_matrix(quaternion)
        else:
            R_body_enu = np.eye(3)
        ray_enu = R_body_enu @ ray_body

        # Step 4: Ray-ground intersection
        # UAV is at altitude (Up component), ground is at Up=0
        # Ray: P = P_uav + t * ray_enu
        # Ground plane: Up = 0 -> t = -altitude / ray_enu[2]
        if abs(ray_enu[2]) < 1e-10:
            # Ray parallel to ground — return UAV position projected
            frame_enu = frame_pose.get("position", [0, 0, 0])
            return self.enu_to_gps(flight_id, (frame_enu[0], frame_enu[1], 0.0))

        t = -altitude / ray_enu[2]
        if t < 0:
            # Ray points upward — use UAV position
            frame_enu = frame_pose.get("position", [0, 0, 0])
            return self.enu_to_gps(flight_id, (frame_enu[0], frame_enu[1], 0.0))

        frame_enu = frame_pose.get("position", [0, 0, 0])
        ground_east = frame_enu[0] + t * ray_enu[0]
        ground_north = frame_enu[1] + t * ray_enu[1]

        # Step 5: ENU -> WGS84
        return self.enu_to_gps(flight_id, (ground_east, ground_north, 0.0))

    def gps_to_pixel(
        self,
        flight_id: str,
        gps: GPSPoint,
        frame_pose: dict,
        camera_params: CameraParameters,
        altitude: float,
        quaternion: np.ndarray | None = None,
    ) -> tuple[float, float]:
        """Project GPS coordinate to image pixel.

        Inverse of pixel_to_gps: WGS84 -> ENU -> body -> camera -> pixel.
        """
        # Step 1: GPS -> ENU
        enu = self.gps_to_enu(flight_id, gps)

        # Step 2: ENU point relative to UAV
        frame_enu = frame_pose.get("position", [0, 0, 0])
        point_enu = np.array([
            enu[0] - frame_enu[0],
            enu[1] - frame_enu[1],
            -altitude,  # ground is at -altitude relative to UAV (UAV is at +altitude)
        ])

        # Step 3: ENU -> body
        if quaternion is not None:
            R_body_enu = _quat_to_rotation_matrix(quaternion)
        else:
            R_body_enu = np.eye(3)
        point_body = R_body_enu.T @ point_enu

        # Step 4: Body -> camera
        R_cam_body = _cam_to_body_rotation()
        point_cam = R_cam_body.T @ point_body

        # Step 5: Camera -> pixel
        if abs(point_cam[2]) < 1e-10:
            cx = camera_params.resolution_width / 2.0
            cy = camera_params.resolution_height / 2.0
            return (cx, cy)

        K = _build_intrinsic_matrix(camera_params)
        pixel_h = K @ (point_cam / point_cam[2])
        return (float(pixel_h[0]), float(pixel_h[1]))

    def image_object_to_gps(
        self,
        flight_id: str,
        frame_id: int,
        object_pixel: tuple[float, float],
        frame_pose: dict | None = None,
        camera_params: CameraParameters | None = None,
        altitude: float = 100.0,
        quaternion: np.ndarray | None = None,
    ) -> GPSPoint:
        """Convert object pixel coordinates to GPS using full projection chain.

        If frame_pose or camera_params are not provided, uses defaults for
        backward compatibility (level flight, ADTI 20L V1 camera).
        """
        if frame_pose is None:
            frame_pose = {"position": [0, 0, 0]}
        if camera_params is None:
            camera_params = CameraParameters(
                focal_length=16.0,
                sensor_width=23.2,
                sensor_height=15.4,
                resolution_width=5456,
                resolution_height=3632,
            )
        return self.pixel_to_gps(
            flight_id, object_pixel, frame_pose, camera_params, altitude, quaternion
        )

    def transform_points(
        self,
        points: list[tuple[float, float]],
        transformation: list[list[float]],
    ) -> list[tuple[float, float]]:
        """Apply 3x3 homography to a list of 2D points using cv2.perspectiveTransform."""
        if not points:
            return []
        H = np.array(transformation, dtype=np.float64)
        pts = np.array(points, dtype=np.float64).reshape(-1, 1, 2)
        transformed = cv2.perspectiveTransform(pts, H)
        return [(float(p[0][0]), float(p[0][1])) for p in transformed]
