"""Dora node for voice control with safe robot commands."""

from __future__ import annotations

import json
import os
import sys
import time
import uuid
from collections import deque
from typing import Any, Deque, Dict, List, Optional, Tuple

import cv2
import numpy as np
import pyarrow as pa
from dora import Node

try:
    import tomllib
except ModuleNotFoundError:
    import tomli as tomllib

# Handle both package and direct script execution
# __package__ is None when run as script, '' when imported from a script
_RUNNING_AS_SCRIPT = not __package__

if _RUNNING_AS_SCRIPT:
    # Running as script - use absolute imports
    _pkg_dir = os.path.dirname(os.path.abspath(__file__))
    if _pkg_dir not in sys.path:
        sys.path.insert(0, _pkg_dir)
    from config import VoiceConfig, load_api_config, load_voice_config
    from parser import normalize, parse_command
    from state import RobotStep, SharedState
    from api import start_api_server
else:
    # Running as package - use relative imports
    from .config import VoiceConfig, load_api_config, load_voice_config
    from .parser import normalize, parse_command
    from .state import RobotStep, SharedState
    from .api import start_api_server


def _within_bounds(
    point_mm: np.ndarray,
    min_xyz: Tuple[Optional[float], Optional[float], Optional[float]],
    max_xyz: Tuple[Optional[float], Optional[float], Optional[float]],
) -> bool:
    """Check if point is within workspace bounds."""
    x, y, z = point_mm.tolist()
    min_x, min_y, min_z = min_xyz
    max_x, max_y, max_z = max_xyz
    if min_x is not None and x < min_x:
        return False
    if max_x is not None and x > max_x:
        return False
    if min_y is not None and y < min_y:
        return False
    if max_y is not None and y > max_y:
        return False
    if min_z is not None and z < min_z:
        return False
    if max_z is not None and z > max_z:
        return False
    return True


def _translate_target(token: str, mapping: Dict[str, str]) -> str:
    """Translate object name using class map."""
    if token in mapping:
        return mapping[token]
    return token


def _load_config_file(path: str) -> Dict[str, Any]:
    """Load TOML configuration file."""
    if not path or not os.path.exists(path):
        return {}
    try:
        with open(path, "rb") as handle:
            return tomllib.load(handle)
    except Exception:
        return {}


def _load_bucket_objects(config_path: str) -> List[Dict[str, Any]]:
    """Load bucket positions from config file."""
    cfg = _load_config_file(config_path)
    buckets = cfg.get("bucket_positions", {})
    obj_cfg = cfg.get("object_parameters", {})
    base_z = float(obj_cfg.get("normal_height", 220.0))
    out = []
    for key, color in [
        ("blue_bucket_pos", "blue"),
        ("red_bucket_pos", "red"),
        ("yellow_bucket_pos", "yellow"),
        ("white_bucket_pos", "white"),
    ]:
        pos = buckets.get(key)
        if not isinstance(pos, list) or len(pos) < 2:
            continue
        out.append(
            {
                "object_type": "box",
                "color": color,
                "size": "big",
                "position_mm": [float(pos[0]), float(pos[1]), base_z],
                "source": "config",
            }
        )
    return out


def _send_dora_command(
    node: Node, output_name: str, action: str, payload: Dict[str, Any]
) -> str:
    """Send a robot command via Dora."""
    command_id = str(uuid.uuid4())
    message = {"id": command_id, "action": action, "payload": payload}
    node.send_output(
        output_name,
        pa.array([json.dumps(message)]),
        metadata={"encoding": "json", "timestamp_ns": time.time_ns()},
    )
    return command_id


def _parse_status_payload(value: pa.Array) -> Optional[Dict[str, Any]]:
    """Parse status payload from robot."""
    if len(value) == 0:
        return None
    raw = value[0].as_py()
    if not raw:
        return None
    try:
        return json.loads(raw)
    except Exception:
        return None


def _log(msg: str) -> None:
    """Print a timestamped log message."""
    timestamp = time.strftime("%H:%M:%S")
    print(f"[voice_control {timestamp}] {msg}", flush=True)


def main() -> None:
    """Main entry point for the voice control node."""
    _log("Starting voice control node...")

    # Load configuration
    cfg = load_voice_config()
    api_cfg = load_api_config()

    # Environment variables for I/O topics
    objects_input = os.getenv("OBJECTS_INPUT", "objects")
    voice_in_input = os.getenv("VOICE_IN_INPUT", "voice_in")
    voice_out_output = os.getenv("VOICE_OUT_OUTPUT", "voice_out")
    scene_output = os.getenv("SCENE_OUTPUT", "scene_update")
    pose_input = os.getenv("POSE_INPUT", "tcp_pose")
    status_input = os.getenv("STATUS_INPUT", "status")
    command_output = os.getenv("COMMAND_OUTPUT", "robot_cmd")
    image_input = os.getenv("IMAGE_INPUT", "image_annotated")
    llm_provider = os.getenv("LLM_PROVIDER", "rules").lower()
    config_file = os.getenv("CONFIG_FILE", "config.toml")

    # Image dimensions (will be detected from first frame)
    image_width = int(os.getenv("IMAGE_WIDTH", "1280"))
    image_height = int(os.getenv("IMAGE_HEIGHT", "720"))

    # Initial/home position for reset command
    init_x = float(os.getenv("INIT_X", "300.0"))
    init_y = float(os.getenv("INIT_Y", "0.0"))
    init_z = float(os.getenv("INIT_Z", "250.0"))
    init_roll = float(os.getenv("INIT_ROLL", "180.0"))
    init_pitch = float(os.getenv("INIT_PITCH", "0.0"))
    init_yaw = float(os.getenv("INIT_YAW", "0.0"))

    _log(f"Config: tcp_offset={cfg.tcp_offset_mm}mm, approach_offset={cfg.approach_offset_mm}mm, step={cfg.step_mm}mm")
    _log(f"Initial position: [{init_x}, {init_y}, {init_z}]")
    _log(f"LLM provider: {llm_provider}")
    _log(f"Dry run: {cfg.dry_run}")

    # Initialize shared state
    shared_state = SharedState()
    state = shared_state.voice_state
    state.static_objects = _load_bucket_objects(config_file)
    pending_intents: Deque[Dict[str, Any]] = deque()

    _log(f"Loaded {len(state.static_objects)} static objects from config")

    # Queue initial position movement on startup (same as reiniciar)
    init_on_start = os.getenv("INIT_ON_START", "true").lower() in ("true", "1", "yes")
    send_init_scene_reset = init_on_start  # Flag to send scene reset after node starts
    if init_on_start:
        _log(f"Startup: resetting scene and moving to home [{init_x}, {init_y}, {init_z}]")
        # Clear detected objects
        state.latest_objects = []
        state.latest_objects_at = None
        # Queue vacuum off and move to home
        state.queue.append(RobotStep(action="vacuum_off", payload={}))
        state.queue.append(
            RobotStep(
                action="move_to_pose",
                payload={
                    "x": init_x,
                    "y": init_y,
                    "z": init_z,
                    "roll": init_roll,
                    "pitch": init_pitch,
                    "yaw": init_yaw,
                },
            )
        )

    def command_handler(transcript: str) -> Dict[str, str]:
        """Handle voice command and return response."""
        _log(f"Voice input received: \"{transcript}\"")
        llm_result = parse_command(transcript, llm_provider)
        _log(f"Parse result: {llm_result}")

        # Update debug state
        shared_state.update_voice_input(transcript, llm_result, time.monotonic())

        if llm_result.get("resultado") != "ok":
            _log("Command not understood")
            return {"text": "No entendi el comando", "status": "error"}

        action = llm_result.get("accion", "error")
        obj = llm_result.get("objeto", "no especificado")
        color = llm_result.get("color", "no especificado")
        size = llm_result.get("tamano", "no especificado")

        _log(f"Intent: action={action}, object={obj}, color={color}, size={size}")

        pending_intents.append(
            {"action": action, "obj": obj, "color": color, "size": size}
        )

        # Add to history
        shared_state.add_to_history({
            "timestamp": time.time(),
            "input": transcript,
            "action": action,
            "object": obj,
            "color": color,
            "size": size,
        })

        return {"text": f"Ok, voy a {action}", "status": "ok"}

    # Set command callback for web interface
    shared_state.set_command_callback(command_handler)

    # Start web API server if enabled
    if api_cfg.enabled:
        start_api_server(shared_state, api_cfg)

    # Create Dora node
    node = Node()
    _log("Dora node created, waiting for events...")

    first_event = True
    for event in node:
        # Send scene reset on first event (startup)
        if first_event and send_init_scene_reset:
            first_event = False
            scene_payload = json.dumps(
                {"objects": list(state.static_objects), "reset": True}
            )
            node.send_output(
                scene_output,
                pa.array([scene_payload]),
                metadata={"encoding": "json", "timestamp_ns": time.time_ns()},
            )
            _log("Sent initial scene reset notification")
        if event["type"] != "INPUT":
            continue

        # Handle voice input
        if event["id"] == voice_in_input:
            raw = event["value"][0].as_py() if len(event["value"]) else ""
            if not raw:
                continue
            response = command_handler(raw)
            node.send_output(
                voice_out_output,
                pa.array([json.dumps(response)]),
                metadata={"encoding": "json", "timestamp_ns": time.time_ns()},
            )
            continue

        # Handle pose updates
        if event["id"] == pose_input:
            tcp_pose = event["value"].to_numpy().astype(np.float64).reshape(-1)
            if tcp_pose.size >= 6:
                state.latest_pose = tcp_pose[:6].tolist()
                state.latest_pose_at = time.monotonic()
            continue

        # Handle object detection updates
        if event["id"] == objects_input:
            raw = event["value"][0].as_py() if len(event["value"]) else ""
            if raw:
                try:
                    payload = json.loads(raw)
                    objects = payload.get("objects", [])
                except Exception:
                    objects = []
                state.latest_objects = objects
                state.latest_objects_at = time.monotonic()
            continue

        # Handle camera image
        if event["id"] == image_input:
            try:
                # Get raw image data
                img_data = event["value"].to_numpy()
                # Reshape to image (assuming BGR format)
                img = img_data.reshape((image_height, image_width, 3)).astype(np.uint8)
                # Encode to JPEG
                _, jpeg_data = cv2.imencode(".jpg", img, [cv2.IMWRITE_JPEG_QUALITY, 80])
                shared_state.update_image(jpeg_data.tobytes(), time.monotonic())
            except Exception as e:
                # Log error but don't crash
                pass
            continue

        # Handle robot status updates
        if event["id"] == status_input:
            payload = _parse_status_payload(event["value"])
            if payload and state.pending_command:
                if payload.get("command_id") == state.pending_command.get("id"):
                    _log(f"Command completed: {state.pending_command.get('action')} (status={payload.get('status', 'ok')})")
                    state.pending_command = None
            continue

        # Process pending intents
        if pending_intents:
            intent = pending_intents.popleft()
            action = intent["action"]
            obj = intent["obj"]
            color = intent["color"]
            size = intent["size"]

            _log(f"Processing intent: {action} {obj} {color} {size}")

            latest_pose = state.latest_pose
            objects = list(state.latest_objects) + list(state.static_objects)
            _log(f"Available objects: {len(state.latest_objects)} detected + {len(state.static_objects)} static")

            if action in ("subir", "bajar") and latest_pose:
                delta = cfg.step_mm if action == "subir" else -cfg.step_mm
                target = np.array(latest_pose[:3], dtype=np.float64)
                target[2] += delta
                if _within_bounds(target, cfg.workspace_min, cfg.workspace_max):
                    step = RobotStep(
                        action="move_to_pose",
                        payload={
                            "x": float(target[0]),
                            "y": float(target[1]),
                            "z": float(target[2]),
                            "roll": cfg.default_roll,
                            "pitch": cfg.default_pitch,
                            "yaw": cfg.default_yaw,
                        },
                    )
                    state.queue.append(step)
                    _log(f"Queued: move Z to {target[2]:.1f}mm (delta={delta:+.1f})")
                else:
                    _log(f"Target {target.tolist()} out of bounds, skipping")

            elif action in ("ir", "tomar", "soltar"):
                target_obj = None
                if obj != "no especificado":
                    target_name = _translate_target(obj, cfg.class_map)
                    target_color = _translate_target(color, cfg.class_map)
                    _log(f"Looking for: type={target_name}, color={target_color}")
                    # Log available objects for debugging
                    for o in objects:
                        _log(f"  -> Available: {o.get('object_type')} {o.get('color')} {o.get('size')} at {o.get('position_mm')}")
                    for o in objects:
                        if o.get("object_type") == target_name:
                            if color == "no especificado" or o.get("color") == target_color:
                                if size == "no especificado" or o.get("size") == _translate_target(size, cfg.class_map):
                                    target_obj = o
                                    break
                if target_obj:
                    _log(f"Found target: {target_obj.get('object_type')} {target_obj.get('color')} at {target_obj.get('position_mm')}")
                    pos = np.array(target_obj["position_mm"], dtype=np.float64)
                    approach = pos.copy()
                    approach[2] += cfg.tcp_offset_mm + cfg.approach_offset_mm
                    target = pos.copy()
                    target[2] += cfg.tcp_offset_mm
                    if _within_bounds(approach, cfg.workspace_min, cfg.workspace_max):
                        state.queue.append(
                            RobotStep(
                                action="move_to_pose",
                                payload={
                                    "x": float(approach[0]),
                                    "y": float(approach[1]),
                                    "z": float(approach[2]),
                                    "roll": cfg.default_roll,
                                    "pitch": cfg.default_pitch,
                                    "yaw": cfg.default_yaw,
                                },
                            )
                        )
                        _log(f"Queued: approach pose at Z={approach[2]:.1f}mm")
                    if _within_bounds(target, cfg.workspace_min, cfg.workspace_max):
                        state.queue.append(
                            RobotStep(
                                action="move_to_pose",
                                payload={
                                    "x": float(target[0]),
                                    "y": float(target[1]),
                                    "z": float(target[2]),
                                    "roll": cfg.default_roll,
                                    "pitch": cfg.default_pitch,
                                    "yaw": cfg.default_yaw,
                                },
                            )
                        )
                        _log(f"Queued: target pose at Z={target[2]:.1f}mm")
                    if action == "tomar":
                        state.queue.append(RobotStep(action="vacuum_on", payload={}))
                        _log("Queued: vacuum_on")
                    elif action == "soltar":
                        state.queue.append(RobotStep(action="vacuum_off", payload={}))
                        _log("Queued: vacuum_off")
                else:
                    _log(f"Target object not found: {obj} {color}")
                    continue

            elif action == "reiniciar":
                _log(f"Reiniciar: resetting scene and moving to home [{init_x}, {init_y}, {init_z}]")
                # Turn off vacuum first
                state.queue.append(RobotStep(action="vacuum_off", payload={}))
                # Clear current detected objects (will be refreshed by detector)
                state.latest_objects = []
                state.latest_objects_at = None
                _log("Cleared detected objects - waiting for fresh detection")
                # Move to initial position
                state.queue.append(
                    RobotStep(
                        action="move_to_pose",
                        payload={
                            "x": init_x,
                            "y": init_y,
                            "z": init_z,
                            "roll": init_roll,
                            "pitch": init_pitch,
                            "yaw": init_yaw,
                        },
                    )
                )
                _log(f"Queued: vacuum_off + move to home")
                # Send scene update to notify clients that scene was reset
                scene_payload = json.dumps(
                    {"objects": list(state.static_objects), "reset": True}
                )
                node.send_output(
                    scene_output,
                    pa.array([scene_payload]),
                    metadata={"encoding": "json", "timestamp_ns": time.time_ns()},
                )
                _log("Sent scene reset notification")

            _log(f"Queue size: {len(state.queue)}")

        # Emit scene updates when objects change
        if event["id"] == objects_input:
            scene_payload = json.dumps(
                {"objects": list(state.latest_objects) + list(state.static_objects)}
            )
            node.send_output(
                scene_output,
                pa.array([scene_payload]),
                metadata={"encoding": "json", "timestamp_ns": time.time_ns()},
            )

        # Send queued robot steps one at a time
        if state.pending_command is None and state.queue:
            step = state.queue.popleft()
            if cfg.dry_run:
                _log(f"[DRY RUN] Would send: {step.action} {step.payload}")
                state.pending_command = None
                continue
            cmd_id = _send_dora_command(node, command_output, step.action, step.payload)
            state.pending_command = {"id": cmd_id, "action": step.action}
            _log(f"Sent command: {step.action} (id={cmd_id[:8]}...) remaining={len(state.queue)}")

            # Update debug state
            shared_state.update_robot_command(
                {"id": cmd_id, "action": step.action, "payload": step.payload},
                time.monotonic(),
            )


if __name__ == "__main__":
    main()
