From f42f58a6632d932af34819f8044a0ed9b4dfa86c Mon Sep 17 00:00:00 2001
From: Alex Janka <alex@alexjanka.com>
Date: Tue, 14 Feb 2023 08:23:24 +1100
Subject: [PATCH] more accurate timing

---
 src/processor/mod.rs   |  7 +++++++
 src/processor/timer.rs | 10 ++++++++--
 2 files changed, 15 insertions(+), 2 deletions(-)

diff --git a/src/processor/mod.rs b/src/processor/mod.rs
index b064df3..5c0a2f7 100644
--- a/src/processor/mod.rs
+++ b/src/processor/mod.rs
@@ -1,3 +1,5 @@
+use std::time::Instant;
+
 use self::{gpu::Gpu, memory::Memory, timer::Timers};
 use crate::{
     util::{clear_bit, get_bit},
@@ -35,6 +37,7 @@ pub struct Cpu {
     halted: bool,
     timers: Timers,
     gamepad_handler: Gilrs,
+    cycle_start: Instant,
 }
 
 // Hz
@@ -62,15 +65,18 @@ impl Cpu {
             halted: false,
             timers: Timers::init(),
             gamepad_handler,
+            cycle_start: Instant::now(),
         }
     }
 
     pub fn exec_next(&mut self) {
+        self.cycle_start = Instant::now();
         let interrupt_cycles = self.handle_interrupts();
         self.increment_timers(interrupt_cycles);
 
         if self.halted {
             self.increment_timers(1);
+            self.sleep(interrupt_cycles + 1);
             return;
         }
 
@@ -94,6 +100,7 @@ impl Cpu {
         let cycles = self.run_opcode(opcode);
         self.memory.user_mode = false;
         self.increment_timers(cycles);
+        self.sleep(interrupt_cycles + cycles);
     }
 
     fn next_opcode(&mut self) -> u8 {
diff --git a/src/processor/timer.rs b/src/processor/timer.rs
index ca0e882..be8444c 100644
--- a/src/processor/timer.rs
+++ b/src/processor/timer.rs
@@ -44,9 +44,15 @@ impl Cpu {
                 self.memory.set(0xFF05, val);
             }
         }
+    }
 
-        let secs = clock_cycles as f64 / CLOCK_SPEED as f64;
-        spin_sleep::sleep(Duration::from_secs_f64(secs / SPEEDUP));
+    pub(super) fn sleep(&mut self, machine_cycles: u8) {
+        let secs = ((machine_cycles as f64) * 4.) / CLOCK_SPEED as f64;
+        if let Some(remaining) =
+            Duration::from_secs_f64(secs / SPEEDUP).checked_sub(self.cycle_start.elapsed())
+        {
+            spin_sleep::sleep(remaining);
+        }
     }
 
     fn timer_scale(&self) -> (bool, usize) {