From 063af3fc400fe550da9061eba75d9888c9e83cf2 Mon Sep 17 00:00:00 2001
From: Gwilym Kuiper <gw@ilym.me>
Date: Thu, 10 Jun 2021 23:34:03 +0100
Subject: [PATCH] Panning working?

---
 agb/src/number.rs            |  9 +++++++
 agb/src/sound/mixer/mixer.rs | 51 ++++++++++++++++++++----------------
 agb/src/sound/mixer/mod.rs   | 11 ++++++++
 3 files changed, 48 insertions(+), 23 deletions(-)

diff --git a/agb/src/number.rs b/agb/src/number.rs
index bb22a1a..d9ac36a 100644
--- a/agb/src/number.rs
+++ b/agb/src/number.rs
@@ -103,6 +103,15 @@ impl<I: FixedWidthUnsignedInteger, const N: usize> From<I> for Num<I, N> {
     }
 }
 
+impl<I, const N: usize> Default for Num<I, N>
+where
+    I: FixedWidthUnsignedInteger,
+{
+    fn default() -> Self {
+        Num(I::zero())
+    }
+}
+
 impl<I, T, const N: usize> Add<T> for Num<I, N>
 where
     I: FixedWidthUnsignedInteger,
diff --git a/agb/src/sound/mixer/mixer.rs b/agb/src/sound/mixer/mixer.rs
index 1d4602a..f79d064 100644
--- a/agb/src/sound/mixer/mixer.rs
+++ b/agb/src/sound/mixer/mixer.rs
@@ -1,6 +1,7 @@
 use super::hw;
 use super::hw::LeftOrRight;
 use super::SoundChannel;
+use crate::number::Num;
 
 pub struct Mixer {
     buffer: MixerBuffer,
@@ -24,13 +25,8 @@ impl Mixer {
         self.buffer.swap();
         self.buffer.clear();
 
-        for channel in self.channels.iter_mut() {
-            if let Some(some_channel) = channel {
-                if self.buffer.write_channel(some_channel) {
-                    channel.take();
-                }
-            }
-        }
+        self.buffer
+            .write_channels(self.channels.iter_mut().flatten());
     }
 
     pub fn play_sound(&mut self, new_channel: SoundChannel) {
@@ -82,28 +78,37 @@ impl MixerBuffer {
         self.get_write_buffer().fill(0);
     }
 
-    fn write_channel(&mut self, channel: &mut SoundChannel) -> bool {
-        let place_to_write_to = self.get_write_buffer();
-        let mut current_point = channel.pos;
+    fn write_channels<'a>(&mut self, channels: impl Iterator<Item = &'a mut SoundChannel>) {
+        let mut buffer: [Num<i16, 4>; SOUND_BUFFER_SIZE * 2] = [Num::new(0); SOUND_BUFFER_SIZE * 2];
 
-        for i in 0..SOUND_BUFFER_SIZE {
-            let v = channel.data[current_point.floor()];
-            current_point += channel.playback_speed;
+        for channel in channels {
+            let mut current_point = channel.pos;
 
-            if current_point.floor() >= channel.data.len() {
-                if channel.should_loop {
-                    channel.pos -= channel.data.len();
-                } else {
-                    return true;
+            let right_amount = (channel.panning - 1) / 2;
+            let left_amount = -right_amount + 1;
+
+            for i in 0..SOUND_BUFFER_SIZE {
+                let v = (channel.data[current_point.floor()] as i8) as i16;
+                let v: Num<i16, 4> = v.into();
+                current_point += channel.playback_speed;
+
+                if current_point.floor() >= channel.data.len() {
+                    if channel.should_loop {
+                        channel.pos -= channel.data.len();
+                    } else {
+                        continue;
+                    }
                 }
-            }
 
-            place_to_write_to[i] = place_to_write_to[i].saturating_add(v as i8);
-            place_to_write_to[i + SOUND_BUFFER_SIZE] =
-                place_to_write_to[i + SOUND_BUFFER_SIZE].saturating_add(v as i8);
+                buffer[i] += v * left_amount;
+                buffer[i + SOUND_BUFFER_SIZE] += v * right_amount;
+            }
         }
 
-        false
+        let write_buffer = self.get_write_buffer();
+        for i in 0..SOUND_BUFFER_SIZE * 2 {
+            write_buffer[i] = buffer[i].floor().clamp(i8::MIN as i16, i8::MAX as i16) as i8
+        }
     }
 
     fn get_write_buffer(&mut self) -> &mut [i8; SOUND_BUFFER_SIZE * 2] {
diff --git a/agb/src/sound/mixer/mod.rs b/agb/src/sound/mixer/mod.rs
index 3acc8bc..280f111 100644
--- a/agb/src/sound/mixer/mod.rs
+++ b/agb/src/sound/mixer/mod.rs
@@ -24,6 +24,8 @@ pub struct SoundChannel {
     should_loop: bool,
 
     playback_speed: Num<usize, 8>,
+
+    panning: Num<i16, 4>, // between -1 and 1
 }
 
 impl SoundChannel {
@@ -33,6 +35,7 @@ impl SoundChannel {
             pos: 0.into(),
             should_loop: false,
             playback_speed: 1.into(),
+            panning: 0.into(),
         }
     }
 
@@ -45,4 +48,12 @@ impl SoundChannel {
         self.playback_speed = playback_speed;
         self
     }
+
+    pub fn panning(mut self, panning: Num<i16, 4>) -> Self {
+        debug_assert!(panning >= Num::new(-1), "panning value must be >= -1");
+        debug_assert!(panning <= Num::new(1), "panning value must be <= 1");
+
+        self.panning = panning;
+        self
+    }
 }