From e67043595f559bc3b6eaedf5e228f323b046cc9b Mon Sep 17 00:00:00 2001
From: Steve Lascos <steve@blackaddr.com>
Date: Thu, 15 Aug 2019 19:44:55 -0400
Subject: [PATCH] converted to heap arrays to save stack space

---
 .../PitchShiftExpansion.ino                   |  20 ++-
 src/AudioEffectPitchShift.h                   |  60 +++++--
 src/effects/AudioEffectPitchShift.cpp         | 165 ++++++++++++------
 3 files changed, 175 insertions(+), 70 deletions(-)

diff --git a/examples/Modulation/PitchShiftExpansion/PitchShiftExpansion.ino b/examples/Modulation/PitchShiftExpansion/PitchShiftExpansion.ino
index c55dd09..880ce3c 100644
--- a/examples/Modulation/PitchShiftExpansion/PitchShiftExpansion.ino
+++ b/examples/Modulation/PitchShiftExpansion/PitchShiftExpansion.ino
@@ -45,7 +45,7 @@ AudioConnection rightOut(cabFilter,0, i2sOut, 1);
 
 //////////////////////////////////////////
 // SETUP PHYSICAL CONTROLS
-// - POT1 (left) will control the rate
+// - POT1 (left) will control the pitch
 // - POT2 (right) will control the depth
 // - POT3 (centre) will control the volume
 // - SW1  (left) will be used as a bypass control
@@ -68,7 +68,7 @@ constexpr unsigned MAX_HEADPHONE_VOL = 10;
 unsigned headphoneVolume = 8; // control headphone volume from 0 to 10.
 
 // BAPhysicalControls returns a handle when you register a new control. We'll uses these handles when working with the controls.
-int bypassHandle, volumeHandle, led1Handle, led2Handle; // Handles for the various controls
+int bypassHandle, volumeHandle, pitchHandle, led1Handle, led2Handle; // Handles for the various controls
 
 void setup() {
   delay(100); // wait a bit for serial to be available
@@ -80,7 +80,7 @@ void setup() {
   bypassHandle = controls.addSwitch(BA_EXPAND_SW1_PIN); // will be used for bypass control
   //button2Handle = controls.addSwitch(BA_EXPAND_SW2_PIN); // will be used for stepping through filters
   // pots
-  //rateHandle   = controls.addPot(BA_EXPAND_POT1_PIN, potCalibMin, potCalibMax, potSwapDirection); // control the amount of delay
+  pitchHandle   = controls.addPot(BA_EXPAND_POT1_PIN, potCalibMin, potCalibMax, potSwapDirection); // control the amount of delay
   //depthHandle  = controls.addPot(BA_EXPAND_POT2_PIN, potCalibMin, potCalibMax, potSwapDirection); 
   volumeHandle = controls.addPot(BA_EXPAND_POT3_PIN, potCalibMin, potCalibMax, potSwapDirection); 
   // leds
@@ -102,6 +102,7 @@ void setup() {
 
   // Set some default values.
   // These can be changed using the controls on the Blackaddr Audio Expansion Board
+  pitchShift.setPitchShiftCents(0);
   pitchShift.bypass(false);
 
   // Guitar cabinet: Setup 2-stages of LPF, cutoff 4500 Hz, Q-factor 0.7071 (a 'normal' Q-factor)
@@ -127,12 +128,13 @@ void loop() {
 //  if (controls.isSwitchToggled(waveformHandle)) {
 //  }
 
-//  // Use POT1 (left) to control the rate setting
-//  if (controls.checkPotValue(rateHandle, potValue)) {
-//    // Pot has changed
-//    Serial.println(String("New RATE setting: ") + potValue);
-//    pitchShift.rate(potValue);
-//  }
+  // Use POT1 (left) to control the rate setting
+  if (controls.checkPotValue(pitchHandle, potValue)) {
+    // Pot has changed
+    
+    float pitch = pitchShift.setPitchKnob(potValue);
+    Serial.println(String("New PITCH setting: ") + potValue + String(" / ") + pitch);
+  }
 
 //  // Use POT2 (right) to control the depth setting
 //  if (controls.checkPotValue(depthHandle, potValue)) {
diff --git a/src/AudioEffectPitchShift.h b/src/AudioEffectPitchShift.h
index 2526031..c8cab4c 100644
--- a/src/AudioEffectPitchShift.h
+++ b/src/AudioEffectPitchShift.h
@@ -26,6 +26,22 @@
 #include "BATypes.h"
 #include "LibBasicFunctions.h"
 
+//#define USE_INT32
+#define USE_FLOAT
+//#define USE_INT16
+
+#ifdef USE_FLOAT
+using fftType_t = float;
+using fftInstance_t = arm_cfft_radix4_instance_f32;
+#define fftInit(A,B,C,D) arm_cfft_radix4_init_f32(A,B,C,D);
+#define int16ToFft(A,B,C) arm_q15_to_float(A,B,C)
+#define fftToInt16(A,B,C) arm_float_to_q15(A,B,C)
+#define fft(A,B) arm_cfft_radix4_f32(A,B)
+#define fastCos(X) arm_cos_f32(X)
+#define fastSin(X) arm_sin_f32(X)
+#endif
+
+
 namespace BAEffects {
 
 /**************************************************************************//**
@@ -35,16 +51,21 @@ class AudioEffectPitchShift : public AudioStream {
 public:
 
     static constexpr unsigned ANALYSIS_SIZE = 1024;
+    static constexpr float    ANALYSIS_SIZE_F = (float)ANALYSIS_SIZE;
+
     static constexpr unsigned FFT_OVERSAMPLE_FACTOR = 1;
-    static constexpr float    FFT_OVERSAMPLE_FACTOR_F = 1.0f;
+    static constexpr float    FFT_OVERSAMPLE_FACTOR_F = (float)(FFT_OVERSAMPLE_FACTOR);
+
     static constexpr unsigned SYNTHESIS_SIZE = ANALYSIS_SIZE * FFT_OVERSAMPLE_FACTOR;
-    static constexpr float    SYNTHESIS_SIZE_F =  (float)(ANALYSIS_SIZE * FFT_OVERSAMPLE_FACTOR);
+    static constexpr float    SYNTHESIS_SIZE_F =  (float)(SYNTHESIS_SIZE);
+
     static constexpr float OVERLAP_FACTOR_F = (float)ANALYSIS_SIZE / (float)AUDIO_BLOCK_SAMPLES;
 
 	///< List of AudioEffectTremolo MIDI controllable parameters
 	enum {
 		BYPASS = 0,  ///<  controls effect bypass
 		VOLUME,      ///< controls the output volume level
+		PITCH,       ///< controls the pitch scaling factor
 		NUM_CONTROLS ///< this can be used as an alias for the number of MIDI controls
 	};
 
@@ -72,6 +93,11 @@ public:
 	/// @param vol Sets the output volume between -1.0 and +1.0
 	void volume(float vol) {m_volume = vol; }
 
+	float setPitchKnob(float val);
+	float setPitchShiftCents(int shiftCents);
+
+	//void pitch(float pitchScale) { m_pitchScale = pitchScale; }
+
 	// ** ENABLE  / DISABLE **
 
 	/// Enables audio processing. Note: when not enabled, CPU load is nearly zero.
@@ -107,18 +133,28 @@ public:
 
 private:
 	audio_block_t *m_inputQueueArray[1];
-	//BALibrary::RingBuffer<audio_block_t*> m_inputFifo = BALibrary::RingBuffer<audio_block_t*>(ANALYSIS_SIZE/AUDIO_BLOCK_SAMPLES);
-	float m_analysisBuffer[ANALYSIS_SIZE];
-	float m_analysisFreqBuffer[2*ANALYSIS_SIZE];
-	float m_synthesisFreqBuffer[2*SYNTHESIS_SIZE];
-	float m_synthesisBuffer[SYNTHESIS_SIZE];
+
+	//fftType_t m_analysisBuffer[ANALYSIS_SIZE];
+	//float m_windowFunction[ANALYSIS_SIZE];
+
+	//fftType_t m_analysisFreqBuffer[2*SYNTHESIS_SIZE];
+	//fftType_t m_synthesisFreqBuffer[2*SYNTHESIS_SIZE];
+	//fftType_t m_synthesisBuffer[SYNTHESIS_SIZE];
+
+	fftType_t *m_analysisBuffer = nullptr;
+	fftType_t *m_windowFunction = nullptr;
+	fftType_t *m_analysisFreqBuffer = nullptr;
+	fftType_t *m_synthesisFreqBuffer = nullptr;
+	fftType_t *m_synthesisBuffer = nullptr;
+	fftType_t *m_windowBuffer = nullptr;
+	fftType_t *m_outputBuffer = nullptr;
 
 	bool m_initFailed = false;
 
 	unsigned m_frameIndex = 0;
 
-//	arm_rfft_instance_f32 fftFwdReal, fftInvReal;
-//    arm_cfft_radix4_instance_f32 fftFwdComplex, fftInvComplex;
+	arm_rfft_instance_f32 fftFwdReal, fftInvReal;
+    arm_cfft_radix4_instance_f32 fftFwdComplex, fftInvComplex;
 //    float32_t *bufInputReal;
 //    float32_t *bufInputComplex;
 //    float32_t *bufOutputReal;
@@ -126,7 +162,7 @@ private:
 
     //arm_cfft_radix4_instance_f32 fft_inst_fwd, fft_inst_inv;
 	//arm_rfft_instance_f32 rfftForwardInst, rfftInverseInst;
-	arm_cfft_radix4_instance_f32 cfftForwardInst, cfftInverseInst;
+	fftInstance_t cfftForwardInst, cfftInverseInst;
 
     //uint8_t ifftFlag = 0; // 0 is FFT, 1 is IFFT
     //uint8_t doBitReverse = 1;
@@ -138,8 +174,10 @@ private:
 
 	float m_volume = 1.0f;
 	float m_pitchScale = 1.0f;
+	int m_shiftCents = 0;
 
-	void m_ocean(float *inputFreq, float *outputFreq, float frameIndex, float pitchScale);
+	void m_ocean(fftType_t *inputFreq, fftType_t *outputFreq, float frameIndex, float pitchScale);
+	//void m_ocean16(int16_t* inputFreq, int16_t* outputFreq, float frameIndex, float pitchScale);
 
 };
 
diff --git a/src/effects/AudioEffectPitchShift.cpp b/src/effects/AudioEffectPitchShift.cpp
index 160ae4a..297c15c 100644
--- a/src/effects/AudioEffectPitchShift.cpp
+++ b/src/effects/AudioEffectPitchShift.cpp
@@ -18,29 +18,54 @@ constexpr unsigned NUM_AUDIO_BLOCKS = AudioEffectPitchShift::ANALYSIS_SIZE / AUD
 constexpr uint32_t FFT_FORWARD = 0;
 constexpr uint32_t FFT_INVERSE = 1;
 constexpr uint32_t FFT_DO_BIT_REVERSE = 1;
+constexpr float WINDOW_GAIN = 0.5;
 
 AudioEffectPitchShift::AudioEffectPitchShift()
 : AudioStream(1, m_inputQueueArray)
 {
-    // clear the audio buffer to avoid pops
+    m_analysisBuffer = (fftType_t *)malloc(ANALYSIS_SIZE*sizeof(fftType_t));
+    m_windowFunction = (fftType_t *)malloc(ANALYSIS_SIZE*sizeof(fftType_t));
+    m_windowBuffer = (fftType_t *)malloc(SYNTHESIS_SIZE*sizeof(fftType_t));
+    m_outputBuffer = (fftType_t *)malloc(ANALYSIS_SIZE*sizeof(fftType_t));
+    m_synthesisBuffer = (fftType_t *)malloc(SYNTHESIS_SIZE*sizeof(fftType_t));
+
+    m_analysisFreqBuffer  = (fftType_t *)malloc(2*SYNTHESIS_SIZE*sizeof(fftType_t));
+    m_synthesisFreqBuffer = (fftType_t *)malloc(2*SYNTHESIS_SIZE*sizeof(fftType_t));
+
+    // clear the audio buffer to avoid pops and configure the Hann window
     for (unsigned i=0; i<AudioEffectPitchShift::ANALYSIS_SIZE; i++) {
         m_analysisBuffer[i] = 0.0f;
+        m_windowFunction[i] = 0.5f * (1.0f - cos(2.0f * M_PI *  (float)i / ANALYSIS_SIZE_F)) * WINDOW_GAIN;
+    }
+
+    for (unsigned i=0; i<SYNTHESIS_SIZE; i++) {
+        m_windowBuffer[i] = 0.0f;
     }
 
     // Configure the FFT
-//    arm_rfft_init_f32(&rfftForwardInst, &cfftForwardInst, AudioEffectPitchShift::ANALYSIS_SIZE,
-//                      FFT_FORWARD, FFT_DO_BIT_REVERSE);
-//    arm_rfft_init_f32(&rfftInverseInst, &cfftInverseInst, AudioEffectPitchShift::SYNTHESIS_SIZE,
-//                          FFT_INVERSE, FFT_DO_BIT_REVERSE);
     unsigned ret;
-    ret = arm_cfft_radix4_init_f32(&cfftForwardInst, ANALYSIS_SIZE, FFT_FORWARD, FFT_DO_BIT_REVERSE); //init FFT
-    if (!ret) { m_initFailed = true; };
-    ret = arm_cfft_radix4_init_f32(&cfftInverseInst, SYNTHESIS_SIZE, FFT_INVERSE, FFT_DO_BIT_REVERSE); //init FFT
-    if (!ret) { m_initFailed = true; };
+    ret = arm_rfft_init_f32(&fftFwdReal, &fftFwdComplex, SYNTHESIS_SIZE, FFT_FORWARD, FFT_DO_BIT_REVERSE); //init FFT
+    if (ret != ARM_MATH_SUCCESS) { m_initFailed = true; };
+    ret = arm_rfft_init_f32(&fftInvReal, &fftInvComplex, SYNTHESIS_SIZE, FFT_INVERSE, FFT_DO_BIT_REVERSE); //init FFT
+    if (ret != ARM_MATH_SUCCESS) { m_initFailed = true; };
 }
 
 AudioEffectPitchShift::~AudioEffectPitchShift()
 {
+    free(m_analysisBuffer);
+    m_analysisBuffer = nullptr;
+    free(m_windowFunction);
+    m_windowFunction = nullptr;
+    free(m_windowBuffer);
+    m_windowBuffer = nullptr;
+    free(m_outputBuffer);
+    m_outputBuffer = nullptr;
+    free(m_synthesisBuffer);
+    m_synthesisBuffer = nullptr;
+    free(m_analysisFreqBuffer);
+    m_analysisFreqBuffer = nullptr;
+    free(m_synthesisFreqBuffer);
+    m_synthesisFreqBuffer = nullptr;
 }
 
 void AudioEffectPitchShift::update(void)
@@ -71,59 +96,57 @@ void AudioEffectPitchShift::update(void)
 	}
 
 	// DO PROCESSING HERE
-	// Update the fifo
-//	m_inputFifo.push_back(inputAudioBlock); // insert the new block
-//	release(m_inputFifo.front()); //
-//	m_inputFifo.pop_front();
 
 	// Convert the contents of the audio blocks to the contiguous buffer
 	// 1) Be aware the audio library stores audio samples in reverse temporal order.
 	// This means the first sample (in time) is in the last location of the buffer.
-	// 2) the oldest audio is at the front of the queue, the latest at the back
-	float *analysisPtr      = &m_analysisBuffer[0];
-	float *analysisFreqPtr  = &m_analysisFreqBuffer[0];
-	float *synthesisFreqPtr = &m_synthesisFreqBuffer[0];
-	float *synthesisPtr     = &m_synthesisBuffer[0];
-
-	// first shift the contents of the float buffer up by AUDIO_BLOCK SAMPLES
-	for (unsigned i=0; i<NUM_AUDIO_BLOCKS-1; i++) {
-	    memcpy(&analysisPtr[i*AUDIO_BLOCK_SAMPLES], &analysisPtr[(i+1)*AUDIO_BLOCK_SAMPLES], AUDIO_BLOCK_SAMPLES*sizeof(float));
+	// 2) the oldest audio is at the front of the queue, the most recent at the back
+	fftType_t *analysisPtr      = &m_analysisBuffer[0];
+	fftType_t *analysisFreqPtr  = &m_analysisFreqBuffer[0];
+	fftType_t *synthesisFreqPtr = &m_synthesisFreqBuffer[0];
+	fftType_t *synthesisPtr     = &m_synthesisBuffer[0];
+
+	// first shift the contents of the fftType_t buffer up by AUDIO_BLOCK SAMPLES
+	for (unsigned i=0; i<(NUM_AUDIO_BLOCKS-1); i++) {
+	    memcpy(&analysisPtr[i*AUDIO_BLOCK_SAMPLES],    &analysisPtr[(i+1)*AUDIO_BLOCK_SAMPLES],    AUDIO_BLOCK_SAMPLES*sizeof(fftType_t));
+	    memcpy(&m_outputBuffer[i*AUDIO_BLOCK_SAMPLES], &m_outputBuffer[(i+1)*AUDIO_BLOCK_SAMPLES], AUDIO_BLOCK_SAMPLES*sizeof(fftType_t));
 	}
-	// Convert the newest incoming audio block to float
-	arm_q15_to_float(inputAudioBlock->data, &analysisPtr[(NUM_AUDIO_BLOCKS-1)*AUDIO_BLOCK_SAMPLES], AUDIO_BLOCK_SAMPLES);
+	// Convert the newest incoming audio block to fftType_t
+	int16ToFft(inputAudioBlock->data, &analysisPtr[(NUM_AUDIO_BLOCKS-1)*AUDIO_BLOCK_SAMPLES], AUDIO_BLOCK_SAMPLES);
+	memset(&m_outputBuffer[(NUM_AUDIO_BLOCKS-1)*AUDIO_BLOCK_SAMPLES], 0, AUDIO_BLOCK_SAMPLES * sizeof(fftType_t));
 	release(inputAudioBlock); // were done with it now
 
-	//if (m_initFailed) { Serial.println("FFT INIT FAILED"); }
+	if (m_initFailed) { Serial.println("FFT INIT FAILED"); }
 
-	// Construct the interleaved FFT buffer
-	unsigned idx = 0;
-	for (unsigned i=0; i<ANALYSIS_SIZE; i++) {
-	    m_analysisFreqBuffer[idx] = analysisPtr[i];
-	    m_analysisFreqBuffer[idx+1] = 0;
-	    idx += 2;
-	}
+	// Window the contents of the analysis buffer to a temp buffer
+	memset(m_windowBuffer, 0, sizeof(float) * SYNTHESIS_SIZE);
+	arm_mult_f32(analysisPtr, &m_windowFunction[0], m_windowBuffer, ANALYSIS_SIZE);
 
 	// Perform the FFT
-	arm_cfft_radix4_f32(&cfftForwardInst, analysisFreqPtr);
+	arm_rfft_f32(&fftFwdReal, m_windowBuffer, analysisFreqPtr);
+
 
+	//memset(analysisPtr, 0, sizeof(float)*ANALYSIS_SIZE);
 
 	// perform the ocean pitch shift
 	m_ocean(analysisFreqPtr, synthesisFreqPtr, (float)(m_frameIndex), m_pitchScale);
-	//memcpy(synthesisFreqPtr, analysisFreqPtr, 2*ANALYSIS_SIZE*sizeof(float));
+	//memcpy(synthesisFreqPtr, analysisFreqPtr, 2*ANALYSIS_SIZE*sizeof(fftType_t));
 
-	// Perform the inverse FFT
-	arm_cfft_radix4_f32(&cfftInverseInst, synthesisFreqPtr);
 
-	// Deinterleave the synthesis buffer
-	idx = 0;
-    for (unsigned i=0; i<(2*SYNTHESIS_SIZE); i=i+2) {
-        m_synthesisBuffer[idx] = synthesisFreqPtr[i];
-        idx++;
-    }
+	arm_rfft_f32(&fftInvReal, synthesisFreqPtr, synthesisPtr);
+
+	// Window the output before adding, only use first part of the synthesized waveform
+	arm_mult_f32(synthesisPtr, &m_windowFunction[0], synthesisPtr, ANALYSIS_SIZE);
+
+	// Add the synthesis to the output buffer
+    arm_add_f32(m_outputBuffer, synthesisPtr, m_outputBuffer, ANALYSIS_SIZE);
+	//memcpy(m_outputBuffer, synthesisPtr, sizeof(fftType_t)*AUDIO_BLOCK_SAMPLES);
 
-    // Convert the float buffer back to integer
+
+    // Convert the fftType_t buffer back to integer
     audio_block_t *outputBlock = allocate();
-    arm_float_to_q15 (synthesisPtr, outputBlock->data, AUDIO_BLOCK_SAMPLES);
+    //fftToInt16 (analysisPtr, outputBlock->data, AUDIO_BLOCK_SAMPLES);
+    fftToInt16 (m_outputBuffer, outputBlock->data, AUDIO_BLOCK_SAMPLES);
 
 	transmit(outputBlock);
 	release(outputBlock);
@@ -151,6 +174,15 @@ void AudioEffectPitchShift::processMidi(int channel, int control, int value)
 		return;
 	}
 
+	if ((m_midiConfig[PITCH][MIDI_CHANNEL] == channel) &&
+        (m_midiConfig[PITCH][MIDI_CONTROL] == control)) {
+        // Volume
+	    int pitchCents = roundf((val - 1.0f) * 1200.0f);
+        Serial.println(String("AudioEffectPitchShift::pitch: ") + pitchCents + String(" cents"));
+        setPitchShiftCents(pitchCents);
+        return;
+    }
+
 }
 
 void AudioEffectPitchShift::mapMidiControl(int parameter, int midiCC, int midiChannel)
@@ -162,16 +194,38 @@ void AudioEffectPitchShift::mapMidiControl(int parameter, int midiCC, int midiCh
 	m_midiConfig[parameter][MIDI_CONTROL] = midiCC;
 }
 
-void AudioEffectPitchShift::m_ocean(float *inputFreq, float *outputFreq, float frameIndex, float pitchScale)
+
+float AudioEffectPitchShift::setPitchKnob(float val)
+{
+    int pitchCents = roundf((val - 0.5f)*2.0f * 1200.0f);
+    float pitchScale = setPitchShiftCents(pitchCents);
+    return pitchScale;
+}
+
+float AudioEffectPitchShift::setPitchShiftCents(int shiftCents)
+{
+    constexpr float ROOT_12TH_OF_2 = 1.0594630944;
+    // alpha = nthroot(2,12)^(pitchShiftCents/100);
+    m_pitchScale = powf(ROOT_12TH_OF_2,((float)(shiftCents) / 100.0f));
+    return m_pitchScale;
+}
+
+void AudioEffectPitchShift::m_ocean(fftType_t *inputFreq, fftType_t *outputFreq, float frameIndex, float pitchScale)
 {
     // zero the output buffer
     for (unsigned i=0; i<(2*SYNTHESIS_SIZE); i++) {
         outputFreq[i] = 0.0f;
     }
 
+    //pitchScale = 2.0f;
+
+//    float phaseAdjustFactor = -((2.0f*((float)(M_PI))*frameIndex)
+//            / (OVERLAP_FACTOR_F * FFT_OVERSAMPLE_FACTOR_F * SYNTHESIS_SIZE_F));
     float phaseAdjustFactor = -((2.0f*((float)(M_PI))*frameIndex)
-            / (OVERLAP_FACTOR_F * FFT_OVERSAMPLE_FACTOR_F * SYNTHESIS_SIZE_F));
+                / (OVERLAP_FACTOR_F * FFT_OVERSAMPLE_FACTOR_F));
 
+    //outputFreq[0] = inputFreq[0];
+    //outputFreq[1] = inputFreq[1];
     for (unsigned k=1; k < SYNTHESIS_SIZE/2; k++) {
 
         float a = (float)k;
@@ -181,18 +235,29 @@ void AudioEffectPitchShift::m_ocean(float *inputFreq, float *outputFreq, float f
         float b = std::roundf( (FFT_OVERSAMPLE_FACTOR_F * pitchScale * a));
         unsigned b_int = (unsigned)(b);
 
-        if (b_int < SYNTHESIS_SIZE/2) {
+        //if (b_int <256) {
+        if ((b_int < (SYNTHESIS_SIZE/2/2))) {
 
             // phaseAdjust = (b-ma) * phaseAdjustFactor
             float phaseAdjust = (b - (FFT_OVERSAMPLE_FACTOR_F * a)) * phaseAdjustFactor;
 
+
             float a_real = inputFreq[2*k];
             float a_imag = inputFreq[2*k+1];
 
-            outputFreq[2*b_int]   = (a_real * arm_cos_f32(phaseAdjust)) - (a_imag * arm_sin_f32(phaseAdjust));
-            outputFreq[2*b_int+1] = (a_real * arm_sin_f32(phaseAdjust)) + (a_imag * arm_cos_f32(phaseAdjust));
-        }
+            // Note the real and imag are interleaved
+            unsigned idx = 2*b_int;
+            outputFreq[idx]      = (a_real * fastCos(phaseAdjust)) - (a_imag * fastSin(phaseAdjust));
+            outputFreq[idx+1]  = (a_real * fastSin(phaseAdjust)) + (a_imag * fastCos(phaseAdjust));
+
+            //if ((int)frameIndex % 512 == 0) {
+            //Serial.println(String("b:") + b_int + String(" idx:") + idx + String(" coeff:") + outputFreq[idx] + String(":") + outputFreq[idx+1]); }
 
+            // Negative Frequencies
+            //unsigned negB = SYNTHESIS_SIZE-b_int;
+            //outputFreq[2*negB]    = outputFreq[idx];
+            //outputFreq[2*negB+1] = -outputFreq[idx+1];
+        }
         // update the imag components
     }
 }