From fa4f763d05e82741ec0a76bc8fc8bd592c92049c Mon Sep 17 00:00:00 2001
From: Hector Martin <marcan@marcan.st>
Date: Thu, 12 Oct 2023 17:18:31 +0900
Subject: [PATCH] Implement catchup/skipping the model forward

Signed-off-by: Hector Martin <marcan@marcan.st>
---
 src/main.rs  | 19 +++++++++++++++++++
 src/types.rs | 18 ++++++++++++++++++
 2 files changed, 37 insertions(+)

diff --git a/src/main.rs b/src/main.rs
index 984ca7b..65f88f4 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -10,6 +10,7 @@ use std::collections::BTreeMap;
 use std::fs::read_to_string;
 use std::io;
 use std::path::PathBuf;
+use std::time::{Duration, Instant};
 use std::{thread::sleep, time};
 
 use clap::Parser;
@@ -167,6 +168,8 @@ fn main() {
         group.gain = 0.0;
     }
 
+    let mut last_update = Instant::now();
+
     loop {
         // Block while we're reading into the buffer
         io.readi(&mut buf).unwrap();
@@ -184,6 +187,22 @@ fn main() {
             panic!("Invalid sample rate");
         }
 
+        let now = Instant::now();
+        let dt = (now - last_update).as_secs_f64();
+        assert!(dt > 0f64);
+
+        let pt = globals.period as f64 / sample_rate as f64;
+        /* If we skipped at least 4 periods, run catchup for that minus one */
+        if dt > (4f64 * pt) {
+            let skip = dt - pt;
+            debug!("Skipping {:.2} seconds", skip);
+            for (_, group) in groups.iter_mut() {
+                group.speakers.iter_mut().for_each(|s| s.skip_model(skip));
+            }
+        }
+
+        last_update = now;
+
         for (idx, group) in groups.iter_mut() {
             let gain = group
                 .speakers
diff --git a/src/types.rs b/src/types.rs
index 650baed..1d02097 100644
--- a/src/types.rs
+++ b/src/types.rs
@@ -354,6 +354,24 @@ impl Speaker {
         s.gain
     }
 
+    pub fn skip_model(&mut self, time: f64) {
+        let s = &mut self.s;
+        let t_coil = s.t_coil - self.g.t_ambient as f64;
+        let t_magnet = s.t_magnet - self.g.t_ambient as f64;
+
+        let eta = 1f64 / (1f64 - (self.tau_coil / self.tau_magnet) as f64);
+        let a = (-time / self.tau_coil as f64).exp() * (t_coil - eta * t_magnet);
+        let b = (-time / self.tau_magnet as f64).exp() * t_magnet;
+
+        s.t_coil = self.g.t_ambient as f64 + a + b * eta;
+        s.t_magnet = self.g.t_ambient as f64 + b;
+
+        debug!(
+            "{}: SKIP: Coil {:.2} °C Magnet {:.2} °C ({:.2} seconds)",
+            self.name, s.t_coil, s.t_magnet, time
+        );
+    }
+
     pub fn update(&mut self, ctl: &Ctl, gain: f32) {
         self.alsa_iface.set_lvl(ctl, gain);
     }