"""Robot I/O handlers and command queue service."""

from __future__ import annotations

import json
import os
import time
from typing import Any, Dict, Optional, TYPE_CHECKING

import cv2
import numpy as np
import pyarrow as pa

from .config import JPEG_QUALITY

if TYPE_CHECKING:
    from .config import VoiceConfig
    from .state import SharedState
    from .node import OutputPort, VoiceControlLogger


class CommandQueueService:
    """Dispatches robot commands from the queue."""

    def __init__(
        self,
        state: "SharedState",
        port: "OutputPort",
        config: "VoiceConfig",
        logger: "VoiceControlLogger",
    ) -> None:
        self._state = state
        self._port = port
        self._config = config
        self._logger = logger

    def dispatch_next(self) -> bool:
        """Dispatch the next command in the queue if possible.

        Returns True if a command was dispatched, False otherwise.
        """
        # Don't dispatch if there's a pending command
        if self._state.get_pending_command() is not None:
            return False

        # Don't dispatch if queue is empty
        if self._state.queue_size() == 0:
            return False

        step = self._state.pop_queue()
        if step is None:
            return False

        # Handle dry run mode
        if self._config.dry_run:
            self._logger.log(f"[DRY RUN] Would send: {step.action} {step.payload}")
            self._state.set_pending_command(None)
            return True

        # Send command
        cmd_id = self._port.send_command(step.action, step.payload)
        self._state.set_pending_command({"id": cmd_id, "action": step.action})
        self._logger.log(f"Sent command: {step.action} (id={cmd_id[:8]}...) remaining={self._state.queue_size()}")

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

        return True


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


class StatusHandler:
    """Handles status events from the robot."""

    def __init__(
        self,
        state: "SharedState",
        logger: "VoiceControlLogger",
    ) -> None:
        self._state = state
        self._logger = logger

    def handle(self, event: Dict[str, Any]) -> None:
        """Handle a status event."""
        payload = _parse_status_payload(event["value"])
        pending = self._state.get_pending_command()

        if payload and pending:
            if payload.get("command_id") == pending.get("id"):
                self._logger.log(
                    f"Command completed: {pending.get('action')} (status={payload.get('status', 'ok')})"
                )
                self._state.set_pending_command(None)


class PoseHandler:
    """Handles tcp_pose events from the Dora node."""

    def __init__(
        self,
        state: "SharedState",
        logger: "VoiceControlLogger",
    ) -> None:
        self._state = state
        self._logger = logger

    def handle(self, event: Dict[str, Any]) -> None:
        """Handle a tcp_pose event."""
        tcp_pose = event["value"].to_numpy().astype(np.float64).reshape(-1)
        if tcp_pose.size >= 6:
            self._state.update_pose(tcp_pose[:6].tolist(), time.monotonic())


class ImageHandler:
    """Handles camera image events from the Dora node."""

    def __init__(
        self,
        state: "SharedState",
        logger: "VoiceControlLogger",
        image_width: Optional[int] = None,
        image_height: Optional[int] = None,
    ) -> None:
        self._state = state
        self._logger = logger
        self._image_width = image_width or int(os.getenv("IMAGE_WIDTH", "1280"))
        self._image_height = image_height or int(os.getenv("IMAGE_HEIGHT", "720"))

    def handle(self, event: Dict[str, Any]) -> None:
        """Handle an image event."""
        try:
            # Get raw image data
            img_data = event["value"].to_numpy()
            # Reshape to image (assuming BGR format)
            img = img_data.reshape((self._image_height, self._image_width, 3)).astype(np.uint8)
            # Encode to JPEG
            _, jpeg_data = cv2.imencode(".jpg", img, [cv2.IMWRITE_JPEG_QUALITY, JPEG_QUALITY])
            self._state.update_image(jpeg_data.tobytes(), time.monotonic())
        except Exception as e:
            self._logger.warn(f"failed to process camera image: {e}")
