"""Camera utilities for creating view matrices and trajectories."""

import logging
from typing import List, Optional, Tuple, Union

import numpy as np
import torch

logger = logging.getLogger(__name__)


def create_rotation_matrix(
    angle_x: float,
    angle_y: float,
    angle_z: float = 0.0
) -> np.ndarray:
    """
    Create a 3x3 rotation matrix from Euler angles.

    Uses XYZ order (rotate around X, then Y, then Z).

    Args:
        angle_x: Rotation around X axis in degrees (pitch/tilt)
        angle_y: Rotation around Y axis in degrees (yaw/pan)
        angle_z: Rotation around Z axis in degrees (roll)

    Returns:
        3x3 rotation matrix as numpy array
    """
    ax, ay, az = np.radians([angle_x, angle_y, angle_z])

    # Rotation around X axis
    Rx = np.array([
        [1, 0, 0],
        [0, np.cos(ax), -np.sin(ax)],
        [0, np.sin(ax), np.cos(ax)]
    ], dtype=np.float32)

    # Rotation around Y axis
    Ry = np.array([
        [np.cos(ay), 0, np.sin(ay)],
        [0, 1, 0],
        [-np.sin(ay), 0, np.cos(ay)]
    ], dtype=np.float32)

    # Rotation around Z axis
    Rz = np.array([
        [np.cos(az), -np.sin(az), 0],
        [np.sin(az), np.cos(az), 0],
        [0, 0, 1]
    ], dtype=np.float32)

    # Combined rotation: Rz @ Ry @ Rx
    return Rz @ Ry @ Rx


def create_orbit_camera(
    angle_x: float,
    angle_y: float,
    distance: float,
    center: Tuple[float, float, float] = (0.0, 0.0, 0.0)
) -> np.ndarray:
    """
    Create a 4x4 camera extrinsic matrix for an orbital view around a scene center.

    Uses OpenGL camera convention (x-right, y-up, z-backward) which is standard
    for mesh rendering with nvdiffrast.

    Args:
        angle_x: Vertical angle in degrees (positive = look from above)
        angle_y: Horizontal angle in degrees (positive = rotate right)
        distance: Distance from camera to scene center
        center: Scene center point (x, y, z). TRELLIS2 meshes are typically
                centered at origin (0, 0, 0).

    Returns:
        4x4 world-to-camera transformation matrix (extrinsic)
    """
    # Convert angles to radians
    theta = np.radians(angle_y)  # Horizontal orbit (azimuth)
    phi = np.radians(angle_x)    # Vertical tilt (elevation)

    # Calculate camera position on a sphere around the center
    # Spherical to Cartesian conversion (OpenGL: y-up)
    x = distance * np.sin(theta) * np.cos(phi)
    y = distance * np.sin(phi)
    z = distance * np.cos(theta) * np.cos(phi)

    # Camera position in world coordinates
    camera_pos = np.array([x, y, z], dtype=np.float32) + np.array(center, dtype=np.float32)

    # Look-at direction (from camera to center)
    forward = np.array(center, dtype=np.float32) - camera_pos
    forward = forward / (np.linalg.norm(forward) + 1e-8)

    # Up vector in OpenGL convention (y-up)
    world_up = np.array([0.0, 1.0, 0.0], dtype=np.float32)

    # Right vector (cross product of forward and up)
    right = np.cross(forward, world_up)
    right_norm = np.linalg.norm(right)

    # Handle gimbal lock case (looking straight up or down)
    if right_norm < 1e-6:
        # Use a different up vector
        world_up = np.array([0.0, 0.0, 1.0], dtype=np.float32)
        right = np.cross(forward, world_up)
        right_norm = np.linalg.norm(right)

    right = right / (right_norm + 1e-8)

    # Recalculate up to be orthogonal
    up = np.cross(right, forward)
    up = up / (np.linalg.norm(up) + 1e-8)

    # Build rotation matrix (camera axes in world coordinates)
    # Camera coordinate system: x=right, y=up, z=-forward (OpenGL)
    R_cam_to_world = np.stack([right, up, -forward], axis=1)

    # World-to-camera rotation
    R = R_cam_to_world.T

    # Translation: camera position in camera coordinates
    t = -R @ camera_pos

    # Build 4x4 extrinsic matrix
    extrinsic = np.eye(4, dtype=np.float32)
    extrinsic[:3, :3] = R
    extrinsic[:3, 3] = t

    return extrinsic


def create_intrinsic_matrix(
    fov: float,
    width: int,
    height: int,
    fov_type: str = "horizontal"
) -> np.ndarray:
    """
    Create a 3x3 camera intrinsic matrix.

    Args:
        fov: Field of view in degrees
        width: Image width in pixels
        height: Image height in pixels
        fov_type: "horizontal" or "vertical" FOV specification

    Returns:
        3x3 intrinsic matrix K
    """
    fov_rad = np.radians(fov)

    if fov_type == "horizontal":
        focal = width / (2 * np.tan(fov_rad / 2))
    else:  # vertical
        focal = height / (2 * np.tan(fov_rad / 2))

    # Principal point at image center
    cx = width / 2.0
    cy = height / 2.0

    K = np.array([
        [focal, 0, cx],
        [0, focal, cy],
        [0, 0, 1]
    ], dtype=np.float32)

    return K


def create_projection_matrix(
    fov: float,
    aspect: float,
    near: float = 0.01,
    far: float = 100.0
) -> np.ndarray:
    """
    Create a 4x4 perspective projection matrix.

    Args:
        fov: Vertical field of view in degrees
        aspect: Aspect ratio (width / height)
        near: Near clipping plane
        far: Far clipping plane

    Returns:
        4x4 projection matrix
    """
    fov_rad = np.radians(fov)
    f = 1.0 / np.tan(fov_rad / 2)

    proj = np.zeros((4, 4), dtype=np.float32)
    proj[0, 0] = f / aspect
    proj[1, 1] = f
    proj[2, 2] = (far + near) / (near - far)
    proj[2, 3] = (2 * far * near) / (near - far)
    proj[3, 2] = -1

    return proj


def interpolate_cameras(
    cam1: np.ndarray,
    cam2: np.ndarray,
    t: float
) -> np.ndarray:
    """
    Interpolate between two camera matrices.

    Uses linear interpolation for translation and SLERP approximation for rotation.

    Args:
        cam1: First 4x4 camera extrinsic matrix
        cam2: Second 4x4 camera extrinsic matrix
        t: Interpolation factor (0 = cam1, 1 = cam2)

    Returns:
        Interpolated 4x4 camera extrinsic matrix
    """
    t = np.clip(t, 0.0, 1.0)

    # Extract rotation and translation
    R1, t1 = cam1[:3, :3], cam1[:3, 3]
    R2, t2 = cam2[:3, :3], cam2[:3, 3]

    # Linear interpolation for translation
    t_interp = t1 * (1 - t) + t2 * t

    # For rotation, we use a simple linear interpolation and re-orthogonalize
    # (proper SLERP would require quaternion conversion)
    R_interp = R1 * (1 - t) + R2 * t

    # Re-orthogonalize using SVD
    U, _, Vt = np.linalg.svd(R_interp)
    R_interp = U @ Vt

    # Ensure proper rotation (det = 1, not -1)
    if np.linalg.det(R_interp) < 0:
        R_interp = -R_interp

    # Build interpolated matrix
    result = np.eye(4, dtype=np.float32)
    result[:3, :3] = R_interp
    result[:3, 3] = t_interp

    return result


def create_camera_trajectory(
    trajectory_type: str,
    angle_x: float,
    angle_y: float,
    distance: float,
    center: Tuple[float, float, float],
    num_frames: int,
    animate: bool = False,
    animation_range: float = 10.0,
    animation_axis: str = "y"
) -> List[np.ndarray]:
    """
    Create a sequence of camera matrices for a trajectory.

    Args:
        trajectory_type: One of "orbit", "dolly", "pan", "static"
        angle_x: Base vertical angle in degrees
        angle_y: Base horizontal angle in degrees
        distance: Base distance from scene center
        center: Scene center point
        num_frames: Number of frames in the trajectory
        animate: Whether to animate the camera over the sequence
        animation_range: Range of animation in degrees (for orbit/pan) or distance (for dolly)
        animation_axis: Axis to animate around ("x", "y", or "z")

    Returns:
        List of 4x4 camera extrinsic matrices, one per frame
    """
    cameras = []

    for i in range(num_frames):
        t = i / max(num_frames - 1, 1)  # Normalized time [0, 1]

        if not animate:
            # Static camera - same for all frames
            cam = create_orbit_camera(angle_x, angle_y, distance, center)
        else:
            # Animated camera
            if trajectory_type == "orbit":
                # Orbit around the scene
                if animation_axis == "y":
                    # Horizontal orbit
                    current_angle_y = angle_y + (t - 0.5) * animation_range
                    cam = create_orbit_camera(angle_x, current_angle_y, distance, center)
                else:  # x axis
                    # Vertical orbit
                    current_angle_x = angle_x + (t - 0.5) * animation_range
                    cam = create_orbit_camera(current_angle_x, angle_y, distance, center)

            elif trajectory_type == "dolly":
                # Move camera closer/farther
                current_distance = distance + (t - 0.5) * animation_range
                current_distance = max(0.5, current_distance)  # Don't go too close
                cam = create_orbit_camera(angle_x, angle_y, current_distance, center)

            elif trajectory_type == "pan":
                # Pan the camera (translate horizontally)
                pan_offset = (t - 0.5) * animation_range * 0.1  # Scale down for translation
                current_center = (
                    center[0] + pan_offset,
                    center[1],
                    center[2]
                )
                cam = create_orbit_camera(angle_x, angle_y, distance, current_center)

            else:  # static or unknown
                cam = create_orbit_camera(angle_x, angle_y, distance, center)

        cameras.append(cam)

    return cameras


def estimate_mesh_center(vertices: Union[torch.Tensor, np.ndarray]) -> Tuple[float, float, float]:
    """
    Estimate the scene center from mesh vertices.

    Args:
        vertices: [N, 3] tensor/array of vertex positions

    Returns:
        Tuple (x, y, z) of estimated scene center
    """
    if isinstance(vertices, torch.Tensor):
        center = vertices.mean(dim=0).cpu().numpy()
    else:
        center = vertices.mean(axis=0)
    return tuple(center.tolist())


def estimate_mesh_distance(
    vertices: Union[torch.Tensor, np.ndarray],
    center: Optional[Tuple[float, float, float]] = None
) -> float:
    """
    Estimate an appropriate camera distance for viewing the mesh.

    Args:
        vertices: [N, 3] tensor/array of vertex positions
        center: Optional pre-computed scene center

    Returns:
        Recommended camera distance
    """
    if center is None:
        center = estimate_mesh_center(vertices)

    if isinstance(vertices, torch.Tensor):
        center_tensor = torch.tensor(center, device=vertices.device, dtype=vertices.dtype)
        distances = torch.norm(vertices - center_tensor, dim=1)
        scene_radius = torch.quantile(distances, 0.95).item()
    else:
        center_arr = np.array(center, dtype=vertices.dtype)
        distances = np.linalg.norm(vertices - center_arr, axis=1)
        scene_radius = np.percentile(distances, 95)

    # Camera should be at roughly 2-3x the scene radius
    return scene_radius * 2.5