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);
        }
    }
}

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

if let Some(contact) = contact_sub.recv() {
    match contact.state {
        s if s == ContactState::Impact as u8 => {
            // Emergency: zero out force immediately
            let zero_cmd = ForceCommand::force_only(Vector3::new(0.0, 0.0, 0.0));
            cmd_pub.send(zero_cmd);
        }
        s if s == ContactState::StableContact as u8 => {
            // Safe to apply task forces
        }
        _ => {}
    }
}

use horus_library::messages::force::TactileArray;

// Create a 4x4 tactile sensor pad
let mut tactile = TactileArray::new(4, 4);

// Set individual taxel readings
tactile.set_force(1, 2, 3.5);  // row 1, col 2 = 3.5 N

// Read a taxel
if let Some(force) = tactile.get_force(1, 2) {
    println!("Force at (1,2): {:.2} N", force);
}

// Aggregate contact info
tactile.in_contact = true;
tactile.total_force = [0.0, 0.0, 12.5];           // Net force vector
tactile.center_of_pressure = [0.45, 0.52];         // Normalized [0..1]
tactile.physical_size = [0.04, 0.04];              // 4cm × 4cm sensor