#!/usr/bin/env python3
"""Face display bridge node.

Maps orchestrator state to face expressions via HTTP WebSocket API.
Bridges /social/orchestrator/state and /saltybot/wake_word_detected to
face display server (localhost:3000/face/{id}).

State mapping:
  IDLE → 0 (Tracking)
  LISTENING → 1 (Alert)
  THINKING → 3 (Searching)
  SPEAKING → 4 (Social)
  Wake word → 1 (Alert) [immediate override]

Subscribed topics:
  /social/orchestrator/state (String) - JSON: {"state": "IDLE|LISTENING|THINKING|SPEAKING|THROTTLED"}
  /saltybot/wake_word_detected (Bool) - Wake word detection trigger

Published topics:
  /face/state (String) - Current face expression ID and status
"""

import json
import threading
import time
from typing import Optional

import rclpy
from rclpy.node import Node
from std_msgs.msg import String, Bool


class FaceDisplayBridge(Node):
    """Bridge orchestrator state to face display expressions."""

    # State to face expression ID mapping
    STATE_TO_FACE_ID = {
        "IDLE": 0,         # Tracking
        "LISTENING": 1,    # Alert
        "THINKING": 3,     # Searching
        "SPEAKING": 4,     # Social
        "THROTTLED": 0,    # Fallback to Tracking
    }

    def __init__(self):
        super().__init__("face_bridge")

        # Parameters
        self.declare_parameter("face_server_url", "http://localhost:3000/face/1")
        self.declare_parameter("http_timeout", 2.0)
        self.declare_parameter("update_interval", 0.1)

        self.face_server_url = self.get_parameter("face_server_url").value
        self.http_timeout = self.get_parameter("http_timeout").value
        self.update_interval = self.get_parameter("update_interval").value

        # Try to import requests, fallback to urllib if unavailable
        try:
            import requests
            self.requests = requests
            self.use_requests = True
        except ImportError:
            import urllib.request
            import urllib.error
            self.urllib = urllib.request
            self.urllib_error = urllib.error
            self.use_requests = False

        # State
        self.current_state = "IDLE"
        self.current_face_id = 0
        self.wake_word_active = False
        self.last_update_time = time.time()
        self.state_lock = threading.Lock()

        # Subscriptions
        self.create_subscription(String, "/social/orchestrator/state", self._on_state_update, 10)
        self.create_subscription(Bool, "/saltybot/wake_word_detected", self._on_wake_word, 10)

        # Publishers
        self.pub_state = self.create_publisher(String, "/face/state", 10)

        # Timer for update loop
        self.create_timer(self.update_interval, self._update_face)

        self.get_logger().info(
            f"Face bridge initialized: face_server_url={self.face_server_url}"
        )

    def _on_state_update(self, msg: String) -> None:
        """Handle orchestrator state update."""
        try:
            data = json.loads(msg.data)
            new_state = data.get("state", "IDLE").upper()

            # Validate state
            if new_state in self.STATE_TO_FACE_ID:
                with self.state_lock:
                    self.current_state = new_state
                self.get_logger().debug(f"State updated: {new_state}")
            else:
                self.get_logger().warn(f"Unknown state: {new_state}")
        except json.JSONDecodeError:
            self.get_logger().error(f"Invalid JSON in state update: {msg.data}")

    def _on_wake_word(self, msg: Bool) -> None:
        """Handle wake word detection - immediate switch to Alert."""
        if msg.data:
            with self.state_lock:
                self.wake_word_active = True
            self.get_logger().info("Wake word detected - switching to Alert")

    def _get_face_id(self) -> int:
        """Get current face expression ID based on state."""
        with self.state_lock:
            if self.wake_word_active:
                face_id = 1  # Alert
                # Clear wake word after one update
                self.wake_word_active = False
            else:
                face_id = self.STATE_TO_FACE_ID.get(self.current_state, 0)

        return face_id

    def _send_face_command(self, face_id: int) -> bool:
        """Send face expression command to display server.

        Args:
            face_id: Expression ID (0-4)

        Returns:
            True if successful
        """
        try:
            if self.use_requests:
                response = self.requests.get(
                    self.face_server_url.format(id=face_id),
                    timeout=self.http_timeout
                )
                return response.status_code == 200
            else:
                url = self.face_server_url.format(id=face_id)
                req = self.urllib.Request(url)
                with self.urllib.urlopen(req, timeout=self.http_timeout) as response:
                    return response.status == 200
        except Exception as e:
            self.get_logger().error(f"Failed to update face display: {e}")
            return False

    def _update_face(self) -> None:
        """Update face expression based on current state."""
        face_id = self._get_face_id()

        # Only send if changed
        if face_id != self.current_face_id:
            if self._send_face_command(face_id):
                self.current_face_id = face_id
                self.last_update_time = time.time()

                # Publish state
                with self.state_lock:
                    state_msg = String(
                        data=json.dumps({
                            "face_id": face_id,
                            "orchestrator_state": self.current_state,
                            "timestamp": self.last_update_time
                        })
                    )
                self.pub_state.publish(state_msg)
                self.get_logger().debug(f"Face updated: {face_id}")


def main(args=None):
    rclpy.init(args=args)
    node = FaceDisplayBridge()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == "__main__":
    main()
