"""
AGIBOT X2 interface specification.

IMPORTANT: every constant in this file was read back from the robot itself
(10.255.254.84, PC2 / Jetson Orin) by introspecting the installed `aimdk_msgs`
package and the live ROS 2 graph - not copied from the published documentation.
Where the two disagree, the robot wins. Notable differences found:

  * McAction has 18 modes, not 5. PASSIVE_DEFAULT (1) and ZERO_TORQUE_DEFAULT
    (4) are distinct; the docs conflated them.
  * McControlArea is a BITMASK (LEFT_HAND=1, RIGHT_HAND=2, HEAD=4, WAIST=8),
    which is why the docs' "area 3" (both arms) and "area 11" (whole body)
    work out - 3 = 1|2 and 11 = 1|2|8.
  * Several documented preset motion IDs (1007, 1010, 1011, 3017, 3024, 3025,
    3031) do not exist in the firmware enum. The real list is below.
  * PmuState spells its fan field `fan_pecentage` (sic) and reports charge in
    `battery_remaining_capacity_percentage`; `battery_remaining_capacity` is
    mAh, not percent.
  * Joint names carry a `_joint` suffix.
  * This unit exposes SIX working camera feeds and a chest LiDAR. An earlier
    revision of this file claimed one camera, no rear camera and an unreachable
    Orbbec; all three were wrong. Corrected by subscribing to each topic and
    decoding a real frame off it - see CAMERA_INVENTORY. SLAM command topics
    are still genuinely absent.
"""

from __future__ import annotations

# --------------------------------------------------------------------------
# Network
# --------------------------------------------------------------------------
# PC1 runs motion control and must never be used as a build/run host.
PC1_MOTION_CONTROL_IP = "10.0.1.40"
# PC2 (this robot's development unit) as seen on the internal develop0 network.
PC2_DEVELOP_IP = "10.0.1.41"

DEFAULT_ROS_DOMAIN_ID = 0

# Port the on-robot agent listens on.
AGENT_PORT = 8781

# Ports probed when scanning a subnet for the robot.
DISCOVERY_PORTS = [AGENT_PORT, 22, 8080, 9090]


# --------------------------------------------------------------------------
# Sensor topics  (verified present on the live graph)
# --------------------------------------------------------------------------
TOPIC_IMU_CHEST = "/aima/hal/imu/chest/state"
TOPIC_IMU_TORSO = "/aima/hal/imu/torso/state"
TOPIC_TOUCH_HEAD = "/aima/hal/sensor/touch_head"
TOPIC_MODULE_INFO = "/aima/hal/sensor/module_info"

# Every feed is on demand - `always_on` is gone. Nothing is subscribed on the
# robot until the operator switches that feed on in the Vision tab, and the
# agent drops the lot when the last browser disconnects. A single frame here is
# 170-430 KB; holding six of them open for a tab nobody is looking at competes
# with the robot's own traffic on the same Wi-Fi.
CAMERAS = [
    {
        "key": "rgb_head_front_center",
        "label": "Head camera (front)",
        "kind": "rgb",
        "rate_hz": 10,
        "topic": "/aima/hal/sensor/rgb_head_front_center/rgb_image/compressed",
        "info_topic": "/aima/hal/sensor/rgb_head_front_center/camera_info",
        "flip": False,
    },
    {
        "key": "rgb_head_rear",
        "label": "Head camera (rear)",
        "kind": "rgb",
        "rate_hz": 10,
        "topic": "/aima/hal/sensor/rgb_head_rear/rgb_image/compressed",
        "info_topic": "/aima/hal/sensor/rgb_head_rear/camera_info",
        "flip": False,
    },
    {
        "key": "rgbd_head_front",
        "label": "RGB-D colour (Orbbec)",
        "kind": "rgb",
        "rate_hz": 10,
        "topic": "/aima/hal/sensor/rgbd_head_front/rgb_image/compressed",
        "info_topic": "/aima/hal/sensor/rgbd_head_front/rgb_camera_info",
        "flip": True,
        "note": "This module is mounted upside down, so the feed is rotated 180 by "
                "default. Use the button below if yours is not.",
    },
    {
        "key": "depth_front",
        "label": "Depth (colourised)",
        "kind": "depth",
        "rate_hz": 14,
        "topic": "/camera/depth/image_raw/compressedDepth",
        "flip": True,
        "note": "16-bit millimetre map, rendered to a JET ramp over 0-4.5 m on the robot "
                "and rotated 180 like the colour feed it shares a module with. "
                "Black is 'no return', not 'very close'.",
    },
    {
        "key": "stereo_head_front_left",
        "label": "Stereo left",
        "kind": "rgb",
        "rate_hz": 10,
        "topic": "/aima/hal/sensor/stereo_head_front_left/rgb_image/compressed",
        "info_topic": "/aima/hal/sensor/stereo_head_front_left/camera_info",
        "flip": False,
    },
    {
        "key": "stereo_head_front_right",
        "label": "Stereo right",
        "kind": "rgb",
        "rate_hz": 10,
        "topic": "/aima/hal/sensor/stereo_head_front_right/rgb_image/compressed",
        "info_topic": "/aima/hal/sensor/stereo_head_front_right/camera_info",
        "flip": False,
    },
    {
        "key": "perception_input",
        "label": "Perception input",
        "kind": "debug",
        "rate_hz": 10,
        "topic": "/mono_perception/debug/input_image",
        "flip": False,
        "note": "Only carries frames while the mono-perception module is running.",
    },
    {
        "key": "perception_seg",
        "label": "Segmentation mask",
        "kind": "debug",
        "rate_hz": 10,
        "topic": "/mono_perception/debug/seg_mask",
        "flip": False,
        "note": "Only carries frames while the mono-perception module is running.",
    },
    {
        "key": "perception_lines",
        "label": "Line colour map",
        "kind": "debug",
        "rate_hz": 10,
        "topic": "/mono_perception/debug/line_color_map",
        "flip": False,
        "note": "Only carries frames while the mono-perception module is running.",
    },
]

# What this unit actually has, established the only way that settles it: by
# subscribing to each topic and decoding a frame. A previous revision inferred
# the inventory from the v4l2 device map and the em app list, and got it wrong
# three ways - the rear camera and the Orbbec feeds are on the shared ROS graph
# and publish perfectly well to an ordinary `agi` subscriber. Reading the device
# tree tells you which sensors the SoC enumerates, not which topics carry data.
CAMERA_INVENTORY = [
    {
        "name": "Head front (imx900c)",
        "detail": "MIPI/GMSL sensor 2-0039, 2064x1552 12-bit Bayer, ISP-processed "
                  "and published as 2688x1944 JPEG at ~10 Hz.",
        "status": "live",
        "where": "/aima/hal/sensor/rgb_head_front_center/rgb_image/compressed",
    },
    {
        "name": "Head rear",
        "detail": "Fitted and publishing 2064x1552 JPEG at ~10 Hz, mounted upright "
                  "(no rotation needed). Confirmed by decoding a frame.",
        "status": "live",
        "where": "/aima/hal/sensor/rgb_head_rear/rgb_image/compressed",
    },
    {
        "name": "Orbbec Gemini 335 (RGB-D)",
        "detail": "Colour and depth both reachable from the agi account. Depth is a "
                  "16-bit millimetre map published as compressedDepth at ~14 Hz, "
                  "topping out around 4.2 m. The module is mounted upside down - both "
                  "its feeds are rotated 180 by default.",
        "status": "live",
        "where": "/aima/hal/sensor/rgbd_head_front/* and /camera/depth/image_raw/compressedDepth",
    },
    {
        "name": "Stereo pair (front left / right)",
        "detail": "Both eyes publish JPEG at ~10 Hz. Useful side by side for depth "
                  "the RGB-D sensor misses on reflective surfaces.",
        "status": "live",
        "where": "/aima/hal/sensor/stereo_head_front_{left,right}/rgb_image/compressed",
    },
    {
        "name": "Perception debug views",
        "detail": "Input image, segmentation mask and line colour map. Real topics, "
                  "but only carry frames while the mono-perception module is running.",
        "status": "intermittent",
        "where": "/mono_perception/debug/*",
    },
]

# --------------------------------------------------------------------------
# Chest LiDAR
# --------------------------------------------------------------------------
# Live on this unit at 2 Hz. Field layout read off the topic rather than
# assumed: x/y/z float32 at offsets 0/4/8, intensity float32 at 16, ring uint16
# at 20, timestamp float64 at 24, point_step 32. A scan is ~25.5k points
# (816 KB), decimated on the robot before it crosses the network.
LIDAR = {
    "key": "lidar_chest_front",
    "label": "Chest LiDAR",
    "topic": "/aima/hal/sensor/lidar_chest_front/lidar_pointcloud_down_sampling",
    "status_topic": "/aima/hal/sensor/lidar_chest_front/lidar_status",
    "type": "sensor_msgs/PointCloud2",
    "rate_hz": 2,
    "frame": "lidar_chest_front",
    "max_points": 4000,
}

SENSOR_TOPICS = [
    {"topic": TOPIC_IMU_CHEST, "type": "sensor_msgs/Imu", "rate_hz": 500, "label": "Chest IMU"},
    {"topic": TOPIC_IMU_TORSO, "type": "sensor_msgs/Imu", "rate_hz": 500, "label": "Torso IMU"},
    {"topic": TOPIC_TOUCH_HEAD, "type": "aimdk_msgs/TouchState", "rate_hz": 100, "label": "Head touch"},
    {"topic": "/aima/hal/pmu/state", "type": "aimdk_msgs/PmuState", "rate_hz": 1, "label": "Power management"},
    {"topic": "/aima/hal/joint/head/state", "type": "aimdk_msgs/JointStateArray", "rate_hz": 100, "label": "Head joints"},
    {"topic": "/aima/hal/joint/waist/state", "type": "aimdk_msgs/JointStateArray", "rate_hz": 100, "label": "Waist joints"},
    {"topic": "/aima/hal/joint/arm/state", "type": "aimdk_msgs/JointStateArray", "rate_hz": 100, "label": "Arm joints"},
    {"topic": "/aima/hal/joint/leg/state", "type": "aimdk_msgs/JointStateArray", "rate_hz": 100, "label": "Leg joints"},
    {"topic": "/aima/hal/joint/hand/state", "type": "aimdk_msgs/HandStateArray", "rate_hz": 30, "label": "Hand state"},
    {"topic": CAMERAS[0]["topic"], "type": "sensor_msgs/CompressedImage", "rate_hz": 10, "label": "Head camera"},
    {"topic": "/aima/mc/leg_odometry", "type": "nav_msgs/Odometry", "rate_hz": 50, "label": "Leg odometry"},
    {"topic": "/face_ui_proxy/status", "type": "aimdk_msgs/FaceEmojiStatus", "rate_hz": 1, "label": "Face status"},
]

# Head touch reports 8 independent zones.
TOUCH_ZONE_COUNT = 8


# --------------------------------------------------------------------------
# Power management  (exact PmuState field names)
# --------------------------------------------------------------------------
TOPIC_PMU_STATE = "/aima/hal/pmu/state"
PMU_RATE_HZ = 1.0

PMU_FIELDS = {
    "battery_pct": "battery_remaining_capacity_percentage",
    "battery_voltage": "battery_voltage",
    "battery_pack_voltage": "battery_pack_voltage",
    "battery_current": "battery_current",
    "battery_temp": "battery_temperature",
    "battery_cycles": "battery_cycle_count",
    "battery_capacity_mah": "battery_remaining_capacity",
    "battery_power": "battery_output_power",
    "pmu_temp": "pmu_temperature",
    "fan_rpm": "fan_speed",
    "fan_pct": "fan_pecentage",          # firmware spells it this way
}

# (display label, voltage field, current field, nominal volts)
PMU_RAILS = [
    {"key": "bus_48v", "label": "48 V bus", "voltage": "bus_48v_voltage",
     "current": "bus_48v_current", "nominal": 48.0},
    {"key": "output_48v", "label": "48 V output", "voltage": "output_48v_voltage",
     "current": "output_48v_current", "nominal": 48.0},
    {"key": "output_12v", "label": "12 V output", "voltage": "output_12v_voltage",
     "current": "output_12v_current", "nominal": 12.0},
    {"key": "head_power", "label": "Head power", "voltage": "head_power_voltage",
     "current": "head_power_current", "nominal": 24.0},
    {"key": "orin", "label": "Orin NX", "voltage": "orin_voltage",
     "current": "orin_current", "nominal": 19.0},
    {"key": "rk3588", "label": "RK3588", "voltage": "rk3588_voltage",
     "current": "rk3588_current", "nominal": 12.0},
    {"key": "fan", "label": "Fan rail", "voltage": "fan_voltage",
     "current": None, "nominal": 12.0},
    {"key": "bus_48v_pmos", "label": "48 V PMOS", "voltage": "bus_48v_pmos_voltage",
     "current": None, "nominal": 48.0},
]

PMU_INFO_FIELDS = [
    ("bms_manufacturer", "BMS manufacturer"),
    ("bms_serial_number", "BMS serial"),
    ("bms_hardware_version", "BMS hardware"),
    ("bms_software_version", "BMS software"),
    ("pmu_software_version", "PMU software"),
    ("pmu_hardware_version", "PMU hardware"),
    ("pmu_protocol_version", "PMU protocol"),
]


# --------------------------------------------------------------------------
# Motion modes  (aimdk_msgs/msg/McAction constants, read from the robot)
# --------------------------------------------------------------------------
SRV_GET_MC_ACTION = "/aimdk_5Fmsgs/srv/GetMcAction"
SRV_SET_MC_ACTION = "/aimdk_5Fmsgs/srv/SetMcAction"

MC_MODES = [
    # -- safety / low level -------------------------------------------------
    {"id": "PASSIVE_DEFAULT", "value": 1, "label": "Passive", "group": "basic",
     "desc": "Joints released, no holding force. The robot must be supported.",
     "danger": True},
    {"id": "ZERO_TORQUE_DEFAULT", "value": 4, "label": "Zero torque", "group": "basic",
     "desc": "Explicit zero-torque command. The robot will collapse if free-standing.",
     "danger": True},
    {"id": "SOFT_EMERGENCY_STOP", "value": 2, "label": "Soft E-stop", "group": "basic",
     "desc": "Controlled emergency stop.", "danger": True},
    {"id": "DAMPING_DEFAULT", "value": 3, "label": "Damping", "group": "basic",
     "desc": "Joints hold damping - the safe resting state.", "danger": False},

    # -- position control ---------------------------------------------------
    {"id": "JOINT_DEFAULT", "value": 100, "label": "Joint control", "group": "joint",
     "desc": "Position-controlled joints. Required for direct joint commands.",
     "danger": False},
    {"id": "JOINT_FREEZE", "value": 101, "label": "Joint freeze", "group": "joint",
     "desc": "Hold the current joint positions.", "danger": False},

    # -- standing -----------------------------------------------------------
    {"id": "STAND_DEFAULT", "value": 200, "label": "Stable stand", "group": "stand",
     "desc": "Active balance. Prerequisite for preset motions and walking.",
     "danger": False},
    {"id": "STAND_BODY_CONTROL", "value": 201, "label": "Body control", "group": "stand",
     "desc": "Standing with body pose control enabled.", "danger": False},

    # -- locomotion ---------------------------------------------------------
    {"id": "LOCOMOTION_DEFAULT", "value": 300, "label": "Walk", "group": "locomotion",
     "desc": "Normal walking gait.", "danger": False},
    {"id": "LOCOMOTION_STEP", "value": 302, "label": "Step", "group": "locomotion",
     "desc": "Stepping gait.", "danger": False},
    {"id": "RUN_DEFAULT", "value": 301, "label": "Run", "group": "locomotion",
     "desc": "Running gait. Needs clear space.", "danger": True},
    {"id": "ASCEND_STAIRS", "value": 2006, "label": "Ascend stairs", "group": "locomotion",
     "desc": "Stair-climbing gait.", "danger": True},
    {"id": "DESCEND_STAIRS", "value": 2008, "label": "Descend stairs", "group": "locomotion",
     "desc": "Stair-descending gait.", "danger": True},

    # -- postures -----------------------------------------------------------
    {"id": "STAND_UP_DEFAULT", "value": 2005, "label": "Stand up", "group": "posture",
     "desc": "Rise to standing.", "danger": False},
    {"id": "SIT_DOWN_DEFAULT", "value": 2000, "label": "Sit down", "group": "posture",
     "desc": "Sit down from standing.", "danger": False},
    {"id": "CROUCH_DOWN_DEFAULT", "value": 2002, "label": "Crouch", "group": "posture",
     "desc": "Crouch down.", "danger": False},
    {"id": "LIE_DOWN_DEFAULT", "value": 2004, "label": "Lie down", "group": "posture",
     "desc": "Lie down.", "danger": False},

    # -- external -----------------------------------------------------------
    {"id": "VR_REMOTE_CONTROLLER", "value": 400, "label": "VR teleop", "group": "external",
     "desc": "Hand control to the VR teleoperation bridge.", "danger": False},
]

MC_MODE_GROUPS = [
    {"key": "basic", "label": "Basic / safety"},
    {"key": "joint", "label": "Joint control"},
    {"key": "stand", "label": "Standing"},
    {"key": "locomotion", "label": "Locomotion"},
    {"key": "posture", "label": "Posture"},
    {"key": "external", "label": "External control"},
]

MC_MODE_BY_ID = {m["id"]: m for m in MC_MODES}
MC_MODE_BY_VALUE = {m["value"]: m for m in MC_MODES}
MC_MODE_IDS = [m["id"] for m in MC_MODES]

# Modes in which the robot will accept a velocity command.
DRIVEABLE_MODES = {"LOCOMOTION_DEFAULT", "LOCOMOTION_STEP", "RUN_DEFAULT",
                   "STAND_DEFAULT", "STAND_BODY_CONTROL"}

# Modes in which direct joint commands make sense.
JOINT_CONTROL_MODES_ALLOWED = {"JOINT_DEFAULT", "JOINT_FREEZE"}

MC_ACTION_STATUS = {0: "Idle", 100: "Running", 200: "Transition"}

COMMON_STATE = {
    0: "Unknown", 1: "Success", 2: "Failure", 3: "Aborted", 4: "Timeout",
    5: "Invalid", 6: "In manual", 100: "Not ready", 200: "Pending",
    300: "Created", 400: "Running",
}


# --------------------------------------------------------------------------
# Locomotion
# --------------------------------------------------------------------------
TOPIC_LOCOMOTION_VELOCITY = "/aima/mc/locomotion/velocity"
TOPIC_LEG_ODOMETRY = "/aima/mc/leg_odometry"
TOPIC_BODY_POSE = "/aima/mc/body_pose"

VELOCITY_THRESHOLDS = {"forward": 0.09, "lateral": 0.60, "angular": 0.03}

VELOCITY_LIMITS = {
    "forward": {"min": -0.6, "max": 0.8, "step": 0.01, "unit": "m/s"},
    "lateral": {"min": -0.7, "max": 0.7, "step": 0.01, "unit": "m/s"},
    "angular": {"min": -0.8, "max": 0.8, "step": 0.01, "unit": "rad/s"},
}

LOCOMOTION_PUBLISH_HZ = 20
LOCOMOTION_DEADMAN_S = 0.5


# --------------------------------------------------------------------------
# Input source arbitration
# --------------------------------------------------------------------------
SRV_GET_CURRENT_INPUT_SOURCE = "/aimdk_5Fmsgs/srv/GetCurrentInputSource"
SRV_SET_MC_INPUT_SOURCE = "/aimdk_5Fmsgs/srv/SetMcInputSource"

INPUT_SOURCE_ACTION = {
    "ADD": 1001, "MODIFY": 1002, "DELETE": 1003, "ENABLE": 2001, "DISABLE": 2002,
}

BUILTIN_INPUT_SOURCES = [
    {"name": "rc", "priority": 80, "timeout": 1000, "desc": "Handheld remote controller"},
    {"name": "vr", "priority": 70, "timeout": 1000, "desc": "VR teleoperation"},
    {"name": "app_proxy", "priority": 60, "timeout": 1000, "desc": "Mobile app"},
    {"name": "interaction", "priority": 50, "timeout": 1000, "desc": "Interaction module"},
    {"name": "pnc", "priority": 40, "timeout": 1000, "desc": "Planning and control"},
]

DASHBOARD_INPUT_SOURCE = {"name": "x2_dashboard", "priority": 30, "timeout": 1000}


# --------------------------------------------------------------------------
# Preset motions  (aimdk_msgs/msg/McPresetMotion constants, read from robot)
# McControlArea is a bitmask: LEFT_HAND=1 RIGHT_HAND=2 HEAD=4 WAIST=8
# --------------------------------------------------------------------------
SRV_SET_PRESET_MOTION = "/aimdk_5Fmsgs/srv/SetMcPresetMotion"

AREA_LEFT_HAND = 1
AREA_RIGHT_HAND = 2
AREA_BOTH_HANDS = 3
AREA_HEAD = 4
AREA_WAIST = 8
AREA_BODY = 11          # both hands + waist

MC_CONTROL_AREAS = {
    0: "None", 1: "Left arm", 2: "Right arm", 3: "Both arms",
    4: "Head", 8: "Waist", 11: "Whole body",
}

PRESET_MOTIONS = [
    # -- greeting -----------------------------------------------------------
    {"key": "wave_right", "label": "Wave", "side": "right", "motion": 1002, "area": AREA_RIGHT_HAND, "group": "greeting", "enum": "WAVE_HAND"},
    {"key": "wave_left", "label": "Wave", "side": "left", "motion": 1002, "area": AREA_LEFT_HAND, "group": "greeting", "enum": "WAVE_HAND"},
    {"key": "shake_right", "label": "Handshake", "side": "right", "motion": 1003, "area": AREA_RIGHT_HAND, "group": "greeting", "enum": "SHAKE_HAND"},
    {"key": "shake_left", "label": "Handshake", "side": "left", "motion": 1003, "area": AREA_LEFT_HAND, "group": "greeting", "enum": "SHAKE_HAND"},
    {"key": "raise_right", "label": "Raise hand", "side": "right", "motion": 1001, "area": AREA_RIGHT_HAND, "group": "greeting", "enum": "RAISE_HAND"},
    {"key": "raise_left", "label": "Raise hand", "side": "left", "motion": 1001, "area": AREA_LEFT_HAND, "group": "greeting", "enum": "RAISE_HAND"},
    {"key": "raise_both", "label": "Raise both hands", "side": "both", "motion": 1001, "area": AREA_BOTH_HANDS, "group": "greeting", "enum": "RAISE_HAND"},
    {"key": "salute_right", "label": "Salute", "side": "right", "motion": 1013, "area": AREA_RIGHT_HAND, "group": "greeting", "enum": "SALUTE"},
    {"key": "salute_left", "label": "Salute", "side": "left", "motion": 1013, "area": AREA_LEFT_HAND, "group": "greeting", "enum": "SALUTE"},
    {"key": "turn_wave", "label": "Turn and wave", "side": "body", "motion": 2001, "area": AREA_BODY, "group": "greeting", "enum": "TURN_WAVE_HAND"},
    {"key": "bow", "label": "Bow", "side": "body", "motion": 3001, "area": AREA_BODY, "group": "greeting", "enum": "INTERACTION_BOW"},

    # -- expressive ---------------------------------------------------------
    {"key": "kiss_right", "label": "Flying kiss", "side": "right", "motion": 1004, "area": AREA_RIGHT_HAND, "group": "expressive", "enum": "FLYING_KISS_HAND"},
    {"key": "kiss_left", "label": "Flying kiss", "side": "left", "motion": 1004, "area": AREA_LEFT_HAND, "group": "expressive", "enum": "FLYING_KISS_HAND"},
    {"key": "blowkiss", "label": "Blow kiss", "side": "body", "motion": 3012, "area": AREA_BODY, "group": "expressive", "enum": "INTERACTION_BLOWKISS"},
    {"key": "sweatheart", "label": "Heart", "side": "body", "motion": 3004, "area": AREA_BODY, "group": "expressive", "enum": "INTERACTION_SWEATHEART"},
    {"key": "like", "label": "Thumbs up", "side": "body", "motion": 3002, "area": AREA_BODY, "group": "expressive", "enum": "INTERACTION_LIKE"},
    {"key": "ye", "label": "Peace sign", "side": "body", "motion": 3003, "area": AREA_BODY, "group": "expressive", "enum": "INTERACTION_YE"},
    {"key": "hug", "label": "Hug", "side": "body", "motion": 3008, "area": AREA_BODY, "group": "expressive", "enum": "INTERACTION_HUG"},
    {"key": "cheer", "label": "Cheer", "side": "body", "motion": 3011, "area": AREA_BODY, "group": "expressive", "enum": "INTERACTION_CHEER"},
    {"key": "sad", "label": "Sad", "side": "body", "motion": 3006, "area": AREA_BODY, "group": "expressive", "enum": "INTERACTION_SAD"},
    {"key": "speak", "label": "Speaking gesture", "side": "body", "motion": 3016, "area": AREA_BODY, "group": "expressive", "enum": "INTERACTION_SPEAK"},

    # -- gesture ------------------------------------------------------------
    {"key": "clap", "label": "Clap", "side": "both", "motion": 1008, "area": AREA_BOTH_HANDS, "group": "gesture", "enum": "CLAP_HAND"},
    {"key": "hitclap", "label": "Hit clap", "side": "body", "motion": 3015, "area": AREA_BODY, "group": "gesture", "enum": "HITCLAP"},
    {"key": "fist_right", "label": "Fist", "side": "right", "motion": 1009, "area": AREA_RIGHT_HAND, "group": "gesture", "enum": "CLIPFIST"},
    {"key": "fist_left", "label": "Fist", "side": "left", "motion": 1009, "area": AREA_LEFT_HAND, "group": "gesture", "enum": "CLIPFIST"},
    {"key": "handx", "label": "Cross arms", "side": "body", "motion": 3009, "area": AREA_BODY, "group": "gesture", "enum": "INTERACTION_HANDX"},
    {"key": "chestwave", "label": "Chest wave", "side": "body", "motion": 3010, "area": AREA_BODY, "group": "gesture", "enum": "INTERACTION_CHESTWAVE"},
    {"key": "lightwave", "label": "Light wave", "side": "body", "motion": 3007, "area": AREA_BODY, "group": "gesture", "enum": "INTERACTION_LIGHTWAVE"},

    # -- performance --------------------------------------------------------
    {"key": "dance1", "label": "Bass dance 1", "side": "body", "motion": 3013, "area": AREA_BODY, "group": "performance", "enum": "INTERACTION_BASSDANCE1"},
    {"key": "dance2", "label": "Bass dance 2", "side": "body", "motion": 3014, "area": AREA_BODY, "group": "performance", "enum": "INTERACTION_BASSDANCE2"},
    {"key": "photo", "label": "Photo pose", "side": "body", "motion": 3018, "area": AREA_BODY, "group": "performance", "enum": "INTERACTION_PHOTOPOSTURE"},
    {"key": "photo3", "label": "Triple photo pose", "side": "body", "motion": 3019, "area": AREA_BODY, "group": "performance", "enum": "INTERACTION_PHOTOTRIPPLEPOSTURE"},

    # -- head ---------------------------------------------------------------
    {"key": "point_head", "label": "Point head", "side": "head", "motion": 4001, "area": AREA_HEAD, "group": "head", "enum": "POINT_HEAD"},
    {"key": "shake_head", "label": "Shake head", "side": "head", "motion": 4002, "area": AREA_HEAD, "group": "head", "enum": "SHAKE_HEAD"},
]

PRESET_GROUPS = [
    {"key": "greeting", "label": "Greeting"},
    {"key": "expressive", "label": "Expressive"},
    {"key": "gesture", "label": "Gesture"},
    {"key": "performance", "label": "Performance"},
    {"key": "head", "label": "Head"},
]


# --------------------------------------------------------------------------
# Joints  (names read from live JointStateArray messages)
# Limits: about_agibot_X2/joint_name_and_limit.html, X2 Ultra ranges.
# --------------------------------------------------------------------------
SRV_GET_ALL_JOINT_STATE = "/aimdk_5Fmsgs/srv/GetAllJointState"


def _j(name, label, lo, hi):
    return {"name": name, "label": label, "min_deg": lo, "max_deg": hi}


JOINT_GROUPS = [
    {
        "key": "head", "label": "Head",
        "command_topic": "/aima/hal/joint/head/command",
        "state_topic": "/aima/hal/joint/head/state",
        "joints": [
            _j("head_yaw_joint", "Head yaw", -20.0, 20.0),
            _j("head_pitch_joint", "Head pitch", 0.0, 0.0),
        ],
    },
    {
        "key": "waist", "label": "Waist",
        "command_topic": "/aima/hal/joint/waist/command",
        "state_topic": "/aima/hal/joint/waist/state",
        "joints": [
            _j("waist_yaw_joint", "Waist yaw", -196.5, 126.5),
            _j("waist_pitch_joint", "Waist pitch", -18.0, 18.0),
            _j("waist_roll_joint", "Waist roll", -28.0, 28.0),
        ],
    },
    {
        "key": "arm", "label": "Arms",
        "command_topic": "/aima/hal/joint/arm/command",
        "state_topic": "/aima/hal/joint/arm/state",
        "joints": [
            _j("left_shoulder_pitch_joint", "L shoulder pitch", -116.5, 176.5),
            _j("left_shoulder_roll_joint", "L shoulder roll", -3.5, 174.5),
            _j("left_shoulder_yaw_joint", "L shoulder yaw", -146.5, 146.5),
            _j("left_elbow_joint", "L elbow", -135.0, 0.0),
            _j("left_wrist_yaw_joint", "L wrist yaw", -146.5, 146.5),
            _j("left_wrist_pitch_joint", "L wrist pitch", -33.0, 33.0),
            _j("left_wrist_roll_joint", "L wrist roll", -86.5, 41.5),
            _j("right_shoulder_pitch_joint", "R shoulder pitch", -116.5, 176.5),
            _j("right_shoulder_roll_joint", "R shoulder roll", -3.5, 174.5),
            _j("right_shoulder_yaw_joint", "R shoulder yaw", -146.5, 146.5),
            _j("right_elbow_joint", "R elbow", -135.0, 0.0),
            _j("right_wrist_yaw_joint", "R wrist yaw", -146.5, 146.5),
            _j("right_wrist_pitch_joint", "R wrist pitch", -33.0, 33.0),
            _j("right_wrist_roll_joint", "R wrist roll", -86.5, 41.5),
        ],
    },
    {
        "key": "leg", "label": "Legs",
        "command_topic": "/aima/hal/joint/leg/command",
        "state_topic": "/aima/hal/joint/leg/state",
        "joints": [
            _j("left_hip_pitch_joint", "L hip pitch", -146.5, 146.5),
            _j("left_hip_roll_joint", "L hip roll", -13.5, 166.5),
            _j("left_hip_yaw_joint", "L hip yaw", -196.5, 96.5),
            _j("left_knee_joint", "L knee", 0.0, 138.0),
            _j("left_ankle_pitch_joint", "L ankle pitch", -26.0, 46.0),
            _j("left_ankle_roll_joint", "L ankle roll", -15.0, 15.0),
            _j("right_hip_pitch_joint", "R hip pitch", -146.5, 146.5),
            _j("right_hip_roll_joint", "R hip roll", -13.5, 166.5),
            _j("right_hip_yaw_joint", "R hip yaw", -196.5, 96.5),
            _j("right_knee_joint", "R knee", 0.0, 138.0),
            _j("right_ankle_pitch_joint", "R ankle pitch", -26.0, 46.0),
            _j("right_ankle_roll_joint", "R ankle roll", -15.0, 15.0),
        ],
    },
]

JOINT_GROUP_BY_KEY = {g["key"]: g for g in JOINT_GROUPS}

JOINT_CONTROL_MODES = [
    {"id": "position", "label": "Position", "unit": "rad", "desc": "Target angle."},
    {"id": "velocity", "label": "Velocity", "unit": "rad/s", "desc": "Target angular velocity."},
    {"id": "torque", "label": "Torque", "unit": "N·m", "desc": "Direct effort output."},
]


# --------------------------------------------------------------------------
# End effectors
# --------------------------------------------------------------------------
TOPIC_HAND_COMMAND = "/aima/hal/joint/hand/command"
TOPIC_HAND_STATE = "/aima/hal/joint/hand/state"
SRV_GET_HAND_TYPE = "/aimdk_5Fmsgs/srv/GetHandType"

HAND_TYPES = {
    0: "None", 1: "Nimble hands", 2: "Claw gripper",
    3: "Leisai nimble hands", 255: "Error",
}

DEXHAND_JOINTS = [
    "ThumbRoll", "ThumbAbAd", "ThumbMCP",
    "IndexAbAd", "IndexPIP", "MiddlePIP",
    "RingAbAd", "RingPIP", "PinkyAbAd", "PinkyPIP",
]

HAND_PRESETS = [
    {"key": "open", "label": "Open", "positions": [0.0] * 10},
    {"key": "close", "label": "Close", "positions": [0.6, 0.6, 1.2, 0.2, 1.4, 1.4, 0.2, 1.4, 0.2, 1.4]},
    {"key": "pinch", "label": "Pinch", "positions": [0.4, 0.5, 1.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0]},
    {"key": "point", "label": "Point", "positions": [0.6, 0.6, 1.2, 0.0, 0.0, 1.4, 0.2, 1.4, 0.2, 1.4]},
]


# --------------------------------------------------------------------------
# Voice / audio
# --------------------------------------------------------------------------
SRV_PLAY_TTS = "/aimdk_5Fmsgs/srv/PlayTts"
SRV_GET_VOLUME = "/aimdk_5Fmsgs/srv/GetVolume"
SRV_SET_VOLUME = "/aimdk_5Fmsgs/srv/SetVolume"
SRV_GET_MUTE = "/aimdk_5Fmsgs/srv/GetMute"
SRV_SET_MUTE = "/aimdk_5Fmsgs/srv/SetMute"
TOPIC_AUDIO_OUTPUT = "/agent/process_audio_output"
TOPIC_TTS_STATUS = "/interaction/tts_status"

# aimdk_msgs/msg/TtsPriorityLevel constants, read from the robot.
TTS_PRIORITIES = [
    {"id": "SAFETY_L10", "value": 10, "label": "Safety", "desc": "Emergency alerts"},
    {"id": "WARNING_L8", "value": 8, "label": "Warning", "desc": "Hazard warnings"},
    {"id": "SYSTEM_L7", "value": 7, "label": "System", "desc": "System notices"},
    {"id": "INTERACTION_L6", "value": 6, "label": "Interaction", "desc": "User responses"},
    {"id": "MISSION_L4", "value": 4, "label": "Mission", "desc": "Task execution"},
    {"id": "SERVICE_L2", "value": 2, "label": "Service", "desc": "Proactive services"},
    {"id": "BACKGROUND_L1", "value": 1, "label": "Background", "desc": "Background logging"},
]

AUDIO_VAD_STATES = {0: "None", 1: "Speech begin", 2: "Processing", 3: "Speech end"}


# --------------------------------------------------------------------------
# Screen / face
# --------------------------------------------------------------------------
SRV_PLAY_EMOJI = "/aimdk_5Fmsgs/srv/PlayEmoji"
TOPIC_FACE_STATUS = "/face_ui_proxy/status"

EMOJI_PLAY_MODES = [{"value": 1, "label": "Once"}, {"value": 2, "label": "Loop"}]

FACE_STATUS = {0: "Idle", 1: "Start", 2: "Running", 3: "Finished", 4: "Stopped"}

EMOJIS = [
    {"id": 1, "label": "Blink", "glyph": "\U0001F60C", "group": "neutral"},
    {"id": 10, "label": "Calm (eyes 1)", "glyph": "\U0001F642", "group": "neutral"},
    {"id": 11, "label": "Calm (eyes 2)", "glyph": "\U0001F642", "group": "neutral"},
    {"id": 20, "label": "Calm (game)", "glyph": "\U0001F3AE", "group": "neutral"},
    {"id": 30, "label": "Cute 1", "glyph": "\U0001F970", "group": "neutral"},
    {"id": 31, "label": "Cute 2", "glyph": "\U0001F970", "group": "neutral"},
    {"id": 32, "label": "Cute 3", "glyph": "\U0001F970", "group": "neutral"},
    {"id": 33, "label": "Cute 4", "glyph": "\U0001F970", "group": "neutral"},
    {"id": 40, "label": "Close eyes", "glyph": "\U0001F636", "group": "neutral"},
    {"id": 50, "label": "Open eyes", "glyph": "\U0001F440", "group": "neutral"},
    {"id": 60, "label": "Bored", "glyph": "\U0001F644", "group": "neutral"},
    {"id": 80, "label": "Sleeping", "glyph": "\U0001F634", "group": "neutral"},

    {"id": 90, "label": "Happy", "glyph": "\U0001F604", "group": "positive"},
    {"id": 100, "label": "Extra happy", "glyph": "\U0001F606", "group": "positive"},
    {"id": 101, "label": "Ecstatic", "glyph": "\U0001F929", "group": "positive"},
    {"id": 150, "label": "Acting cute", "glyph": "\U0001F60A", "group": "positive"},
    {"id": 200, "label": "Adoration", "glyph": "\U0001F60D", "group": "positive"},
    {"id": 210, "label": "Extra adoring", "glyph": "\U0001F929", "group": "positive"},

    {"id": 110, "label": "Sad", "glyph": "\U0001F622", "group": "negative"},
    {"id": 120, "label": "Sympathy", "glyph": "\U0001F97A", "group": "negative"},
    {"id": 130, "label": "Confused", "glyph": "\U0001F615", "group": "negative"},
    {"id": 140, "label": "Shocked", "glyph": "\U0001F62E", "group": "negative"},
    {"id": 160, "label": "Serious", "glyph": "\U0001F610", "group": "negative"},
    {"id": 170, "label": "Thinking", "glyph": "\U0001F914", "group": "negative"},
    {"id": 180, "label": "Angry", "glyph": "\U0001F620", "group": "negative"},
    {"id": 190, "label": "Extra angry", "glyph": "\U0001F621", "group": "negative"},

    {"id": 70, "label": "Abnormal", "glyph": "⚠️", "group": "system"},
    {"id": 220, "label": "Charging", "glyph": "\U0001F50B", "group": "system"},
]

EMOJI_GROUPS = [
    {"key": "neutral", "label": "Neutral"},
    {"key": "positive", "label": "Positive"},
    {"key": "negative", "label": "Negative"},
    {"key": "system", "label": "System"},
]


# --------------------------------------------------------------------------
# LED strip
# --------------------------------------------------------------------------
SRV_SET_PMU_LED = "/aimdk_5Fmsgs/srv/SetPmuLed"
TOPIC_PMU_LED_STATE = "/aima/hal/pmu/led_state"

LED_MODES = [
    {"value": 0, "label": "Steady", "desc": "Continuous solid colour"},
    {"value": 1, "label": "Breathing", "desc": "4 s sinusoidal brightness cycle"},
    {"value": 2, "label": "Blinking", "desc": "1 s cycle, toggles every 0.5 s"},
    {"value": 3, "label": "Flowing", "desc": "2 s cycle, left to right then off"},
]

LED_DEFAULT_PRIORITY = 6

LED_SWATCHES = [
    {"label": "Ice", "r": 57, "g": 135, "b": 229},
    {"label": "Aqua", "r": 25, "g": 158, "b": 112},
    {"label": "Amber", "r": 201, "g": 133, "b": 0},
    {"label": "Ember", "r": 217, "g": 89, "b": 38},
    {"label": "Rose", "r": 213, "g": 81, "b": 129},
    {"label": "Violet", "r": 144, "g": 133, "b": 233},
    {"label": "White", "r": 255, "g": 255, "b": 255},
    {"label": "Off", "r": 0, "g": 0, "b": 0},
]


# --------------------------------------------------------------------------
# System / diagnostics topics present on this robot
# --------------------------------------------------------------------------
TOPIC_ALERT_CODES = "/aima/hds/alert_code_list"
TOPIC_SM_SYSTEM_STATE = "/aima/sm/system_state"
TOPIC_TASK_MASTER_STATE = "/task_master/state"
PROCESS_INFO_TOPICS = [
    "/aima/hds/process/info/soc0",
    "/aima/hds/process/info/soc1",
    "/aima/hds/process/info/soc2",
]


# --------------------------------------------------------------------------
# Safety notes shown in the UI
# --------------------------------------------------------------------------
SAFETY_NOTES = [
    {"scope": "mode", "level": "critical",
     "text": "Passive and Zero-torque release all joint holding force. A free-standing robot "
             "will collapse. Only select them when the robot is supported or already seated."},
    {"scope": "locomotion", "level": "warning",
     "text": "Enter Stable stand and register an input source before driving. Keep the handheld "
             "remote controller within reach - it outranks this dashboard."},
    {"scope": "preset", "level": "warning",
     "text": "Preset motions require Stable stand and clear space around the robot."},
    {"scope": "network", "level": "critical",
     "text": f"Never use PC1 ({PC1_MOTION_CONTROL_IP}) as a build or run host. The dashboard "
             f"agent runs on PC2 ({PC2_DEVELOP_IP})."},
]


def client_spec() -> dict:
    """Everything the browser needs, in one payload."""
    return {
        "modes": MC_MODES,
        "mode_groups": MC_MODE_GROUPS,
        "driveable_modes": sorted(DRIVEABLE_MODES),
        "joint_control_modes_allowed": sorted(JOINT_CONTROL_MODES_ALLOWED),
        "action_status": MC_ACTION_STATUS,
        "presets": PRESET_MOTIONS,
        "preset_groups": PRESET_GROUPS,
        "control_areas": MC_CONTROL_AREAS,
        "joint_groups": JOINT_GROUPS,
        "joint_modes": JOINT_CONTROL_MODES,
        "velocity_limits": VELOCITY_LIMITS,
        "velocity_thresholds": VELOCITY_THRESHOLDS,
        "cameras": CAMERAS,
        "camera_inventory": CAMERA_INVENTORY,
        "lidar": LIDAR,
        "sensor_topics": SENSOR_TOPICS,
        "touch_zones": TOUCH_ZONE_COUNT,
        "pmu_rails": PMU_RAILS,
        "pmu_info_fields": PMU_INFO_FIELDS,
        "emojis": EMOJIS,
        "emoji_groups": EMOJI_GROUPS,
        "emoji_modes": EMOJI_PLAY_MODES,
        "face_status": FACE_STATUS,
        "led_modes": LED_MODES,
        "led_swatches": LED_SWATCHES,
        "tts_priorities": TTS_PRIORITIES,
        "hand_presets": HAND_PRESETS,
        "hand_types": HAND_TYPES,
        "dexhand_joints": DEXHAND_JOINTS,
        "builtin_input_sources": BUILTIN_INPUT_SOURCES,
        "dashboard_input_source": DASHBOARD_INPUT_SOURCE,
        "safety_notes": SAFETY_NOTES,
        "agent_port": AGENT_PORT,
        "topics": {
            "locomotion": TOPIC_LOCOMOTION_VELOCITY,
            "pmu": TOPIC_PMU_STATE,
            "imu_chest": TOPIC_IMU_CHEST,
            "imu_torso": TOPIC_IMU_TORSO,
            "touch_head": TOPIC_TOUCH_HEAD,
            "hand_command": TOPIC_HAND_COMMAND,
            "hand_state": TOPIC_HAND_STATE,
            "odometry": TOPIC_LEG_ODOMETRY,
            "camera": CAMERAS[0]["topic"],
        },
    }