Improve lovense solace pro support

crates/buttplug_server/src/device/protocol_impl/lovense/lovense_stroker.rs

@@ -18,22 +18,24 @@ use buttplug_server_device_config::Endpoint;
 use futures::future::BoxFuture;
 use std::{
   sync::{
-    Arc,
-    atomic::{AtomicU32, Ordering},
-  },
-  time::Duration,
+    Arc, RwLock
+  }, time::Duration
 };
 use uuid::{Uuid, uuid};
 
+use instant::Instant;
+
 const LOVENSE_STROKER_PROTOCOL_UUID: Uuid = uuid!("a97fc354-5561-459a-bc62-110d7c2868ac");
 
+const LINEAR_STEP_INTERVAL: Duration = Duration::from_millis(100);
+
 pub struct LovenseStroker {
-  linear_info: Arc<(AtomicU32, AtomicU32)>,
+  linear_info: Arc<RwLock<(u32, u32, Instant)>>,
 }
 
 impl LovenseStroker {
   pub fn new(hardware: Arc<Hardware>) -> Self {
-    let linear_info = Arc::new((AtomicU32::new(0), AtomicU32::new(0)));
+    let linear_info = Arc::new(RwLock::new((0, 0, Instant::now())));
     async_manager::spawn(update_linear_movement(
       hardware.clone(),
       linear_info.clone(),
@@ -54,8 +56,7 @@ impl ProtocolHandler for LovenseStroker {
     position: u32,
     duration: u32,
   ) -> Result<Vec<HardwareCommand>, ButtplugDeviceError> {
-    self.linear_info.0.store(position, Ordering::Relaxed);
-    self.linear_info.1.store(duration, Ordering::Relaxed);
+    *self.linear_info.write().unwrap() = (position, duration, Instant::now());
     Ok(vec![])
   }
 
@@ -70,39 +71,69 @@ impl ProtocolHandler for LovenseStroker {
   }
 }
 
-async fn update_linear_movement(device: Arc<Hardware>, linear_info: Arc<(AtomicU32, AtomicU32)>) {
-  let mut last_goal_position = 0i32;
-  let mut current_move_amount = 0i32;
-  let mut current_position = 0i32;
+async fn update_linear_movement(device: Arc<Hardware>, linear_info: Arc<RwLock<(u32, u32, Instant)>>) {
+  let mut current_position = 0u32;
+  let mut start_position = 0u32;
+  let mut last_goal_position = 0u32;
+  let mut last_start_time = Instant::now();
   loop {
-    // See if we've updated our goal position
-    let goal_position = linear_info.0.load(Ordering::Relaxed) as i32;
-    // If we have and it's not the same, recalculate based on current status.
-    if last_goal_position != goal_position {
+    let (goal_position, goal_duration, start_time) = { *linear_info.read().unwrap() };
+    let current_time = Instant::now();
+    let end_time = start_time + Duration::from_millis(goal_duration.try_into().unwrap());
+
+    // Sleep, accounting for time passed during loop (mostly from bt call time)
+    let fn_sleep = async || {
+      let elapsed = Instant::now() - current_time;
+      if elapsed < LINEAR_STEP_INTERVAL {
+        sleep(LINEAR_STEP_INTERVAL - elapsed).await
+      };
+    };
+
+    //debug!("lovense: goal data {:?}/{:?}/{:?}", goal_position, goal_duration, start_time);
+
+    // At rest
+    if current_position == goal_position {
+      fn_sleep().await;
+      continue;
+    }
+
+    // If parameters changed, re-capture the current position as the new starting position.
+    if last_start_time != start_time || last_goal_position != goal_position {
+      start_position = current_position;
+      last_start_time = start_time;
       last_goal_position = goal_position;
-      // We move every 100ms, so divide the movement into that many chunks.
-      // If we're moving so fast it'd be under our 100ms boundary, just move in 1 step.
-      let move_steps = (linear_info.1.load(Ordering::Relaxed) / 100).max(1);
-      current_move_amount = (goal_position - current_position) / move_steps as i32;
     }
 
-    // If we aren't going anywhere, just pause then restart
-    if current_position == last_goal_position {
-      sleep(Duration::from_millis(100)).await;
+    // Determine where in the motion we should be
+    assert!(current_time >= start_time);
+    let step_position = if current_time < end_time {
+      let movement_range = goal_position as f64 - start_position as f64;
+      let time_elapsed_ms = (current_time - start_time).as_millis();
+
+      let step_percentage = (time_elapsed_ms as f64) / (goal_duration as f64);
+      let step_position_dbl = step_percentage * movement_range + (start_position as f64);
+      let step_position = step_position_dbl.round() as u32;
+
+      //debug!("lovense: calculating step for time {:?} with start of {:?} and end of {:?}. Pct movement is {:?} from {:?} to {:?}, result {:?}",
+      //       current_time, start_time, end_time, step_percentage, start_position, goal_position, step_position);
+
+      step_position
+    } else {
+      goal_position
+    };
+
+    // No movement over this window
+    if current_position == step_position {
+      fn_sleep().await;
       continue;
     }
 
-    // Update our position, make sure we don't overshoot
-    current_position += current_move_amount;
-    if current_move_amount < 0 {
-      if current_position < last_goal_position {
-        current_position = last_goal_position;
-      }
-    } else if current_position > last_goal_position {
-      current_position = last_goal_position;
-    }
+    //debug!("lovense: moving to position {:?} from {:?}, goal {:?}", step_position, current_position, goal_position);
+
+    current_position = step_position;
 
-    let lovense_cmd = format!("FSetSite:{current_position};");
+    //let lovense_cmd = format!("FSetSite:{current_position};");
+    let lovense_cmd = format!("SetPoint:{current_position};");
 
     let hardware_cmd: HardwareWriteCmd = HardwareWriteCmd::new(
       &[LOVENSE_STROKER_PROTOCOL_UUID],
@@ -113,6 +144,7 @@ async fn update_linear_movement(device: Arc<Hardware>, linear_info: Arc<(AtomicU
     if device.write_value(&hardware_cmd).await.is_err() {
       return;
     }
-    sleep(Duration::from_millis(100)).await;
+
+    fn_sleep().await;
   }
 }