Force & Haptic Messages

use horus::prelude::*;

// Create wrench measurement
let force = Vector3::new(10.0, 5.0, -2.0);   // Newtons
let torque = Vector3::new(0.1, 0.2, 0.05);   // Newton-meters

let wrench = WrenchStamped::new(force, torque)
    .with_frame_id("tool0");

// Check magnitudes
println!("Force magnitude: {:.2} N", wrench.force_magnitude());
println!("Torque magnitude: {:.3} Nm", wrench.torque_magnitude());

// Safety check
let max_force = 50.0;   // N
let max_torque = 5.0;   // Nm
if wrench.exceeds_limits(max_force, max_torque) {
    println!("Safety limits exceeded!");
}

// Create from force only
let force_only = WrenchStamped::force_only(Vector3::new(0.0, 0.0, -10.0));

// Create from torque only
let torque_only = WrenchStamped::torque_only(Vector3::new(0.0, 0.0, 0.5));

// Low-pass filter noisy sensor readings
let mut current = wrench;
current.filter(&previous_wrench, 0.1);  // alpha = 0.1 (heavy filtering)

use horus::prelude::*;

// Default impedance (moderate compliance)
let mut impedance = ImpedanceParameters::new();

// Compliant mode (low stiffness - for delicate tasks)
let compliant = ImpedanceParameters::compliant();
// stiffness: [100, 100, 100, 10, 10, 10]
// damping: [20, 20, 20, 2, 2, 2]

// Stiff mode (high stiffness - for precision tasks)
let stiff = ImpedanceParameters::stiff();
// stiffness: [5000, 5000, 5000, 500, 500, 500]
// damping: [100, 100, 100, 10, 10, 10]

// Enable/disable
impedance.enable();
impedance.disable();

// Custom parameters
impedance.stiffness = [500.0, 500.0, 200.0, 50.0, 50.0, 50.0];  // [Kx, Ky, Kz, Krx, Kry, Krz]
impedance.damping = [30.0, 30.0, 20.0, 3.0, 3.0, 3.0];          // [Dx, Dy, Dz, Drx, Dry, Drz]
impedance.force_limits = [50.0, 50.0, 30.0, 5.0, 5.0, 5.0];     // Safety limits

use horus::prelude::*;

// Pure force command
let force_cmd = ForceCommand::force_only(Vector3::new(0.0, 0.0, -5.0));  // 5N downward

// Hybrid force/position control
// Force control on Z axis, position control on X/Y
let force_axes: [u8; 6] = [0, 0, 1, 0, 0, 0];  // 1=force, 0=position per axis
let target_force = Vector3::new(0.0, 0.0, -10.0);
let target_position = Vector3::new(0.5, 0.3, 0.0);

let hybrid_cmd = ForceCommand::hybrid(force_axes, target_force, target_position);

// Surface contact following
let surface_normal = Vector3::new(0.0, 0.0, 1.0);  // Horizontal surface
let contact_force = 5.0;  // 5N contact force
let surface_cmd = ForceCommand::surface_contact(contact_force, surface_normal);

// Set timeout
let cmd_with_timeout = force_cmd.with_timeout(5.0);  // 5 second timeout

use horus_library::messages::force::{ContactInfo, ContactState};

// Create contact info
let contact = ContactInfo::new(ContactState::StableContact, 15.0);  // 15N force

// Check contact state
if contact.is_in_contact() {
    println!("Contact force: {:.1} N", contact.contact_force);
    println!("Duration: {:.2}s", contact.contact_duration_seconds());
    println!("Confidence: {:.0}%", contact.confidence * 100.0);
}

use horus::prelude::*;
use horus_library::messages::force::HapticFeedback;

// Vibration feedback
let vibration = HapticFeedback::vibration(
    0.8,   // intensity (0-1)
    250.0, // frequency (Hz)
    0.5    // duration (seconds)
);

// Force feedback
let force = HapticFeedback::force(
    Vector3::new(1.0, 0.0, 0.0),  // Force direction
    1.0                           // Duration (seconds)
);

// Pulse pattern
let pulse = HapticFeedback::pulse(
    0.6,   // intensity
    100.0, // frequency
    0.3    // duration
);

use horus::prelude::*;

struct ForceControlNode {
    wrench_sub: Topic<WrenchStamped>,
    cmd_pub: Topic<ForceCommand>,
    impedance_pub: Topic<ImpedanceParameters>,
    target_force: f64,
    prev_wrench: Option<WrenchStamped>,
}

impl Node for ForceControlNode {
    fn name(&self) -> &str { "ForceControl" }

    fn tick(&mut self) {
        if let Some(mut wrench) = self.wrench_sub.recv() {
            // Apply low-pass filter
            if let Some(prev) = &self.prev_wrench {
                wrench.filter(prev, 0.2);
            }
            self.prev_wrench = Some(wrench);

            // Safety check
            if wrench.exceeds_limits(100.0, 10.0) {
                // Switch to compliant mode
                let compliant = ImpedanceParameters::compliant();
                self.impedance_pub.send(compliant);
                return;
            }

            // Force control to maintain target contact force
            let error = self.target_force - wrench.force.z;
            let correction = error * 0.001;  // Simple P control

            let cmd = ForceCommand::force_only(
                Vector3::new(0.0, 0.0, self.target_force + correction)
            );
            self.cmd_pub.send(cmd);
        }
    }
}