haply 1.3.1

//! Toggle control mode demo -- switch between position hold and zero-force with VerseGrip Button A.
//!
//! Usage:
//!   cargo run --example toggle_position_force_mode_demo
//!
//! Starts in position control and continuously commands `TARGET_POSITION`.
//! Press Button A on a wireless/custom VerseGrip to toggle modes:
//! - Position mode: sends `TARGET_POSITION` every tick
//! - Force mode: sends zero force every tick
//!
//! Stop with Ctrl+C.

use haply::{
    device_model::{ Force, ForceInput, Inverse3CommandClear, Linear3D, PositionInput },
    HaplyDevice,
};
use std::io::Write;
use std::time::Duration;
use tokio::time::{ interval, sleep };

const HTTP_BASE: &str = "http://localhost:10001";
const WS_URL: &str = "ws://localhost:10001/";
const TARGET_POSITION: Linear3D = Linear3D { x: 0.03, y: -0.1, z: 0.2 };
const ZERO_FORCE: Force = Force { x: 0.0, y: 0.0, z: 0.0 };

#[derive(Copy, Clone, Debug, PartialEq, Eq)]
enum ControlLoopMode {
    Position,
    Force,
}

impl ControlLoopMode {
    fn label(self) -> &'static str {
        match self {
            Self::Position => "position",
            Self::Force => "force",
        }
    }
}

#[tokio::main]
async fn main() -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
    let mut device = HaplyDevice::new(HTTP_BASE, WS_URL).await?;

    sleep(Duration::from_millis(300)).await;
    let state = device.read_state().await?;
    let inv = state.inverse3.first().ok_or("No Inverse3 device connected")?;
    let id = inv.device_id.clone();

    println!("Using Inverse3 device: {}", id);
    println!(
        "Starting in POSITION mode at target ({:.4}, {:.4}, {:.4})",
        TARGET_POSITION.x,
        TARGET_POSITION.y,
        TARGET_POSITION.z
    );
    println!("Press VerseGrip Button A to toggle position <-> force (zero force). Ctrl+C to stop.");

    // Ensure no stale force command remains when entering the initial position-hold mode.
    device.clear_inverse3_commands(
        &id,
        Inverse3CommandClear {
            set_cursor_force: true,
            ..Default::default()
        }
    ).await?;

    let mut mode = ControlLoopMode::Position;
    let mut last_button_a = false;
    let mut ticker = interval(Duration::from_millis(10));
    let mut tick_count: u32 = 0;

    loop {
        ticker.tick().await;
        tick_count += 1;

        let state = device.read_state().await?;
        let button_a = state.custom_verse_grip
            .iter()
            .find_map(|d| d.state.buttons.map(|b| b.a))
            .or_else(|| state.wireless_verse_grip.iter().find_map(|d| d.state.buttons.map(|b| b.a)))
            .unwrap_or(false);

        // Rising-edge detect so one press toggles once.
        if button_a && !last_button_a {
            mode = match mode {
                ControlLoopMode::Position => {
                    device.clear_inverse3_commands(
                        &id,
                        Inverse3CommandClear {
                            set_cursor_position: true,
                            ..Default::default()
                        }
                    ).await?;
                    ControlLoopMode::Force
                }
                ControlLoopMode::Force => {
                    device.clear_inverse3_commands(
                        &id,
                        Inverse3CommandClear {
                            set_cursor_force: true,
                            ..Default::default()
                        }
                    ).await?;
                    ControlLoopMode::Position
                }
            };

            println!("\nButton A pressed -> switched to {} mode", mode.label());
        }
        last_button_a = button_a;

        match mode {
            ControlLoopMode::Position => {
                device.update_position(
                    vec![PositionInput {
                        device_id: id.clone(),
                        positions: TARGET_POSITION,
                    }],
                    Some(false)
                ).await?;
            }
            ControlLoopMode::Force => {
                device.update_force(
                    vec![ForceInput {
                        device_id: id.clone(),
                        forces: ZERO_FORCE,
                    }],
                    Some(true),
                    Some(false)
                ).await?;
            }
        }

        if tick_count % 10 == 0 {
            device.send_force_full_render().await?;
        } else {
            device.send_command().await?;
        }

        if let Some(inv) = state.inverse3.first() {
            let pos = inv.state.cursor_position.unwrap_or_default();
            let rendered_force = inv.state.current_cursor_force.unwrap_or_default();
            let control_mode = inv.state.control_mode
                .map(|m| format!("{:?}", m))
                .unwrap_or_else(|| "None".to_string());
            let control_domain = inv.state.control_domain
                .map(|d| format!("{:?}", d))
                .unwrap_or_else(|| "None".to_string());

            print!(
                "\rMode: {:8}  ButtonA: {}  Domain: {:10}  Control: {:10}  Target: ({:.4},{:.4},{:.4})  Pos: ({:.4},{:.4},{:.4})  Rendered Force: ({:.4},{:.4},{:.4})   ",
                mode.label(),
                button_a,
                control_domain,
                control_mode,
                TARGET_POSITION.x,
                TARGET_POSITION.y,
                TARGET_POSITION.z,
                pos.x,
                pos.y,
                pos.z,
                rendered_force.x,
                rendered_force.y,
                rendered_force.z
            );
            std::io::stdout().flush().unwrap();
        }
    }
}