"""Endpoint per la tab Braccio DX (G1, polso destro, Nuwa eye-in-hand)."""
from __future__ import annotations

from typing import Any

from fastapi import APIRouter, HTTPException
from fastapi.responses import Response

from ..clients import G1Unavailable
from ..config import update_config
from ..geometry import quaternion_xyzw_to_rpy_deg
from ..timing import Stopwatch
from . import _deps

router = APIRouter()


def _state_payload(sw: Stopwatch) -> dict[str, Any]:
    cfg = _deps.tab_right_arm_cfg()
    with sw.step("g1_wrist"):
        wrist = _deps.g1().wrist()["right_wrist"]
    pos = wrist["position"]
    quat = wrist["quaternion"]
    rpy = quaternion_xyzw_to_rpy_deg(quat)
    return {
        "wrist": {"position": pos, "quaternion": quat,
                  "rpy_deg": [rpy[0], rpy[1], rpy[2]]},
        "robot_active": _deps.active_robot_key(),
        "side": _deps.default_side(),
        "home_pose": cfg.get("home_pose"),
        "custom_pose": cfg.get("custom_pose"),
        "offset_camera_mm": cfg.get("move_offset_camera_mm"),
        "axis_signs": cfg.get("axis_sign_cam_to_wrist"),
        "max_step_m": cfg.get("max_step_m"),
        "z_floor_m": cfg.get("z_floor_m"),
        "timings_ms": sw.as_dict(),
    }


@router.get("/state")
def get_state() -> dict[str, Any]:
    sw = Stopwatch()
    try:
        return _state_payload(sw)
    except G1Unavailable as e:
        raise HTTPException(status_code=502, detail=str(e))


@router.post("/activate")
def post_activate() -> dict[str, Any]:
    sw = Stopwatch()
    with sw.step("config_update"):
        update_config({"robot": {"active": "g1", "wrist_side": "right"}})
    try:
        return _state_payload(sw)
    except G1Unavailable as e:
        raise HTTPException(status_code=502, detail=str(e))


@router.get("/snapshot/front")
def snapshot_front() -> Response:
    try:
        jpeg = _deps.front_camera().rgb_jpeg()
    except Exception as e:
        raise HTTPException(status_code=502, detail=f"front_camera unreachable: {e}")
    return Response(content=jpeg, media_type="image/jpeg")


@router.get("/snapshot/wrist")
def snapshot_wrist() -> Response:
    try:
        jpeg = _deps.nuwa().rgb_jpeg()
    except Exception as e:
        raise HTTPException(status_code=502, detail=f"wrist nuwa unreachable: {e}")
    return Response(content=jpeg, media_type="image/jpeg")