From 844e74777f9201f77502996369083ea18fbaa9fe Mon Sep 17 00:00:00 2001
From: Samuel Walker <swalker@piwalker.net>
Date: Fri, 14 Feb 2025 18:18:06 -0700
Subject: [PATCH] fixed high pass filter

---
 lib/audio.c | 142 ++++++++++++++++++++++++++++++++++------------------
 1 file changed, 93 insertions(+), 49 deletions(-)

diff --git a/lib/audio.c b/lib/audio.c
index 0e7ee4f..3ad84c1 100644
--- a/lib/audio.c
+++ b/lib/audio.c
@@ -23,8 +23,8 @@ static float wave_time = 0;
 static float lfsr_timer = 0;
 static float lfsr_clock = LFSR_BASE_CLOCK;
 
-static float left_cap = 0.0;
-static float right_cap = 0.0;
+static float left_cap = 0.0f;
+static float right_cap = 0.0f;
 
 const u8 square_sample_00[8] = {
     0x0,
@@ -83,9 +83,9 @@ static int audio_callback(const void* input_uffer, void *output_buffer,
                             PaStreamCallbackFlags status_flags,
                             void *userData ) {
     float *out = (float *)output_buffer;
-    float left = 0;
-    float right = 0;
     for(int i = 0; i < framesPerBuffer; i++) {
+        float left = 0;
+        float right = 0;
         audio_time += TIME_PER_SAMPLE;
         wave_time += TIME_PER_SAMPLE;
         for(;audio_time >= TIME_PER_AUDIO_TICK;audio_time -= TIME_PER_AUDIO_TICK) {
@@ -132,52 +132,92 @@ static int audio_callback(const void* input_uffer, void *output_buffer,
         float ch3_val = 0;
         float ch4_val = 0;
         if(ctx.audio_enabled){
-            if(ctx.sq1_enable) {
-                sq1_val = ((float)ctx.sq1_volume/15.0f) * (((float)(square_sample[ctx.sq1_duty][ctx.sq1_sample]) - 7.5f)/7.5f);
-                if(ctx.ch1_left) {
-                    left += sq1_val;
-                }
-                if(ctx.ch1_right) {
-                    right += sq1_val;
+            if(ctx.ch1_dac){
+                sq1_val = -1;
+                if(ctx.sq1_enable) {
+                    sq1_val = ((float)ctx.sq1_volume/15.0f) * (((float)(square_sample[ctx.sq1_duty][ctx.sq1_sample]) - 7.5f)/7.5f);
+                    if(ctx.ch1_left) {
+                        left += sq1_val;
+                    }else {
+                        left -= 1;
+                    }
+                    if(ctx.ch1_right) {
+                        right += sq1_val;
+                    }else {
+                        right -= 1;
+                    }
+                } else {
+                    left -= 1;
+                    right -= 1;
                 }
             }
-
-            if(ctx.sq2_enable) {
-                sq2_val = ((float)ctx.sq2_volume/15.0f) * (((float)(square_sample[ctx.sq2_duty][ctx.sq2_sample]) - 7.5f)/7.5f);
-                if(ctx.ch2_left) {
-                    left += sq2_val;
-                }
-                if(ctx.ch2_right) {
-                    right += sq2_val;
+            if(ctx.ch2_dac){
+                sq2_val = -1;
+                if(ctx.sq2_enable) {
+                    sq2_val = ((float)ctx.sq2_volume/15.0f) * (((float)(square_sample[ctx.sq2_duty][ctx.sq2_sample]) - 7.5f)/7.5f);
+                    if(ctx.ch2_left) {
+                        left += sq2_val;
+                    }else {
+                        left -= 1;
+                    }
+                    if(ctx.ch2_right) {
+                        right += sq2_val;
+                    }else {
+                        right -= 1;
+                    }
+                } else {
+                    left -= 1;
+                    right -= 1;
                 }
             }
-
-            if(ctx.ch3_enable && ctx.ch3_volume != 0x0) {
-                u8 shift = 0;
-                if(ctx.ch3_volume == 0b10) {
-                    shift = 1;
-                }
-                if(ctx.ch3_volume == 0b11) {
-                    shift = 2;
-                }
-                ch3_val = (((float)(ctx.ch3_last_sample >> shift) - 7.5f)/15.0f);
-                if(ctx.ch3_left) {
-                    left += ch3_val;
-                    //printf("left: %d\n", ctx.ch3_volume);
-                }
-                if(ctx.ch3_right) {
-                    right += ch3_val;
+            if(ctx.ch3_dac){
+                ch3_val = -1;
+                if(ctx.ch3_enable) {
+                    u8 shift = 0;
+                    if(ctx.ch3_volume == 0b10) {
+                        shift = 1;
+                    }
+                    if(ctx.ch3_volume == 0b00) {
+                        shift = 4;
+                    }
+                    if(ctx.ch3_volume == 0b11) {
+                        shift = 2;
+                    }
+                    ch3_val = (((float)(ctx.ch3_last_sample >> shift) - 7.5f)/15.0f);
+                    if(ctx.ch3_left) {
+                        left += ch3_val;
+                        //printf("left: %d\n", ctx.ch3_volume);
+                    }else {
+                        left -= 1;
+                    }
+                    if(ctx.ch3_right) {
+                        right += ch3_val;
+                    }else {
+                        right -= 1;
+                    }
+                } else {
+                    left -= 1;
+                    right -= 1;
                 }
             }
-
-            if(ctx.ch4_enable) {
-                ch4_val = ((float)ctx.ch4_volume/15.0f) * ((ctx.ch4_lfsr & 0b1) ? 0 : 1.0f);
-                if(ctx.ch4_left) {
-                    left += ch4_val;
-                    //printf("left: %d\n", ctx.ch3_volume);
-                }
-                if(ctx.ch4_right) {
-                    right += ch4_val;
+            if(ctx.ch4_dac){
+                ch4_val = -1;
+                if(ctx.ch4_enable) {
+                    ch4_val = (ctx.ch4_lfsr & 0b1) ? (((float)ctx.ch4_volume - 7.5f)/7.5f) : -1.0f;
+                    if(ctx.ch4_left) {
+                        left += ch4_val;
+                        //printf("left: %d\n", ctx.ch3_volume);
+                    }else {
+                        left -= 1;
+                    }
+                    if(ctx.ch4_right) {
+                        right += ch4_val;
+                    } else {
+                        right -= 1;
+                    }
+                } else {
+                    left -= 1;
+                    right -= 1;
                 }
             }
         }
@@ -205,12 +245,9 @@ static int audio_callback(const void* input_uffer, void *output_buffer,
         left *= (float)left_vol/7.0f;
         right *= (float)right_vol/7.0f;
 
-        left /= 4;
-        right /= 4;
+        //left /= 4;
+        //right /= 4;
 
-        if(left > 1.0f) {
-            printf("Uh Oh! %02X\n", ctx.volume_left);
-        }
 
         bool dacs = ctx.ch1_dac || ctx.ch2_dac || ctx.ch3_dac || ctx.ch4_dac;
         float left_out = 0;
@@ -222,6 +259,13 @@ static int audio_callback(const void* input_uffer, void *output_buffer,
             right_cap = right - right_out * 0.996f;
         }
 
+        
+        left_out /= 4;
+        right_out /= 4;
+
+        if(left_out < -1.0f) {
+            printf("Uh Oh! %f\n", left_out);
+        }
         ctx.left_history[ctx.left_index++] = left_out;
         if (ctx.left_index == 4410) {
             ctx.left_index = 0;