TransformFrame Transform System

use horus::prelude::*; // Provides TransformFrame, TransformFrameConfig, Transform, timestamp_now

// Create with default config (256 frames)
let hf = TransformFrame::new();

// Register frame hierarchy: world → base_link → camera
hf.register_frame("world", None)?;
hf.register_frame("base_link", Some("world"))?;
hf.register_frame("camera", Some("base_link"))?;

// Update transform (camera position relative to base_link)
let tf = Transform::new(
    [0.1, 0.0, 0.3],         // translation [x, y, z] in meters
    [0.0, 0.0, 0.0, 1.0],    // quaternion [x, y, z, w]
);
hf.update_transform("camera", &tf, timestamp_now())?;

// Query: where is camera relative to world?
let camera_to_world = hf.tf("camera", "world")?;
let point_in_world = camera_to_world.transform_point([1.0, 0.0, 0.0]);

pub struct Transform {
    pub translation: [f64; 3],  // [x, y, z] in meters
    pub rotation: [f64; 4],     // quaternion [x, y, z, w] (Hamilton convention)
}

// From translation + quaternion
let tf = Transform::new([1.0, 2.0, 3.0], [0.0, 0.0, 0.0, 1.0]);

// Translation only (identity rotation)
let tf = Transform::from_translation([1.0, 0.0, 0.0]);

// Rotation only (no translation)
let tf = Transform::from_rotation([0.0, 0.0, 0.0, 1.0]);

// From Euler angles (roll, pitch, yaw)
let tf = Transform::from_euler([1.0, 0.0, 0.0], [0.1, 0.2, 0.3]);

// From axis-angle (translation, axis, angle)
let tf = Transform::from_axis_angle([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], std::f64::consts::PI / 2.0);

// Identity transform
let tf = Transform::identity();

// Compose two transforms
let composed = tf1.compose(&tf2);

// Invert a transform
let inverse = tf.inverse();

// Apply to a point (translation + rotation)
let point_world = tf.transform_point([1.0, 0.0, 0.0]);

// Apply to a vector (rotation only)
let vector_world = tf.transform_vector([1.0, 0.0, 0.0]);

// Interpolate between transforms (t=0.0 to 1.0)
// Uses linear interpolation for translation, SLERP for rotation
let tf_mid = tf1.interpolate(&tf2, 0.5);

// Convert to/from 4x4 matrix
let matrix = tf.to_matrix();
let tf = Transform::from_matrix(matrix);

// Extract Euler angles [roll, pitch, yaw]
let euler = tf.to_euler();

// Register with parent relationship
let camera_id = hf.register_frame("camera", Some("base_link"))?;

// Root frame (no parent)
hf.register_frame("world", None)?;

// Update transform over time
hf.update_transform("camera", &new_tf, timestamp_now())?;

// Or use cached frame ID for faster updates
hf.update_transform_by_id(camera_id, &new_tf, timestamp_now());

// Register with initial transform
let mount_tf = Transform::from_translation([0.1, 0.0, 0.2]);
hf.register_static_frame("lidar_mount", Some("base_link"), &mount_tf)?;

// Update static transform (rare, e.g., recalibration)
hf.set_static_transform("lidar_mount", &new_tf)?;

// Get frame ID (cache this for hot paths!)
let id: Option<FrameId> = hf.frame_id("camera");

// Get frame name by ID
let name: Option<String> = hf.frame_name(id);

// Check if frame exists
if hf.has_frame("camera") { /* ... */ }

// List all frames
let all: Vec<String> = hf.all_frames();

// Get parent/children
let parent: Option<String> = hf.parent("camera");
let children: Vec<String> = hf.children("base_link");

// Get transform from source frame to destination frame
let tf = hf.tf("camera", "world")?;

// Check if transform path exists
if hf.can_transform("camera", "world") {
    let tf = hf.tf("camera", "world")?;
}

// Cache IDs once at startup
let camera_id = hf.frame_id("camera").unwrap();
let world_id = hf.frame_id("world").unwrap();

// Use IDs in hot loop (~50ns vs ~200ns for name-based)
loop {
    let tf = hf.tf_by_id(camera_id, world_id);
    if let Some(tf) = tf {
        let target_in_world = tf.transform_point(target_in_camera);
        // Control logic...
    }
}

// Get transform at a specific past time
let past = timestamp_now() - 100_000_000; // 100ms ago
let tf = hf.tf_at("camera", "world", past)?;

// ID-based version
let tf = hf.tf_at_by_id(camera_id, world_id, past);

// Default / Small (256 frames, ~550KB)
let hf = TransformFrame::new();       // Same as TransformFrame::small()
let hf = TransformFrame::small();

// Medium (1024 frames, ~2.2MB)
let hf = TransformFrame::medium();

// Large (4096 frames, ~9MB)
let hf = TransformFrame::large();

// Massive (16384 frames, ~35MB) - 100+ robot simulations
let hf = TransformFrame::with_config(TransformFrameConfig::massive());

// Unlimited frames (HashMap overflow for dynamic environments)
let hf = TransformFrame::with_config(TransformFrameConfig::unlimited());

let hf = TransformFrame::with_config(TransformFrameConfig {
    max_frames: 2048,           // Total frames (16-65536)
    max_static_frames: 1024,    // Static frames (less memory, faster)
    history_len: 64,            // Past transforms per dynamic frame (4-256)
    enable_overflow: false,     // Allow HashMap for unlimited frames
    chain_cache_size: 256,      // LRU cache for chain lookups
});

let hf = TransformFrame::with_config(
    TransformFrameConfig::custom()
        .max_frames(512)
        .history_len(64)
        .build()?
);

# List all coordinate frames
horus tf list
horus tf list --json     # JSON output

# Echo transform between frames (like ros2 run tf2_ros tf2_echo)
horus tf echo camera base_link
horus tf echo camera world --rate 10  # 10 Hz

# Show frame tree (like ros2 run tf2_tools view_frames)
horus tf tree

# Get detailed info about a specific frame
horus tf info camera

# Check if transform exists between two frames
horus tf can camera world

# Monitor transform update rates
horus tf hz

// Get usage statistics
let stats = hf.stats();
println!("Frames: {}/{}", stats.total_frames, stats.max_frames);
println!("Static: {}, Dynamic: {}", stats.static_frames, stats.dynamic_frames);

// Validate frame tree integrity
hf.validate()?;