From 8d90da2e6d986055ebc197d244667f9bb6b58216 Mon Sep 17 00:00:00 2001
From: Alex Janka <alex@alexjanka.com>
Date: Fri, 17 Feb 2023 12:17:20 +1100
Subject: [PATCH] implement functionality behind div-apu ticks

---
 src/processor/memory/mmio/apu.rs          |  4 +++
 src/processor/memory/mmio/apu/channels.rs | 36 ++++++++++++++++++++++-
 2 files changed, 39 insertions(+), 1 deletion(-)

diff --git a/src/processor/memory/mmio/apu.rs b/src/processor/memory/mmio/apu.rs
index daaaba7..4fafd6d 100644
--- a/src/processor/memory/mmio/apu.rs
+++ b/src/processor/memory/mmio/apu.rs
@@ -140,12 +140,16 @@ impl Apu {
         if self.div_apu % 8 == 0 {
             // envelope sweep
             self.channels.one.envelope_tick();
+            self.channels.two.envelope_tick();
         }
         if self.div_apu % 4 == 0 {
             // ch1 frequency sweep
+            self.channels.one.frequency_tick();
         }
         if self.div_apu % 2 == 0 {
             // tick sound length timers
+            self.channels.one.length_tick();
+            self.channels.two.length_tick();
         }
     }
 
diff --git a/src/processor/memory/mmio/apu/channels.rs b/src/processor/memory/mmio/apu/channels.rs
index c4249ea..35d491b 100644
--- a/src/processor/memory/mmio/apu/channels.rs
+++ b/src/processor/memory/mmio/apu/channels.rs
@@ -10,6 +10,7 @@ struct Sweep {
     pace: u8,
     mode: EnvelopeMode,
     slope: u8,
+    counter: u8,
 }
 
 impl Default for Sweep {
@@ -18,6 +19,7 @@ impl Default for Sweep {
             pace: 0,
             mode: EnvelopeMode::Increase,
             slope: 0,
+            counter: 0,
         }
     }
 }
@@ -41,6 +43,7 @@ impl Envelope {
             counter: 0,
         }
     }
+
     fn tick(&mut self) {
         if self.rate == 0 {
             return;
@@ -167,7 +170,38 @@ impl PwmChannel {
     }
 
     pub(super) fn envelope_tick(&mut self) {
-        self.envelope.tick();
+        if self.enabled {
+            self.envelope.tick();
+        }
+    }
+
+    pub(super) fn frequency_tick(&mut self) {
+        if self.sweep.slope == 0 || !self.enabled {
+            return;
+        }
+        self.sweep.counter += 1;
+        if self.sweep.counter % self.sweep.pace == 0 {
+            self.sweep.counter = 0;
+            let wavelength_diff = self.wavelength / (2_u16.pow(self.sweep.slope as u32));
+            let new_wavelength = match self.sweep.mode {
+                EnvelopeMode::Increase => self.wavelength + wavelength_diff,
+                EnvelopeMode::Decrease => self.wavelength.saturating_sub(wavelength_diff),
+            };
+            if new_wavelength > 0x7FF {
+                self.enabled = false;
+            } else {
+                self.wavelength = new_wavelength;
+            }
+        }
+    }
+
+    pub(super) fn length_tick(&mut self) {
+        if self.length_enable && self.enabled {
+            self.length_timer += 1;
+            if self.length_timer >= 64 {
+                self.enabled = false;
+            }
+        }
     }
 
     pub(super) fn update_sweep(&mut self, pace: u8, mode: EnvelopeMode, slope: u8) {