"""Behavior for the Littlehand robot."""

from __future__ import annotations

import os
from typing import Callable, Optional

from ...core.behavior import ActionContext, RobotBehavior, _log
from .actions import LITTLEHAND_ACTIONS
from .signature import LittlehandSignature

_XY_MATCH_RADIUS_MM = float(os.getenv("BAJAR_XY_RADIUS_MM", "40.0"))

class LittlehandBehavior(RobotBehavior):
    """Littlehand behavior using the default pick-and-place actions."""

    ACTIONS = LITTLEHAND_ACTIONS
    CommandSignature = LittlehandSignature

    def action_handlers(self) -> dict[str, Callable[[ActionContext], bool]]:
        return {
            "subir": self.action_subir,
            "bajar": self.action_bajar,
            "ir": self.action_ir,
            "tomar": self.action_tomar,
            "soltar": self.action_soltar,
            "reiniciar": self.action_reiniciar,
        }

    def action_subir(self, ctx: ActionContext) -> bool:
        """Move to the home height (initial Z)."""
        if ctx.pose is None:
            return False
        target_z = ctx.home_pose.get("z", ctx.pose[2])
        return self._queue_move(ctx, ctx.pose[0], ctx.pose[1], target_z)

    def action_bajar(self, ctx: ActionContext) -> bool:
        """Move down by step_mm or to top of object under the tool."""
        target = self._find_object_under_pose(ctx)
        if target is not None:
            target_z = target.position_mm[2] + ctx.config.tcp_offset_mm
            _log(
                f"bajar: using object '{target.object_type}' color={target.color} "
                f"obj_z={target.position_mm[2]:.1f} tcp_offset={ctx.config.tcp_offset_mm:.1f} "
                f"target_z={target_z:.1f} at pose_z={ctx.pose[2]:.1f}"
            )
            return self._queue_move(ctx, ctx.pose[0], ctx.pose[1], target_z)
        target_z = ctx.pose[2] - self.config.step_mm
        _log(f"bajar: no object under pose, step to z={target_z:.1f}")
        return self._queue_move(ctx, ctx.pose[0], ctx.pose[1], target_z)

    def action_ir(self, ctx: ActionContext) -> bool:
        """Move to object X/Y while keeping current Z."""
        if ctx.pose is None or ctx.target is None:
            return False
        pos = ctx.target.position_mm
        return self._queue_move(ctx, pos[0], pos[1], ctx.pose[2])

    def action_tomar(self, ctx: ActionContext) -> bool:
        """Activate tool (low-level grab)."""
        self._queue_steps(ctx, self.robot_adapter.grab())
        return True

    def action_soltar(self, ctx: ActionContext) -> bool:
        """Deactivate tool (low-level release)."""
        self._queue_steps(ctx, self.robot_adapter.release())
        return True

    def action_reiniciar(self, ctx: ActionContext) -> bool:
        """Reset: release tool, move home, clear objects."""
        self._queue_steps(ctx, self.robot_adapter.reset_tool())
        self._queue_steps(ctx, self.robot_adapter.move(ctx.home_pose))
        ctx.scene.clear_detected()
        return True

    def _find_object_under_pose(self, ctx: ActionContext) -> Optional["SceneObject"]:
        """Find the topmost object near the current pose x,y (mm)."""
        if ctx.pose is None:
            _log("bajar: missing pose, cannot find object under tool")
            return None
        px, py, pz = ctx.pose[0], ctx.pose[1], ctx.pose[2]
        _log(
            "bajar: current pose x={:.1f} y={:.1f} z={:.1f}".format(px, py, pz)
        )
        candidates = []
        for obj in ctx.scene.query():
            dx = px - obj.position_mm[0]
            dy = py - obj.position_mm[1]
            dist2 = dx * dx + dy * dy
            if dist2 > _XY_MATCH_RADIUS_MM * _XY_MATCH_RADIUS_MM:
                continue
            top_surface = obj.position_mm[2] + obj.height_mm
            candidates.append((top_surface, obj))
            _log(
                "bajar: near id={} type={} color={} center=({:.1f},{:.1f}) "
                "dist_xy={:.1f} obj_z={:.1f} height={:.1f} top_z={:.1f}".format(
                    obj.id,
                    obj.object_type,
                    obj.color,
                    obj.position_mm[0],
                    obj.position_mm[1],
                    (dist2 ** 0.5),
                    obj.position_mm[2],
                    obj.height_mm,
                    top_surface,
                )
            )
        if not candidates:
            _log(f"bajar: no objects within radius={_XY_MATCH_RADIUS_MM:.1f}mm")
            return None
        candidates.sort(key=lambda item: item[0], reverse=True)
        return candidates[0][1]
