"""Open3D camera helpers for finger mesh examples."""

from __future__ import annotations

from pathlib import Path

import numpy as np

DEFAULT_FINGER_VIZ_POSE = "assets/finger/visualization/world_to_cam.npy"

_CAMERA_X_ROT_90 = np.array(
    [
        [1.0, 0.0, 0.0],
        [0.0, 0.0, -1.0],
        [0.0, -1.0, 0.0],
    ],
    dtype=np.float64,
)

_CAMERA_Z_ROT_180 = np.array(
    [
        [1.0, 0.0, 0.0],
        [0.0, 1.0, -1],
        [0.0, 0.0, -1],
    ],
    dtype=np.float64,
)


def resolve_viz_pose_path(path: str | Path | None = None) -> Path:
    """Resolve world_to_cam.npy from cwd or packaged asset layout."""
    raw = Path(path or DEFAULT_FINGER_VIZ_POSE).expanduser()
    if raw.is_file():
        return raw.resolve()

    here = Path(__file__).resolve()
    candidates = [
        Path.cwd() / raw,
        here.parents[2] / raw,
    ]
    for candidate in candidates:
        if candidate.is_file():
            return candidate.resolve()

    checked = ", ".join(str(p) for p in candidates)
    raise FileNotFoundError(f"Visualization pose file not found: {raw}. Checked: {checked}")


def camera_look_at_from_world_to_cam(
    world_to_cam: np.ndarray,
    *,
    lookat: np.ndarray,
    distance_scale: float = 2.5,
) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """Derive Open3D look_at(center, eye, up) from a Blender world_to_cam matrix."""
    world_to_cam = np.asarray(world_to_cam, dtype=np.float64)
    cam_to_world = np.linalg.inv(world_to_cam)
    rotation = cam_to_world[:3, :3] @ _CAMERA_X_ROT_90 @ _CAMERA_Z_ROT_180

    front = rotation @ np.array([0.0, 0.0, -1.0])
    up = rotation @ np.array([0.0, 1.0, 0.0])
    front_norm = np.linalg.norm(front)
    up_norm = np.linalg.norm(up)
    if front_norm > 0:
        front = front / front_norm
    if up_norm > 0:
        up = up / up_norm

    lookat = np.asarray(lookat, dtype=np.float64).reshape(3)
    extent = float(np.linalg.norm(lookat - cam_to_world[:3, 3]))
    if extent < 1e-6:
        extent = 1.0
    eye = lookat - front * (extent * distance_scale)
    return lookat, eye, up


def apply_view_from_world_to_cam(vis, world_to_cam: np.ndarray, *, lookat: np.ndarray) -> None:
    """Apply Blender-exported world_to_cam as the Open3D default view."""
    world_to_cam = np.asarray(world_to_cam, dtype=np.float64)
    cam_to_world = np.linalg.inv(world_to_cam)
    rotation = cam_to_world[:3, :3] @ _CAMERA_X_ROT_90 @ _CAMERA_Z_ROT_180

    front = rotation @ np.array([0.0, 0.0, -1.0])
    up = rotation @ np.array([0.0, 1.0, 0.0])
    front_norm = np.linalg.norm(front)
    up_norm = np.linalg.norm(up)
    if front_norm > 0:
        front = front / front_norm
    if up_norm > 0:
        up = up / up_norm

    view_control = vis.get_view_control()
    view_control.set_lookat(lookat.astype(np.float64))
    view_control.set_front(front.astype(np.float64))
    view_control.set_up(up.astype(np.float64))
    vis.update_renderer()


def apply_mesh_lighting(vis) -> None:
    """Enable shaded lighting so the tactile mesh reads as a 3D surface."""
    import open3d as o3d

    render_option = vis.get_render_option()
    if hasattr(render_option, "light_on"):
        render_option.light_on = True
    if hasattr(render_option, "mesh_color_option"):
        render_option.mesh_color_option = o3d.visualization.MeshColorOption.Color
    if hasattr(render_option, "mesh_shade_option"):
        render_option.mesh_shade_option = o3d.visualization.MeshShadeOption.Color
    vis.update_renderer()