#!/usr/bin/env python3
"""ROS Humble smoke test for bt_mock_servers; run only in a built ROS overlay."""
import json
import threading
import time

import rclpy
from rclpy.action import ActionClient
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node

from bt_mock_servers.mock_skills import ACTION_ENDPOINTS, ACTION_TYPES, MockSkills
from bt_skill_interfaces.msg import VerificationEvidence
from bt_skill_interfaces.srv import GetRobotState, ReconcileGoal


def trace(value):
    value.task_id, value.subtask_id = "smoke-task", "smoke-step"
    value.attempt = value.task_revision = value.plan_version = 1
    value.run_id, value.execution_generation = "smoke-run", 1


def goal_for(name):
    goal = ACTION_TYPES[name].Goal()
    goal.timeout.sec = 2
    if hasattr(goal, "trace"):
        trace(goal.trace)
    if hasattr(goal, "task_id"):
        goal.task_id = "smoke-task"
    if hasattr(goal, "subtask_id"):
        goal.subtask_id = "smoke-step"
    if name == "navigate":
        goal.target_pose.header.frame_id = "map"
        goal.target_pose.pose.orientation.w = 1.0
        goal.position_tolerance = goal.yaw_tolerance = 0.1
    elif name == "navigate_semantic":
        goal.kind, goal.reference, goal.registry_version = "LOCATION", "bin_A", 1
        goal.position_tolerance = goal.orientation_tolerance = 0.1
    elif name == "execute_manipulation":
        goal.skill, goal.instruction = "pick", "pick the smoke target"
        goal.target.object_ref, goal.target.description = "smoke-target", "smoke target"
    elif name == "execute_posture":
        goal.posture_id, goal.expected_geometry_epoch = "home", 1
    elif name == "plan_task":
        goal.task_id, goal.task_revision, goal.planning_generation = "smoke-task", 1, 1
        goal.instruction = "fetch smoke target"
        goal.known_info_json = goal.context_snapshot_json = goal.constraints_json = "{}"
    elif name == "verify_state":
        goal.check, goal.expected_geometry_epoch = goal.PRECHECK, 1
        goal.target.object_ref, goal.target.description = "smoke-target", "smoke target"
    elif name == "locate_shelf_column":
        goal.target_ref, goal.target_description = "smoke-target", "smoke target"
        goal.source_region_ref, goal.observation_station_id = "shelf_zone", "station_A"
        goal.station_registry_version = 1
    elif name == "localize_target_3d":
        goal.target_ref, goal.target_description, goal.expected_geometry_epoch = "smoke-target", "smoke target", 1
        goal.shelf_id, goal.column_id, goal.station_binding_ref = "shelf_A", "1", "station_A"
    elif name == "check_free_space":
        goal.destination_ref, goal.destination_description = "bin_A", "bin A"
        goal.object_ref, goal.object_description, goal.placement_constraints_json = "smoke-target", "smoke target", "{}"
    elif name == "assess_grasp":
        goal.allowed_posture_ids = ["pregrasp"]
        goal.target_binding.target.object_ref, goal.target_binding.target.description = "smoke-target", "smoke target"
        goal.target_binding.context.schema_version = goal.target_binding.context.geometry_epoch = 1
        goal.robot_state.robot_id = "robot_01"
        goal.robot_state.valid_until.sec = 2_000_000_000
    elif name == "evaluate_progress":
        goal.task_description, goal.sequence = "fetch smoke target", 1
        goal.window_json = '[{"stamp":1,"views":{"front":"/tmp/smoke.png"}}]'
    elif name == "execute_task":
        goal.approved_plan_json, goal.context_json = '{"subtasks":[]}', "{}"
    return goal


def await_future(future, seconds=5.0):
    deadline = time.monotonic() + seconds
    while not future.done() and time.monotonic() < deadline:
        time.sleep(0.01)
    if not future.done():
        raise TimeoutError("ROS smoke future timed out")
    return future.result()


def main():
    rclpy.init()
    server, client_node = MockSkills(), Node("bt_mock_live_smoke", namespace="/sim/robot_01")
    executor = MultiThreadedExecutor(num_threads=8)
    executor.add_node(server); executor.add_node(client_node)
    thread = threading.Thread(target=executor.spin, daemon=True); thread.start()
    try:
        for name, action_type in ACTION_TYPES.items():
            feedback = []
            client = ActionClient(client_node, action_type, ACTION_ENDPOINTS[name])
            if not client.wait_for_server(timeout_sec=3.0):
                raise RuntimeError("unreachable action: " + name)
            handle = await_future(client.send_goal_async(
                goal_for(name), feedback_callback=lambda item, out=feedback: out.append(item.feedback)))
            if not handle.accepted:
                raise RuntimeError("valid smoke goal rejected: " + name)
            wrapped = await_future(handle.get_result_async())
            if wrapped.result is None:
                raise RuntimeError("missing result: " + name)
            if not feedback:
                raise RuntimeError("missing feedback: " + name)
            client.destroy()

        server.scenarios["navigate"] = [{"kind": "cancel_stop_unknown", "duration_seconds": 1.0}]
        cancel_client = ActionClient(client_node, ACTION_TYPES["navigate"], ACTION_ENDPOINTS["navigate"])
        handle = await_future(cancel_client.send_goal_async(goal_for("navigate")))
        cancel = await_future(handle.cancel_goal_async())
        if not cancel.goals_canceling:
            raise RuntimeError("cancel was not acknowledged")
        canceled = await_future(handle.get_result_async())
        if (canceled.result.result.status != canceled.result.result.CANCELED or
                canceled.result.result.stop_state != canceled.result.result.UNKNOWN):
            raise RuntimeError("cancel terminal did not preserve unknown stop")

        state_client = client_node.create_client(GetRobotState, "get_robot_state")
        if not state_client.wait_for_service(timeout_sec=3.0):
            raise RuntimeError("GetRobotState unavailable")
        state = await_future(state_client.call_async(GetRobotState.Request(robot_id="robot_01")))
        if not state.available:
            raise RuntimeError("GetRobotState did not return simulator state")

        reconcile_client = client_node.create_client(ReconcileGoal, "reconcile_goal")
        if not reconcile_client.wait_for_service(timeout_sec=3.0):
            raise RuntimeError("ReconcileGoal unavailable")
        request = ReconcileGoal.Request()
        trace(request.trace)
        request.goal_id = bytes(handle.goal_id.uuid).hex()
        request.operator_id, request.reason = "smoke", "live smoke"
        evidence = request.evidence
        evidence.status = VerificationEvidence.PASSED
        evidence.context.trace = request.trace
        evidence.context.source_goal_id = request.goal_id
        evidence.context.writer = "independent_smoke_observer"
        evidence.context.observed_at = client_node.get_clock().now().to_msg()
        evidence.context.valid_until.sec = evidence.context.observed_at.sec + 5
        evidence.context.valid_until.nanosec = evidence.context.observed_at.nanosec
        evidence.stopped_valid = evidence.stopped = True
        evidence.source = "SIMULATOR_INDEPENDENT_FIXTURE"
        evidence.evidence_ref = "sim://smoke/stopped"
        reconciled = await_future(reconcile_client.call_async(request))
        if not reconciled.accepted:
            raise RuntimeError("bound reconciliation rejected: " + reconciled.error_code)
        print(json.dumps({"actions": len(ACTION_TYPES), "services": 2, "status": "passed"}))
    finally:
        executor.shutdown(timeout_sec=2.0)
        server.destroy_node(); client_node.destroy_node()
        if rclpy.ok(): rclpy.shutdown()
        thread.join(timeout=2.0)


if __name__ == "__main__":
    main()