Dynamically adapt audio speed to keep latency low

5 files changed, 101 insertions, 16 deletions

diff --git a/src/consts.rs b/src/consts.rs
index 2b93133..3e464e3 100644
--- a/src/consts.rs
+++ b/src/consts.rs
@@ -8,5 +8,5 @@ pub const DISPLAY_UPDATE_SLEEP_TIME_MICROS: u64 =
     ((1000000 / DISPLAY_UPDATE_RATE) as f64 / SPEEDUP_FACTOR) as u64;
 
 pub const CPU_CLOCK_SPEED: u64 = 4_194_304;
-pub const CPU_CYCLE_LENGTH_NANOS: u64 =
-    ((1_000_000_000 / CPU_CLOCK_SPEED) as f64 / SPEEDUP_FACTOR) as u64;
+pub const CPU_CYCLE_LENGTH_NANOS: f64 =
+    ((1_000_000_000. / CPU_CLOCK_SPEED as f64) / SPEEDUP_FACTOR) as f64;
diff --git a/src/desktop/audio.rs b/src/desktop/audio.rs
index a6905f8..42dc129 100644
--- a/src/desktop/audio.rs
+++ b/src/desktop/audio.rs
@@ -1,15 +1,63 @@
-use rodio::{OutputStream, Sink, Source};
+use rodio::{Sink, Source};
+use rodio::stream::{OutputStreamBuilder, OutputStream};
+use cpal::BufferSize;
 
 use crate::audio::{MutableWave, SAMPLE_RATE};
 use crate::io::{Audio, Wave};
+use crate::logs::{log, LogLevel};
 use std::mem;
-use std::time::Duration;
+use std::time::{SystemTime, Duration};
+
+const BUFFER_SIZE: usize = 256;
+const RODIO_BUFFER_SIZE: usize = 1024;
+const RODIO_BUFFER_SINK_LATE_EXPECTED: f32 = 0.;
+const LATE_SPEEDUP_INTENSITY_INV: f32 = 2048.0;
+const SPEEDUP_SKIP_LIMIT: f32 = 1.008;
+
+const TIME_RING_BUFFER_SIZE: usize = (SAMPLE_RATE as usize / BUFFER_SIZE) * 10;
+struct SpeedFinder {
+    buf: [SystemTime; TIME_RING_BUFFER_SIZE],
+    i: usize,
+    has_circled: bool,
+}
+
+impl SpeedFinder {
+    fn new() -> Self {
+        Self {
+            buf: [SystemTime::now(); TIME_RING_BUFFER_SIZE],
+            i: 0,
+            has_circled: false,
+        }
+    }
+
+    fn tick(&mut self) -> Option<f32> {
+        if self.i >= TIME_RING_BUFFER_SIZE {
+            self.i = 0;
+            self.has_circled = true;
+        }
 
-const BUFFER_SIZE: usize = 1024;
+        let previous = self.buf[self.i];
+        let now = SystemTime::now();
+
+        self.buf[self.i] = now;
+        self.i += 1;
+        if !self.has_circled {
+            if self.i == 1 {
+                return None;
+            } else {
+                return Some(now.duration_since(self.buf[0]).unwrap().as_secs_f32() / (self.i - 1) as f32);
+            }
+        } else {
+            return Some(now.duration_since(previous).unwrap().as_secs_f32() / TIME_RING_BUFFER_SIZE as f32);
+        }
+    }
+}
 
 pub struct RodioAudio {
-    _stream: OutputStream,
     sink: Sink,
+    stream: OutputStream,
+
+    speed_finder: SpeedFinder,
     wave: RodioWave<MutableWave>,
     buffer: Box<[f32; BUFFER_SIZE]>,
     buffer_i: usize,
@@ -40,7 +88,7 @@ impl<I: Iterator<Item = f32>> Iterator for RodioBuffer<I> {
 }
 
 impl<I: Iterator<Item = f32>> Source for RodioBuffer<I> {
-    fn current_frame_len(&self) -> Option<usize> {
+    fn current_span_len(&self) -> Option<usize> {
         None
     }
 
@@ -59,17 +107,18 @@ impl<I: Iterator<Item = f32>> Source for RodioBuffer<I> {
 
 impl Audio for RodioAudio {
     fn new(wave: MutableWave) -> Self {
-        let (stream, stream_handle) = OutputStream::try_default().unwrap();
+        let stream = OutputStreamBuilder::from_default_device().unwrap().with_sample_rate(SAMPLE_RATE).with_buffer_size(BufferSize::Fixed(RODIO_BUFFER_SIZE as u32)).open_stream().unwrap();
 
-        let sink = Sink::try_new(&stream_handle).unwrap();
+        let sink = Sink::connect_new(stream.mixer());
         let wave = RodioWave(wave, 0);
 
         RodioAudio {
-            _stream: stream,
+            speed_finder: SpeedFinder::new(),
             sink: sink,
             wave,
             buffer: Box::new([0.0; BUFFER_SIZE]),
             buffer_i: 0,
+            stream,
         }
     }
 
@@ -82,6 +131,26 @@ impl Audio for RodioAudio {
                 self.buffer_i = 0;
                 let mut buffer = Box::new([0.0; BUFFER_SIZE]);
                 mem::swap(&mut self.buffer, &mut buffer);
+                if let Some(speed) = self.speed_finder.tick() {
+                    let mut late_speedup: f32;
+                    let rodio_buffers_sink_late = self.sink.len() as f32 / (RODIO_BUFFER_SIZE / BUFFER_SIZE) as f32;
+                    late_speedup = ((rodio_buffers_sink_late - RODIO_BUFFER_SINK_LATE_EXPECTED).powi(3) / LATE_SPEEDUP_INTENSITY_INV) + 1.;
+
+                    if late_speedup > SPEEDUP_SKIP_LIMIT {
+                        while late_speedup > 1.0 {
+                            let rodio_buffers_sink_late = self.sink.len() as f32 / (RODIO_BUFFER_SIZE / BUFFER_SIZE) as f32;
+                            late_speedup = ((rodio_buffers_sink_late - RODIO_BUFFER_SINK_LATE_EXPECTED).powi(3) / LATE_SPEEDUP_INTENSITY_INV) + 1.;
+
+                            self.sink.skip_one();
+                        }
+                        late_speedup = 1.;
+                    }
+                    let average_speed = (1./speed) / (2 * SAMPLE_RATE / BUFFER_SIZE as u32) as f32;
+                    let rodio_buffers_sink_late = self.sink.len() as f32 / (RODIO_BUFFER_SIZE / BUFFER_SIZE) as f32;
+                    log(LogLevel::AudioLatency, format!("audio sink latency: {}ms", (1000. * rodio_buffers_sink_late / ((2*SAMPLE_RATE) as f32 / RODIO_BUFFER_SIZE as f32))));
+
+                    self.sink.set_speed(late_speedup * average_speed);
+                }
                 self.sink.append(RodioBuffer(buffer.into_iter()));
             }
         }
diff --git a/src/io.rs b/src/io.rs
index 8ed35e8..f2ed8d0 100644
--- a/src/io.rs
+++ b/src/io.rs
@@ -164,11 +164,21 @@ impl<I: Input, W: Window, S: Serial, A: Audio, LS: LoadSave> Gameboy<I, W, S, A,
 
         let mut last_ram_bank_enabled = false;
         let mut now = SystemTime::now();
+        let mut last_halt_cycle = now;
+        let mut last_halt_cycle_counter: u128 = 0;
         let mut next_precise_gamepad_update: Option<u128> = None;
 
         while !state.is_stopped {
             if was_previously_halted && !state.mem.halt {
-                log(LogLevel::HaltCycles, format!("Halt cycles {}", halt_time));
+                let n = SystemTime::now();
+                log(
+                    LogLevel::HaltCycles,
+                    format!(
+                        "Halt cycles {} (system average speed: {}Hz)",
+                        halt_time,
+                        last_halt_cycle_counter as f32 / n.duration_since(last_halt_cycle).unwrap().as_secs_f32(),
+                    )
+                );
                 halt_time = 0;
             }
             was_previously_halted = state.mem.halt;
@@ -179,6 +189,7 @@ impl<I: Input, W: Window, S: Serial, A: Audio, LS: LoadSave> Gameboy<I, W, S, A,
                 4
             };
 
+            last_halt_cycle_counter += c as u128;
             state.cpu.dbg_cycle_counter += c;
             total_cycle_counter += c as u128;
             audio_counter += c;
@@ -194,7 +205,7 @@ impl<I: Input, W: Window, S: Serial, A: Audio, LS: LoadSave> Gameboy<I, W, S, A,
             state.check_interrupts();
             state.mem.update_serial(total_cycle_counter);
 
-            nanos_sleep += c as f64 * (consts::CPU_CYCLE_LENGTH_NANOS as f64 / *speed) as f64;
+            nanos_sleep += c as f64 * (consts::CPU_CYCLE_LENGTH_NANOS / *speed) as f64;
 
             if nanos_sleep >= 0.0
                 || next_precise_gamepad_update.map_or(false, |c| (c >= total_cycle_counter))
@@ -233,11 +244,12 @@ impl<I: Input, W: Window, S: Serial, A: Audio, LS: LoadSave> Gameboy<I, W, S, A,
                     }
                 }
 
-                thread::sleep(time::Duration::from_nanos(nanos_sleep as u64 / 10));
+                thread::sleep(time::Duration::from_nanos(1)); //nanos_sleep as u64));
 
+                let new_now = SystemTime::now();
                 nanos_sleep =
-                    nanos_sleep - SystemTime::now().duration_since(now).unwrap().as_nanos() as f64;
-                now = SystemTime::now();
+                    nanos_sleep - new_now.duration_since(now).unwrap().as_nanos() as f64;
+                now = new_now;
 
                 if last_ram_bank_enabled && !state.mem.ram_bank_enabled {
                     if let Err(err) = load_save.save_external_ram(state.mem.external_ram.as_ref()) {
diff --git a/src/logs.rs b/src/logs.rs
index 4eefc3e..cf00cae 100644
--- a/src/logs.rs
+++ b/src/logs.rs
@@ -7,6 +7,7 @@ pub enum LogLevel {
     Debug,
     OpcodeDump,
     HaltCycles,
+    AudioLatency,
     Error,
 }
 
@@ -31,6 +32,9 @@ pub fn set_log_level(verbosity: String) {
             "errors" => {
                 set.insert(LogLevel::Error);
             }
+            "audio_latency" => {
+                set.insert(LogLevel::AudioLatency);
+            }
             "none" => {}
             _ => panic!("Unknown log level \"{}\"", level),
         }
diff --git a/src/main.rs b/src/main.rs
index ee815f8..3f71987 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -84,7 +84,7 @@ struct Cli {
     #[arg(long, default_value_t = false)]
     no_response: bool,
 
-    /// Verbosity. Coma separated values (possible values: infos,debug,opcode_dump,halt_cycles,errors,none)
+    /// Verbosity. Coma separated values (possible values: infos,debug,opcode_dump,halt_cycles,audio_latency,errors,none)
     #[arg(short, long, default_value = "infos,errors")]
     verbosity: String,
 }