//! Topic remapping rules for the merge.
//!
//! - Ego topics are kept verbatim (already namespaced under `/ego/...`, plus
//!   `/tf` and `/tf_static`).
//! - Wrist topics `/camera/<x>` become `/wrist_left/<x>` or `/wrist_right/<x>`.
//! - `/tf_static` from every input maps to a single shared `/tf_static`
//!   channel, merging the three static-transform streams into one tree.

use crate::window::Role;

/// Output topic for a message given its source role and input topic.
pub fn output_topic(role: Role, topic: &str) -> String {
    match role {
        Role::Ego => topic.to_string(),
        Role::WristLeft => remap_wrist(topic, "wrist_left"),
        Role::WristRight => remap_wrist(topic, "wrist_right"),
    }
}

fn remap_wrist(topic: &str, side: &str) -> String {
    // Static transforms are merged across all inputs, so they keep the shared
    // name rather than being namespaced per camera.
    if topic == "/tf_static" {
        return "/tf_static".to_string();
    }
    match topic.strip_prefix("/camera/") {
        Some(rest) => format!("/{side}/{rest}"),
        // Anything unexpected passes through unchanged rather than being lost.
        None => topic.to_string(),
    }
}

#[cfg(test)]
mod tests {
    use super::*;

    #[test]
    fn wrist_left_mapping() {
        assert_eq!(output_topic(Role::WristLeft, "/camera/image/compressed"), "/wrist_left/image/compressed");
        assert_eq!(output_topic(Role::WristLeft, "/camera/imu/data"), "/wrist_left/imu/data");
        assert_eq!(output_topic(Role::WristLeft, "/tf_static"), "/tf_static");
    }

    #[test]
    fn wrist_right_mapping() {
        assert_eq!(output_topic(Role::WristRight, "/camera/image/camera_info"), "/wrist_right/image/camera_info");
    }

    #[test]
    fn ego_untouched() {
        assert_eq!(output_topic(Role::Ego, "/ego/zed_head/odom"), "/ego/zed_head/odom");
        assert_eq!(output_topic(Role::Ego, "/tf"), "/tf");
    }
}
