Geometry Messages

use horus::prelude::*;

// Create 3D velocity command
let twist = Twist::new(
    [1.0, 0.0, 0.0],    // linear: [x, y, z] in m/s
    [0.0, 0.0, 0.5]     // angular: [roll, pitch, yaw] in rad/s
);

// For 2D robots (common case)
let cmd = Twist::new_2d(0.5, 0.3);  // 0.5 m/s forward, 0.3 rad/s rotation
println!("Linear X: {}, Angular Z: {}", cmd.linear[0], cmd.angular[2]);

// Stop command (all zeros)
let stop = Twist::stop();

// Validate the message
if twist.is_valid() {
    println!("Twist is valid");
}

use horus::prelude::*;

// Create 2D pose
let pose = Pose2D::new(5.0, 3.0, 1.57);  // x, y, theta
println!("Position: ({}, {}), Orientation: {} rad", pose.x, pose.y, pose.theta);

// Create pose at origin
let origin = Pose2D::origin();

// Calculate distance between poses
let other = Pose2D::new(8.0, 7.0, 0.0);
let distance = pose.distance_to(&other);
println!("Distance: {:.2} m", distance);

// Normalize angle to [-π, π]
let mut pose_copy = pose;
pose_copy.normalize_angle();

// Check validity
assert!(pose.is_valid());

use horus::prelude::*;

// Create transform with translation and quaternion rotation
let transform = TransformStamped::new(
    [1.0, 2.0, 0.5],           // translation [x, y, z]
    [0.0, 0.0, 0.0, 1.0]       // rotation [x, y, z, w] quaternion
);

// Identity transform (no translation or rotation)
let identity = TransformStamped::identity();

// Create from 2D pose (z=0, only yaw rotation)
let pose = Pose2D::new(3.0, 4.0, 1.57);
let tf_from_pose = TransformStamped::from_pose_2d(&pose);

// Validate quaternion is normalized
if transform.is_valid() {
    println!("Transform is valid (quaternion normalized)");
}

// Normalize quaternion if needed
let mut tf = TransformStamped::new([0.0; 3], [1.0, 1.0, 1.0, 1.0]);
tf.normalize_rotation();

use horus::prelude::*;

// Create 3D point
let point = Point3::new(1.0, 2.0, 3.0);
println!("Point: ({}, {}, {})", point.x, point.y, point.z);

// Create point at origin
let origin = Point3::origin();

// Calculate distance between points
let other = Point3::new(4.0, 6.0, 3.0);
let distance = point.distance_to(&other);
println!("Distance: {:.2} m", distance);  // 5.0 m

use horus::prelude::*;

// Create 3D vector
let v = Vector3::new(3.0, 4.0, 0.0);
println!("Vector: ({}, {}, {})", v.x, v.y, v.z);

// Zero vector
let zero = Vector3::zero();

// Calculate magnitude
let mag = v.magnitude();
println!("Magnitude: {:.2}", mag);  // 5.0

// Normalize vector
let mut unit = Vector3::new(3.0, 4.0, 0.0);
unit.normalize();
println!("Unit vector: ({:.2}, {:.2}, {})", unit.x, unit.y, unit.z);  // (0.6, 0.8, 0)

// Dot product
let v1 = Vector3::new(1.0, 2.0, 3.0);
let v2 = Vector3::new(4.0, 5.0, 6.0);
let dot = v1.dot(&v2);
println!("Dot product: {}", dot);  // 32.0

// Cross product
let i = Vector3::new(1.0, 0.0, 0.0);
let j = Vector3::new(0.0, 1.0, 0.0);
let k = i.cross(&j);
println!("i × j = ({}, {}, {})", k.x, k.y, k.z);  // (0, 0, 1)

use horus::prelude::*;

// Create quaternion directly
let q = Quaternion::new(0.0, 0.0, 0.0, 1.0);  // [x, y, z, w]

// Identity quaternion (no rotation)
let identity = Quaternion::identity();
assert_eq!(identity.w, 1.0);

// Create from Euler angles (roll, pitch, yaw)
let q_from_euler = Quaternion::from_euler(
    0.0,    // roll (rotation around X)
    0.0,    // pitch (rotation around Y)
    1.57    // yaw (rotation around Z) - 90 degrees
);

// Normalize quaternion
let mut q_unnorm = Quaternion::new(1.0, 1.0, 1.0, 1.0);
q_unnorm.normalize();

// Validate
assert!(q.is_valid());

use horus::prelude::*;

// HORUS automatically uses zero-copy for compatible types
let twist_topic: Topic<Twist> = Topic::new("velocity_cmd")?;
let pose_topic: Topic<Pose2D> = Topic::new("robot_pose")?;

// Send - automatically zero-copy for Pod types
twist_topic.send(Twist::new_2d(0.5, 0.1));

// Receive - zero-copy access
if let Some(pose) = pose_topic.recv() {
    println!("Robot at ({:.2}, {:.2})", pose.x, pose.y);
}

use horus::prelude::*;

struct DifferentialDriveController {
    pose_sub: Topic<Pose2D>,
    goal_sub: Topic<Pose2D>,
    cmd_pub: Topic<Twist>,
}

impl Node for DifferentialDriveController {
    fn name(&self) -> &str { "DiffDriveController" }

    fn tick(&mut self) {
        // Get current pose and goal
        let pose = match self.pose_sub.recv() {
            Some(p) => p,
            None => return,
        };

        let goal = match self.goal_sub.recv() {
            Some(g) => g,
            None => return,
        };

        // Calculate distance and angle to goal
        let dx = goal.x - pose.x;
        let dy = goal.y - pose.y;
        let distance = (dx * dx + dy * dy).sqrt();
        let angle_to_goal = dy.atan2(dx);
        let angle_error = angle_to_goal - pose.theta;

        // Simple proportional controller
        let cmd = if distance > 0.1 {
            // Move towards goal
            Twist::new_2d(
                0.3 * distance.min(1.0),     // Linear velocity (capped)
                1.0 * angle_error            // Angular velocity
            )
        } else {
            // Goal reached
            Twist::stop()
        };

        self.cmd_pub.send(cmd);
    }
}