"""Dora node for voice control - thin orchestrator."""

from __future__ import annotations

import os

from dora import Node

from dora_voice_control.core import (
    load_voice_config,
    load_llm_config,
    load_api_config,
    SharedState,
    ActionContext,
    VoiceInputHandler,
    IntentQueue,
    ObjectsHandler,
    SceneNotifier,
    SceneState,
    PoseHandler,
    StatusHandler,
    CommandQueueService,
    ImageHandler,
    DoraNodeAdapter,
    EventDispatcher,
    VoiceControlLogger,
    NodeContext,
)
from dora_voice_control.web import start_api_server
from dora_voice_control.robots import get_behavior, get_robot_adapter


def main() -> None:
    """Main entry point for the voice control node."""
    logger = VoiceControlLogger()
    logger.log("Starting voice control node...")

    # Configuration
    cfg = load_voice_config()
    llm_cfg = load_llm_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")
    pose_input = os.getenv("POSE_INPUT", "tcp_pose")
    status_input = os.getenv("STATUS_INPUT", "status")
    image_input = os.getenv("IMAGE_INPUT", "image_annotated")

    # Home position
    home_pose = {
        "x": float(os.getenv("INIT_X", "300.0")),
        "y": float(os.getenv("INIT_Y", "0.0")),
        "z": float(os.getenv("INIT_Z", "250.0")),
        "roll": float(os.getenv("INIT_ROLL", "180.0")),
        "pitch": float(os.getenv("INIT_PITCH", "0.0")),
        "yaw": float(os.getenv("INIT_YAW", "0.0")),
    }

    # State
    state = SharedState()
    scene = SceneState()
    loaded = scene.load_from_config()
    logger.log(f"Loaded {loaded} static objects from config")

    # Robot adapter and behavior
    robot_type = os.getenv("ROBOT_TYPE", "vacuum").lower()
    robot_adapter = get_robot_adapter(robot_type, logger)
    behavior = get_behavior(robot_type, cfg, llm_cfg)
    logger.log(f"Robot type: {robot_type} ({type(robot_adapter).__name__})")
    logger.log(f"Available actions: {list(behavior.get_all_keywords().keys())}")
    logger.log(f"Config: tcp_offset={cfg.tcp_offset_mm}mm, approach_offset={cfg.approach_offset_mm}mm, step={cfg.step_mm}mm")
    logger.log(f"Initial position: [{home_pose['x']}, {home_pose['y']}, {home_pose['z']}]")
    logger.log(f"LLM provider: {llm_cfg.provider}/{llm_cfg.model}")
    logger.log(f"Dry run: {cfg.dry_run}")

    # Node context with output ports
    ctx = NodeContext.create(
        adapter=DoraNodeAdapter(Node()),
        logger=logger,
        robot_output=os.getenv("COMMAND_OUTPUT", "robot_cmd"),
        scene_output=os.getenv("SCENE_OUTPUT", "scene_update"),
        voice_output=os.getenv("VOICE_OUT_OUTPUT", "voice_out"),
    )

    # Services (using typed output ports)
    intent_queue = IntentQueue()
    scene_notifier = SceneNotifier(scene, ctx.scene_out, ctx.logger)
    command_queue = CommandQueueService(state, ctx.robot_out, cfg, ctx.logger)

    # Handlers
    voice_handler = VoiceInputHandler(state, behavior, intent_queue, ctx.voice_out, ctx.logger)

    # Set command callback for web interface
    state.set_command_callback(voice_handler.create_command_callback())

    # Queue initial movements on startup
    init_on_start = os.getenv("INIT_ON_START", "true").lower() in ("true", "1", "yes")
    if init_on_start:
        ctx.logger.log(f"Startup: resetting scene and moving to home [{home_pose['x']}, {home_pose['y']}, {home_pose['z']}]")
        scene.clear_detected()
        for step in robot_adapter.reset_tool():
            state.append_queue(step)
        for step in robot_adapter.move(home_pose):
            state.append_queue(step)

    # Event dispatcher
    dispatcher = EventDispatcher(ctx.adapter, ctx.logger)
    dispatcher.register(voice_in_input, voice_handler)
    dispatcher.register(pose_input, PoseHandler(state, ctx.logger))
    dispatcher.register(objects_input, ObjectsHandler(scene, scene_notifier, ctx.logger))
    dispatcher.register(image_input, ImageHandler(state, ctx.logger))
    dispatcher.register(status_input, StatusHandler(state, ctx.logger))

    # First event callback - send initial scene
    if init_on_start:
        dispatcher.on_first_event(scene_notifier.send_initial_scene)

    # Tick callbacks - process intents and dispatch commands
    def process_intents() -> None:
        """Process pending voice intents."""
        intent = intent_queue.popleft()
        if not intent:
            return

        ctx.logger.log(f"Processing intent: {intent.action} {intent.obj} {intent.color} {intent.size}")

        latest_pose = state.get_pose()
        detected_count = len(scene.query(source="detected"))
        config_count = len(scene.query(source="config"))
        ctx.logger.log(f"Available objects: {detected_count} detected + {config_count} config")

        # Find target object if action requires it
        action_info = behavior.get_action_info(intent.action)
        target_obj = None
        if action_info and action_info.requires_object:
            target_obj = scene.find_by_criteria(
                intent.obj, intent.color, intent.size, cfg.class_map
            )
            if not target_obj:
                ctx.logger.log(f"Target object not found: {intent.obj} {intent.color}")
                return
            ctx.logger.log(f"Found target: {target_obj.object_type} {target_obj.color} at {target_obj.position_mm}")

        # Build action context
        context = ActionContext(
            pose=latest_pose,
            target=target_obj,
            scene=scene,
            config=cfg,
            home_pose=home_pose,
            shared_state=state,
        )

        # Execute action
        if behavior.execute(intent.action, context):
            ctx.logger.log(f"Executed action: {intent.action}")
            if intent.action == "reiniciar":
                scene_notifier.send_scene_reset()
        else:
            ctx.logger.log(f"Failed to execute action: {intent.action}")

        ctx.logger.log(f"Queue size: {state.queue_size()}")

    dispatcher.on_tick(process_intents)
    dispatcher.on_tick(command_queue.dispatch_next)

    # Start API server
    if api_cfg.enabled:
        start_api_server(state, scene, api_cfg, behavior.get_all_keywords)

    # Run event loop
    dispatcher.run()


if __name__ == "__main__":
    main()
