import numpy as np
from typing import Tuple, List
from abc import ABC, abstractmethod
from f02_1_flight_lifecycle_manager import CameraParameters

class ICameraModel(ABC):
    @abstractmethod
    def project(self, point_3d: np.ndarray, camera_params: CameraParameters) -> Tuple[float, float]: pass
    @abstractmethod
    def unproject(self, pixel: Tuple[float, float], depth: float, camera_params: CameraParameters) -> np.ndarray: pass
    @abstractmethod
    def get_focal_length(self, camera_params: CameraParameters) -> Tuple[float, float]: pass
    @abstractmethod
    def apply_distortion(self, pixel: Tuple[float, float], distortion_coeffs: List[float]) -> Tuple[float, float]: pass
    @abstractmethod
    def remove_distortion(self, pixel: Tuple[float, float], distortion_coeffs: List[float]) -> Tuple[float, float]: pass

class CameraModel(ICameraModel):
    """H01: Pinhole camera projection model with Brown-Conrady distortion handling."""
    def get_focal_length(self, camera_params: CameraParameters) -> Tuple[float, float]:
        w = camera_params.resolution.get("width", 1920)
        h = camera_params.resolution.get("height", 1080)
        sw = getattr(camera_params, 'sensor_width_mm', 36.0)
        sh = getattr(camera_params, 'sensor_height_mm', 24.0)
        fx = (camera_params.focal_length_mm * w) / sw if sw > 0 else w
        fy = (camera_params.focal_length_mm * h) / sh if sh > 0 else h
        return fx, fy

    def _get_intrinsics(self, camera_params: CameraParameters) -> np.ndarray:
        fx, fy = self.get_focal_length(camera_params)
        cx = camera_params.resolution.get("width", 1920) / 2.0
        cy = camera_params.resolution.get("height", 1080) / 2.0
        return np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]], dtype=np.float64)

    def project(self, point_3d: np.ndarray, camera_params: CameraParameters) -> Tuple[float, float]:
        if point_3d[2] == 0: return (-1.0, -1.0)
        K = self._get_intrinsics(camera_params)
        p = K @ point_3d
        return (p[0] / p[2], p[1] / p[2])

    def unproject(self, pixel: Tuple[float, float], depth: float, camera_params: CameraParameters) -> np.ndarray:
        K = self._get_intrinsics(camera_params)
        x = (pixel[0] - K[0, 2]) / K[0, 0]
        y = (pixel[1] - K[1, 2]) / K[1, 1]
        return np.array([x * depth, y * depth, depth])

    def apply_distortion(self, pixel: Tuple[float, float], distortion_coeffs: List[float]) -> Tuple[float, float]:
        return pixel

    def remove_distortion(self, pixel: Tuple[float, float], distortion_coeffs: List[float]) -> Tuple[float, float]:
        return pixel