TransformFrame API

use horus::prelude::*;

// Default: 256 frames, 128 static, 32-entry history
let tf = TransformFrame::new();

// Presets
let tf = TransformFrame::small();    // 256 frames (~550KB)
let tf = TransformFrame::medium();   // 1024 frames (~2.2MB)
let tf = TransformFrame::large();    // 4096 frames (~9MB)
let tf = TransformFrame::massive();  // 16384 frames (~35MB)

// Custom
let tf = TransformFrame::with_config(
    TransformFrameConfig::custom()
        .max_frames(512)
        .history_len(64)
        .enable_overflow(false)  // Hard RT: no heap fallback
        .build()?
);

let config = TransformFrameConfig::custom()
    .max_frames(512)
    .max_static_frames(256)
    .history_len(64)
    .enable_overflow(false)
    .chain_cache_size(128)
    .build()?;

println!("Memory: {}", config.memory_estimate());  // "1.1 MB"

// Root frame (no parent)
tf.add_frame("world").build()?;

// Dynamic child frame
tf.add_frame("base_link").parent("world").build()?;

// Static frame with fixed offset (e.g., sensor mount)
tf.add_frame("camera")
    .parent("base_link")
    .static_transform(&Transform::xyz(0.1, 0.0, 0.5))
    .build()?;

// Dynamic frame
let id = tf.register_frame("base_link", Some("world"))?;

// Static frame with fixed transform
let id = tf.register_static_frame("camera", Some("base_link"), &Transform::xyz(0.1, 0.0, 0.5))?;

// Unregister (dynamic only)
tf.unregister_frame("temp_frame")?;

let now = timestamp_now();
tf.update_transform("base_link", &Transform::xyz(1.0, 2.0, 0.0), now)?;

// By ID (fastest, lock-free, no name lookup)
let id = tf.frame_id("base_link").unwrap();
tf.update_transform_by_id(id, &Transform::xyz(1.0, 2.0, 0.0), now)?;

// Update static frame offset
tf.set_static_transform("camera", &Transform::xyz(0.1, 0.0, 0.6))?;

// Latest transform from camera to world
let t = tf.query("camera").to("world").lookup()?;

// At a specific timestamp (with LERP + SLERP interpolation)
let t = tf.query("camera").to("world").at(timestamp_ns)?;

// Strict: error if extrapolation needed
let t = tf.query("camera").to("world").at_strict(timestamp_ns)?;

// With tolerance window
let t = tf.query("camera").to("world").at_with_tolerance(timestamp_ns, 100_000_000)?;  // 100ms

// Transform a point
let world_pt = tf.query("lidar").to("world").point([1.0, 0.0, 0.0])?;

// Transform a vector (rotation only, no translation)
let world_vec = tf.query("imu").to("world").vector([0.0, 0.0, 9.81])?;

// Check availability
if tf.query("sensor").to("world").can_at(timestamp_ns) {
    let t = tf.query("sensor").to("world").at(timestamp_ns)?;
}

// Get the frame chain
let chain = tf.query("camera").to("world").chain()?;  // ["camera", "base_link", "world"]

pub fn lookup(&self) -> Result<Transform>

pub fn at(&self, timestamp_ns: u64) -> Result<Transform>

pub fn wait(&self, timeout: Duration) -> Result<Transform>

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

let t = Transform::identity();
let t = Transform::new([1.0, 2.0, 0.0], [0.0, 0.0, 0.0, 1.0]);
let t = Transform::xyz(1.0, 2.0, 0.0);
let t = Transform::x(1.0);         // X-axis only
let t = Transform::yaw(1.57);      // Z-axis rotation (radians)
let t = Transform::rpy(0.0, 0.0, 1.57);  // Roll, pitch, yaw
let t = Transform::from_euler([1.0, 2.0, 0.0], [0.0, 0.0, 1.57]);
let t = Transform::from_translation([1.0, 2.0, 0.0]);
let t = Transform::from_rotation([0.0, 0.0, 0.707, 0.707]);
let t = Transform::from_matrix(matrix_4x4);

// Chainable
let t = Transform::xyz(1.0, 0.0, 0.0).with_yaw(1.57);
let t = Transform::xyz(1.0, 0.0, 0.5).with_rpy(0.0, 0.1, 0.0);

pub fn tf(&self, src: &str, dst: &str) -> HorusResult<Transform>

use horus::prelude::*;

// Where is the camera relative to the world?
let camera_to_world = tf.tf("camera", "world")?;
println!("Camera at: ({:.2}, {:.2}, {:.2})",
    camera_to_world.translation[0],
    camera_to_world.translation[1],
    camera_to_world.translation[2]);

// Using wall-clock time
if tf.is_stale_now("imu", 500_000_000) {  // 500ms
    hlog!(warn, "IMU data is stale!");
}

// Using custom time (for simulation)
if tf.is_stale("imu", 500_000_000, sim_time_ns) {
    hlog!(warn, "IMU stale in sim time");
}

// Time since last update
if let Some(age_ns) = tf.time_since_last_update_now("imu") {
    println!("IMU age: {}ms", age_ns / 1_000_000);
}

let stats = tf.stats();
println!("{}", stats);  // Pretty-printed summary

println!("Total: {}, Static: {}, Dynamic: {}",
    stats.total_frames, stats.static_frames, stats.dynamic_frames);
println!("Tree depth: {}, Roots: {}", stats.tree_depth, stats.root_count);

if let Some(info) = tf.frame_info("camera") {
    println!("Frame: {}, Parent: {:?}, Static: {}", info.name, info.parent, info.is_static);
    println!("Depth: {}, Children: {}", info.depth, info.children_count);
}

// All frames
let all = tf.frame_info_all();

tf.print_tree();                    // Print to stderr
let text = tf.format_tree();        // Human-readable tree string
let dot = tf.frames_as_dot();       // Graphviz DOT format
let yaml = tf.frames_as_yaml();     // YAML (TF2-style)
tf.validate()?;                     // Validate tree structure

use horus::prelude::*;

let now: u64 = timestamp_now();  // Current time in nanoseconds (UNIX epoch)

let id: FrameId = tf.frame_id("base_link").unwrap();

// Fast by-ID operations (no string lookup)
tf.update_transform_by_id(id, &transform, timestamp_now())?;
let t = tf.tf_by_id(src_id, dst_id);

use horus::prelude::*;

fn main() -> Result<()> {
    let tf = TransformFrame::new();

    // Build frame tree
    tf.add_frame("world").build()?;
    tf.add_frame("odom").parent("world").build()?;
    tf.add_frame("base_link").parent("odom").build()?;
    tf.add_frame("lidar")
        .parent("base_link")
        .static_transform(&Transform::xyz(0.2, 0.0, 0.3))
        .build()?;
    tf.add_frame("camera")
        .parent("base_link")
        .static_transform(&Transform::xyz(0.1, 0.0, 0.5).with_rpy(0.0, 0.1, 0.0))
        .build()?;

    // Update dynamic frames
    let now = timestamp_now();
    tf.update_transform("odom", &Transform::xyz(1.0, 0.5, 0.0).with_yaw(0.3), now)?;
    tf.update_transform("base_link", &Transform::xyz(0.05, 0.02, 0.0), now)?;

    // Query transforms
    let lidar_to_world = tf.query("lidar").to("world").lookup()?;
    println!("Lidar position in world: {:?}", lidar_to_world.translation);

    // Transform a LiDAR point into world frame
    let world_pt = tf.query("lidar").to("world").point([5.0, 0.0, 0.0])?;
    println!("Obstacle at world: ({:.2}, {:.2}, {:.2})", world_pt[0], world_pt[1], world_pt[2]);

    // Check staleness
    if tf.is_stale_now("base_link", 100_000_000) {  // 100ms
        println!("Odometry is stale!");
    }

    // Print tree
    tf.print_tree();

    Ok(())
}

let tf = TransformFrame::new();

// Static frames (don't change)
tf.register_frame("world", None)?;
tf.register_frame("odom", Some("world"))?;
tf.register_frame("base_link", Some("odom"))?;

// Sensor mounts (static transforms from CAD/measurement)
tf.register_frame("laser_link", Some("base_link"))?;
tf.update_transform("laser_link",
    &Transform::new([0.15, 0.0, 0.3], [0.0, 0.0, 0.0, 1.0]),
    timestamp_now())?;

tf.register_frame("camera_link", Some("base_link"))?;
tf.update_transform("camera_link",
    &Transform::new([0.1, -0.05, 0.25], [0.0, 0.0, 0.0, 1.0]),
    timestamp_now())?;

// Query: transform lidar point to camera frame
let laser_to_cam = tf.tf("laser_link", "camera_link")?;
let point_in_cam = laser_to_cam.transform_point([2.0, 0.0, 0.0]);

let tf = TransformFrame::new();

tf.register_frame("base", None)?;
for i in 1..=6 {
    let parent = if i == 1 { "base" } else { &format!("link{}", i - 1) };
    tf.register_frame(&format!("link{}", i), Some(parent))?;
}
tf.register_frame("tool", Some("link6"))?;
tf.register_frame("gripper", Some("tool"))?;

// Update joint angles each tick (from forward kinematics)
// tf.update_transform("link1", &fk_transform, timestamp_now())?;

// Query: where is the gripper in base frame?
let gripper_pose = tf.tf("gripper", "base")?;