haply 0.8.2

Haply Robotics Client Library for the Inverse Service
Documentation
use haply::{
    device_model::{ Config, DeviceType, Force, ForceInput },
    http::InverseHttpClient,
    HaplyDevice,
};
use std::{ env, time::Duration };
use tokio::time::{ interval, sleep };

fn has_flag(args: &[String], flag: &str) -> bool {
    args.iter().any(|a| a == flag)
}

fn value_after_flag(args: &[String], flag: &str) -> Option<String> {
    args.iter()
        .position(|a| a == flag)
        .and_then(|idx| args.get(idx + 1).cloned())
}

#[tokio::main]
async fn main() -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
    let args: Vec<String> = env::args().skip(1).collect();
    let http_only = has_flag(&args, "--http-only");
    let stream_mode = has_flag(&args, "--stream");
    let zero_force = has_flag(&args, "--zero-force");
    let probe_id = value_after_flag(&args, "--probe");

    if stream_mode && zero_force {
        eprintln!("--stream and --zero-force cannot be combined; pick one.");
        std::process::exit(1);
    }

    let http_base = "http://localhost:10000";
    let ws_url = "ws://localhost:10001/";

    if http_only {
        run_http_only(http_base).await?;
        return Ok(());
    }

    let mut device = HaplyDevice::new(http_base, ws_url).await?;

    if stream_mode {
        run_stream_mode(&mut device, probe_id).await;
        return Ok(());
    }

    if zero_force {
        run_zero_force_mode(&mut device, probe_id).await;
        return Ok(());
    }

    let state = device.read_state().await?;
    println!("Current device state: {:?}", state);
    Ok(())
}

async fn run_http_only(http_base: &str) -> Result<(), Box<dyn std::error::Error + Send + Sync>> {
    let client = InverseHttpClient::new(http_base);

    let version = client.get_version().await?;
    println!("Haply Inverse Service Version: {:?}", version);

    let configs = client.get_devices().await?;
    println!("Found {} connected device(s)", configs.len());

    for (idx, config) in configs.iter().enumerate() {
        match config {
            Config::DeviceConfig(dc) => {
                println!("Device #{}: Inverse3           - ID: {}", idx + 1, dc.device_info.id);
            }
            Config::VGConfig(vg) => {
                println!("Device #{}: VerseGrip         - ID: {}", idx + 1, vg.id);
            }
            Config::WVGConfig(wvg) => {
                let label = match wvg.type_ {
                    DeviceType::CustomVerseGrip => "CustomVerseGrip",
                    _ => "WirelessVerseGrip",
                };
                println!("Device #{}: {} - ID: {}", idx + 1, label, wvg.id);
            }
        }
    }

    Ok(())
}

async fn run_stream_mode(device: &mut HaplyDevice, probe_id: Option<String>) {
    let mut ticker = interval(Duration::from_millis(100));
    loop {
        ticker.tick().await;

        if let Some(id) = &probe_id {
            if let Err(err) = device.probe_cursor_position(vec![id.clone()], Some(true)).await {
                eprintln!("Probe command failed for {}: {}", id, err);
            } else {
                sleep(Duration::from_millis(200)).await;
            }
        }

        let state = match device.force_read_full_state(None).await {
            Ok(state) => state,
            Err(err) => {
                eprintln!("Failed to read full state: {}", err);
                continue;
            }
        };

        for (idx, inv) in state.inverse3.iter().enumerate() {
            println!("Device #{}: Inverse3           - ID: {}", idx + 1, inv.device_id);
        }
        for (idx, vg) in state.verse_grip.iter().enumerate() {
            println!("Device #{}: VerseGrip         - ID: {}", idx + 1, vg.device_id);
        }
        for (idx, wvg) in state.wireless_verse_grip.iter().enumerate() {
            println!("Device #{}: WirelessVerseGrip - ID: {}", idx + 1, wvg.device_id);
        }
        for (idx, cvg) in state.custom_verse_grip.iter().enumerate() {
            println!("Device #{}: CustomVerseGrip   - ID: {}", idx + 1, cvg.device_id);
        }

        println!("----------------------------------------------------------------------");
    }
}

async fn run_zero_force_mode(device: &mut HaplyDevice, probe_id: Option<String>) {
    let mut ticker = interval(Duration::from_millis(100));
    loop {
        ticker.tick().await;

        if let Some(id) = &probe_id {
            if let Err(err) = device.probe_cursor_position(vec![id.clone()], Some(true)).await {
                eprintln!("Probe command failed for {}: {}", id, err);
            } else {
                sleep(Duration::from_millis(200)).await;
            }
        }

        let state = match device.read_state().await {
            Ok(state) => state,
            Err(err) => {
                eprintln!("Failed to read state: {}", err);
                continue;
            }
        };

        println!("Current device state: {:?}", state);

        if let Some(inv) = state.inverse3.get(0) {
            let force = Force {
                x: 0.0,
                y: 0.0,
                z: 0.0,
            };
            let update = ForceInput {
                device_id: inv.device_id.clone(),
                forces: force,
            };

            if let Err(err) = device.update_force(vec![update], Some(true), Some(true)).await {
                eprintln!("Failed to update force: {}", err);
            }
        } else {
            println!("No Inverse3 devices available to send force updates.");
        }
    }
}