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

import logging
from typing import Any, Optional

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

import numpy as np

from ..clients import G1Unavailable
from ..config import update_config
from ..geometry import quaternion_xyzw_to_rpy_deg, rpy_deg_to_quaternion_xyzw
from ..perception.sam21_bridge import Sam21Fallback, segment_with_sam21
from ..right_arm.move import DeltaOversize, apply_z_floor, compute_delta_wrist
from ..timing import Stopwatch
from . import _deps
from .perception_routes import _bbox_to_roi

router = APIRouter()
log = logging.getLogger(__name__)


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")


class DetectFrontReq(BaseModel):
    class_name: Optional[str] = None
    conf: Optional[float] = None
    proxy_base_url: Optional[str] = None  # se YOLO non raggiunge la front camera


def _yolo_detect_with_fallback(image_url: str, conf: float | None,
                                proxy_image_url: str | None, sw: Stopwatch
                                ) -> dict[str, Any]:
    """Prova YOLO sull'URL diretto; se fallisce e proxy_image_url è settato, ritenta."""
    yolo = _deps.yolo()
    try:
        with sw.step("yolo"):
            return yolo.detect_url(image_url=image_url, conf=conf)
    except Exception as e:
        if not proxy_image_url:
            raise HTTPException(status_code=502, detail=f"YOLO unreachable: {e}")
        log.warning("YOLO failed on direct URL (%s), retrying via proxy %s", e, proxy_image_url)
        try:
            with sw.step("yolo_proxy"):
                return yolo.detect_url(image_url=proxy_image_url, conf=conf)
        except Exception as e2:
            raise HTTPException(status_code=502, detail=f"YOLO unreachable (proxy): {e2}")


def _top_detection(payload: dict[str, Any], class_name: str | None) -> dict[str, Any] | None:
    dets = payload.get("detections") or payload.get("data", {}).get("detections") or []
    if class_name:
        dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()]
    if not dets:
        return None
    dets.sort(key=lambda d: float(d.get("confidence", 0.0)), reverse=True)
    return dets[0]


@router.post("/detect/front")
def detect_front(req: DetectFrontReq) -> dict[str, Any]:
    sw = Stopwatch()
    cam = _deps.front_camera()
    image_url = cam.rgb_url()
    proxy_url = None
    if req.proxy_base_url:
        proxy_url = f"{req.proxy_base_url.rstrip('/')}/api/right_arm/snapshot/front"
    res = _yolo_detect_with_fallback(image_url, req.conf, proxy_url, sw)
    dets_raw = res.get("detections") or res.get("data", {}).get("detections") or []
    if req.class_name:
        dets_filtered = [d for d in dets_raw
                         if (d.get("class") or "").lower() == req.class_name.lower()]
    else:
        dets_filtered = list(dets_raw)
    if not dets_filtered:
        raise HTTPException(status_code=404, detail=f"no '{req.class_name or 'object'}' detected")
    dets_filtered.sort(key=lambda d: float(d.get("confidence", 0.0)), reverse=True)
    intr = cam.intrinsics()
    return {
        "detections": dets_filtered,
        "image_size": [int(intr.get("width", 0)), int(intr.get("height", 0))],
        "timings_ms": sw.as_dict(),
    }


class DetectWristReq(BaseModel):
    class_name: Optional[str] = None
    conf: Optional[float] = None


def _detect_wrist_core(class_name: str | None, conf: float | None, sw: Stopwatch
                       ) -> dict[str, Any]:
    """Funzione core riusata da /detect/wrist e da /move/toward."""
    nuwa = _deps.nuwa()
    image_url = nuwa.rgb_url()
    try:
        with sw.step("yolo"):
            yres = _deps.yolo().detect_url(image_url=image_url, conf=conf)
    except Exception as e:
        raise HTTPException(status_code=502, detail=f"YOLO unreachable: {e}")
    top = _top_detection(yres, class_name)
    if top is None:
        raise HTTPException(status_code=404, detail=f"no '{class_name or 'object'}' detected")
    bbox = _bbox_to_roi(top["bbox"])
    try:
        seg = segment_with_sam21(nuwa, _deps.sam21(), bbox, sw)
    except Sam21Fallback as e:
        raise HTTPException(status_code=422, detail=f"SAM21 fallback: {e}")
    pts = np.asarray(seg.points_cv_m, dtype=np.float64)
    if pts.shape[0] < 10:
        raise HTTPException(status_code=422, detail=f"too few sam21 points: {pts.shape[0]}")
    with sw.step("centroid"):
        centroid = pts.mean(axis=0)
        distance = float(np.linalg.norm(centroid))
    return {
        "detection": top,
        "bbox": list(bbox),
        "centroid_camera_m": [float(centroid[0]), float(centroid[1]), float(centroid[2])],
        "distance_m": distance,
        "n_sam21_points": int(pts.shape[0]),
    }


@router.post("/detect/wrist")
def detect_wrist(req: DetectWristReq) -> dict[str, Any]:
    sw = Stopwatch()
    body = _detect_wrist_core(req.class_name, req.conf, sw)
    body["timings_ms"] = sw.as_dict()
    return body


class MoveTowardReq(BaseModel):
    class_name: Optional[str] = None
    conf: Optional[float] = None
    dry_run: bool = True
    force: bool = False


def _guard_g1_right() -> None:
    if _deps.active_robot_key() != "g1":
        raise HTTPException(status_code=412,
                            detail=f"activate G1 right first (active={_deps.active_robot_key()})")
    if _deps.default_side() != "right":
        raise HTTPException(status_code=412,
                            detail=f"activate G1 right first (side={_deps.default_side()})")


@router.post("/move/toward")
def move_toward(req: MoveTowardReq) -> dict[str, Any]:
    sw = Stopwatch()
    _guard_g1_right()
    cfg = _deps.tab_right_arm_cfg()
    off_mm = cfg.get("move_offset_camera_mm", {"x": 0, "y": 0, "z": 0})
    sign = cfg.get("axis_sign_cam_to_wrist", {"x": 1, "y": 1, "z": 1})
    max_step_m = float(cfg.get("max_step_m", 0.15))
    z_floor = cfg.get("z_floor_m")
    z_floor_m = float(z_floor) if z_floor is not None else None

    det = _detect_wrist_core(req.class_name, req.conf, sw)
    centroid = np.asarray(det["centroid_camera_m"], dtype=np.float64)
    offset_m = np.array([off_mm["x"], off_mm["y"], off_mm["z"]], dtype=np.float64) / 1000.0
    signs = (int(sign["x"]), int(sign["y"]), int(sign["z"]))

    try:
        delta = compute_delta_wrist(centroid, offset_m, signs,
                                    max_step_m=max_step_m, force=req.force)
    except DeltaOversize as e:
        raise HTTPException(status_code=422, detail=str(e))

    # Leggi posa polso PRIMA del move per applicare z-floor
    try:
        with sw.step("g1_wrist_before"):
            wrist_before = _deps.g1().wrist()["right_wrist"]
    except G1Unavailable as e:
        raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")

    delta, z_clipped = apply_z_floor(float(wrist_before["position"][2]), delta, z_floor_m)

    try:
        with sw.step("wrist_relative"):
            _deps.g1(dry_run=req.dry_run).wrist_relative(
                side="right", delta_position=delta.tolist(),
            )
    except G1Unavailable as e:
        raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")

    wrist_after: dict[str, Any] | None = None
    if not req.dry_run:
        try:
            with sw.step("g1_wrist_after"):
                wrist_after = _deps.g1().wrist()["right_wrist"]
        except G1Unavailable:
            wrist_after = None  # best-effort; il move è stato già inviato

    log.info(
        "move_toward class=%s conf=%s centroid=%s offset=%s sign=%s delta=%s dry_run=%s applied=%s",
        req.class_name, req.conf, centroid.tolist(),
        offset_m.tolist(), signs, delta.tolist(), req.dry_run, not req.dry_run,
    )

    return {
        "detection": det["detection"],
        "centroid_camera_m": det["centroid_camera_m"],
        "distance_m": det["distance_m"],
        "offset_camera_m": offset_m.tolist(),
        "axis_signs": list(signs),
        "delta_wrist_m": delta.tolist(),
        "delta_wrist_mm": (delta * 1000.0).tolist(),
        "z_clipped": bool(z_clipped),
        "wrist_before": wrist_before,
        "wrist_after": wrist_after,
        "applied": not req.dry_run,
        "n_sam21_points": det["n_sam21_points"],
        "timings_ms": sw.as_dict(),
    }


class PoseExecReq(BaseModel):
    dry_run: bool = False


class PoseUpdateReq(BaseModel):
    position: Optional[list[float]] = None
    rpy_deg: Optional[list[float]] = None


class OffsetUpdateReq(BaseModel):
    move_offset_camera_mm: Optional[dict[str, float]] = None
    axis_sign_cam_to_wrist: Optional[dict[str, int]] = None


def _exec_named_pose(pose_key: str, dry_run: bool, sw: Stopwatch) -> dict[str, Any]:
    _guard_g1_right()
    cfg = _deps.tab_right_arm_cfg()
    pose = cfg.get(pose_key) or {}
    pos = pose.get("position")
    rpy = pose.get("rpy_deg")
    if not (pos and rpy and len(pos) == 3 and len(rpy) == 3):
        raise HTTPException(status_code=422, detail=f"{pose_key} malformed in config")
    quat = rpy_deg_to_quaternion_xyzw(rpy[0], rpy[1], rpy[2])
    try:
        with sw.step("wrist_absolute"):
            res = _deps.g1(dry_run=dry_run).wrist_absolute(
                side="right", position=list(pos), quaternion=quat.tolist(),
            )
    except G1Unavailable as e:
        raise HTTPException(status_code=502, detail=f"G1 unavailable: {e}")
    return {"pose_key": pose_key, "position": pos, "rpy_deg": rpy,
            "quaternion": quat.tolist(), "result": res, "dry_run": dry_run,
            "applied": not dry_run, "timings_ms": sw.as_dict()}


@router.post("/home")
def post_home(req: PoseExecReq) -> dict[str, Any]:
    return _exec_named_pose("home_pose", req.dry_run, Stopwatch())


@router.post("/custom_pose")
def post_custom_pose(req: PoseExecReq) -> dict[str, Any]:
    return _exec_named_pose("custom_pose", req.dry_run, Stopwatch())


@router.put("/custom_pose")
def put_custom_pose(req: PoseUpdateReq) -> dict[str, Any]:
    patch: dict[str, Any] = {}
    if req.position is not None:
        if len(req.position) != 3:
            raise HTTPException(status_code=422, detail="position must be 3 floats")
        patch["position"] = list(req.position)
    if req.rpy_deg is not None:
        if len(req.rpy_deg) != 3:
            raise HTTPException(status_code=422, detail="rpy_deg must be 3 floats")
        patch["rpy_deg"] = list(req.rpy_deg)
    if not patch:
        raise HTTPException(status_code=422, detail="nothing to update")
    update_config({"tab_right_arm": {"custom_pose": patch}})
    return {"custom_pose": _deps.tab_right_arm_cfg().get("custom_pose")}


@router.put("/offset")
def put_offset(req: OffsetUpdateReq) -> dict[str, Any]:
    patch: dict[str, Any] = {}
    if req.move_offset_camera_mm is not None:
        for k in ("x", "y", "z"):
            if k not in req.move_offset_camera_mm:
                raise HTTPException(status_code=422, detail=f"move_offset_camera_mm missing '{k}'")
        patch["move_offset_camera_mm"] = {k: float(req.move_offset_camera_mm[k]) for k in ("x", "y", "z")}
    if req.axis_sign_cam_to_wrist is not None:
        for k in ("x", "y", "z"):
            v = req.axis_sign_cam_to_wrist.get(k)
            if v not in (-1, 1):
                raise HTTPException(status_code=422, detail=f"axis_sign_cam_to_wrist['{k}'] must be +/-1")
        patch["axis_sign_cam_to_wrist"] = {k: int(req.axis_sign_cam_to_wrist[k]) for k in ("x", "y", "z")}
    if not patch:
        raise HTTPException(status_code=422, detail="nothing to update")
    update_config({"tab_right_arm": patch})
    cfg = _deps.tab_right_arm_cfg()
    return {"move_offset_camera_mm": cfg.get("move_offset_camera_mm"),
            "axis_sign_cam_to_wrist": cfg.get("axis_sign_cam_to_wrist")}