/*-----------------------------------------------------------------------------

   File:        Modules.cpp

   Description: Implementation of useful C++ modules such as delay lines,
                filters, oscillators, etc.

   Author:      Jasmin Frenette
   Date:        11/27/2000

-----------------------------------------------------------------------------*/

#include <windows.h>
#include <math.h>
#include <assert.h>
#include <crtdbg.h>
#include <memory.h> 

#include "PlgMem.hpp"
#include "Modules.hpp"



//-----------------------------------------------------------------------------
// Memory Management Procedures
//-----------------------------------------------------------------------------

void Module::UpdateMemory(long* &pMem, long newSize)
{
   ChangeMemory((void*&)pMem, newSize*sizeof(long));
}

void Module::UpdateMemory(float* &pMem, long newSize)
{
   ChangeMemory((void*&)pMem, newSize*sizeof(float));
}

void Module::UpdateMemory(float** &pMem, long newSize)
{
   ChangeMemory((void*&)pMem, newSize*sizeof(float*));
}




//-----------------------------------------------------------------------------
// 1st order FIR Low Pass Filter Module (version a)
//-----------------------------------------------------------------------------

MLPF1a::MLPF1a() : Module()
{
   b0 = a1 = 0;

   Suspend();      // flush buffer
}

void MLPF1a::Suspend(void)
{
   buffer = 0;
}

void MLPF1a::SetCoeffs(float in_b0, float in_a1)
{
   b0 = in_b0;
   a1 = in_a1;
}

//-----------------------------------------------------------------------------
//   Get Functions

void MLPF1a::GetCoeffs(float* out_b0, float* out_a1)
{
   *out_b0 = b0;
   *out_a1 = a1;
}



//-----------------------------------------------------------------------------
// 1st order FIR Low Pass Filter Module (version b)
//-----------------------------------------------------------------------------

MLPF1b::MLPF1b() : Module()
{
   k = a1 = 0;

   Suspend();      // flush buffer
}

void MLPF1b::Suspend(void)
{
   bufferX = bufferY = 0;
}

void MLPF1b::SetCoeffs(float in_k, float in_a1)
{
   k = in_k;
   a1 = in_a1;
}

//-----------------------------------------------------------------------------
//   Get Functions

void MLPF1b::GetCoeffs(float* out_k, float* out_a1)
{
   *out_k = k;
   *out_a1 = a1;
}



//-----------------------------------------------------------------------------
// 2nd order FIR Low Pass Filter Module (version b)
//-----------------------------------------------------------------------------
MLPF2b::MLPF2b() : Module()
{
   k = b1 = a1 = a2 = 0;

   Suspend();      // flush buffer
}

void MLPF2b::Suspend(void)
{
   bufferX1 = bufferX2 = bufferY1 = bufferY2 = 0;
}

void MLPF2b::SetCoeffs(double in_k, double in_b1, double in_a1, double in_a2)
{
   k = in_k;
   b1 = in_b1;
   a1 = in_a1;
   a2 = in_a2;
}

//-----------------------------------------------------------------------------
//   Get Functions

void MLPF2b::GetCoeffs(double* out_k,double* out_b1,double* out_a1,double* out_a2)
{
   *out_k = k;
   *out_b1 = b1;
   *out_a1 = a1;
   *out_a2 = a2;
}




//-----------------------------------------------------------------------------
// Sinusoidal Oscillator Module
//-----------------------------------------------------------------------------

MSinOsc::MSinOsc() : MModulator()
{
   fFreq = 0;
   a = 0;

   Suspend();      // flush buffer
}

MSinOsc::~MSinOsc()
{
}

void MSinOsc::Suspend(void)
{
   buffer1 = buffer2 = resetBuffer2 = 0;
}

void MSinOsc::SetFreq(float freq, float sampleRate, float phase)
{
   fFreq = freq;
   double w = TWOPI*fFreq/sampleRate;
   a = 2*cos(w);
   buffer2 = sin(TWOPI*phase/360 - w);
   buffer1 = sin(TWOPI*phase/360);
   resetBuffer2 = sin(PI_DIV_BY_2 - w);
}

float MSinOsc::GetFreq()
{
   return fFreq;
}




//-----------------------------------------------------------------------------
//  Random Number Generator Module
//-----------------------------------------------------------------------------

MRNG::MRNG() : MModulator()
{
   a = c = seed = 0;

   Suspend();
}

MRNG::~MRNG()
{
}

void MRNG::Suspend(void)
{
   buffer = seed;
}

void MRNG::SetParams(unsigned int in_a, unsigned int in_c,
                     unsigned int in_seed)
{
   a = in_a;
   c = in_c;
   seed = in_seed;

   Suspend();
}

void MRNG::GetParams(unsigned int* out_a, unsigned int* out_c,
                     unsigned int* out_seed)
{
   *out_a = a;
   *out_c = c;
   *out_seed = seed;
}




//-----------------------------------------------------------------------------
//  Low-Pass Filtered (4rth order) Module
//-----------------------------------------------------------------------------

MLPFMod::MLPFMod() : MModulator()
{
   lRate = 0;
   pMod = NULL;
   pLPF1 = NULL;
   pLPF2 = NULL;

   index = 0;
}

MLPFMod::~MLPFMod()
{
}

void MLPFMod::SetRefreshRate(long rate)
{
   lRate = rate;
}

void MLPFMod::SetModulator(MModulator* mod)
{
   assert(mod!=NULL);
   pMod = mod;
}

void MLPFMod::SetLPF1(MLPF2b* lpf)
{
   assert(lpf!=NULL);
   pLPF1 = lpf;
}

void MLPFMod::SetLPF2(MLPF2b* lpf)
{
   assert(lpf!=NULL);
   pLPF2 = lpf;
}

long MLPFMod::GetRefreshRate(void)
{
   return lRate;
}

MModulator* MLPFMod::GetModulator(void)
{
   return pMod;
}

MLPF2b* MLPFMod::GetLPF1(void)
{
   return pLPF1;
}

MLPF2b* MLPFMod::GetLPF2(void)
{
   return pLPF2;
}

float MLPFMod::GetCurValue(void){
   double out;

   if(index == 0)
      out = pMod->GetCurValue();
   else
      out = 0;

   if(++index == lRate)
      index = 0;

   pLPF1->Process(out, out);
   pLPF2->Process(out, out);

   return (float) out;
}




//-----------------------------------------------------------------------------
// Delay Line Module
//-----------------------------------------------------------------------------

MDelayLine::MDelayLine() : Module()
{
   // Buffer parameters
   pfBuffer = pfBufferEnd = NULL;
   lBufferSize = 0;
   pfInPos = pfOutPos = NULL; 

   lMaxDelay = 0;

   Suspend();      // flush buffer
}

MDelayLine::~MDelayLine()
{
   if(pfBuffer)
      UpdateMemory(pfBuffer,0);
}

void MDelayLine::Suspend(void)
{
   if(pfBuffer)
      memset(pfBuffer, 0, lBufferSize * sizeof(float));
}

long MDelayLine::GetMem(void)
{
   return (long) (lBufferSize * sizeof(float));
}

void MDelayLine::SetMaxDelay(long delay)
{
   lMaxDelay = delay;

   lBufferSize = lMaxDelay + 10 + 1;   // keeps 10 extra samples for interp.

   UpdateMemory(pfBuffer,lBufferSize); 
   pfBufferEnd = pfBuffer + lBufferSize;

   pfInPos = pfBuffer;

   pfOutPos = pfInPos - lMaxDelay;
   if(pfOutPos < pfBuffer)
      pfOutPos += lBufferSize;
}

//-----------------------------------------------------------------------------
//   Get Functions

long MDelayLine::GetMaxDelay(void)
{
   return lMaxDelay;
}




//-----------------------------------------------------------------------------
// Modulated Delay Line Module
//-----------------------------------------------------------------------------

MModDelayLine::MModDelayLine() : MDelayLine()
{
   fModOutPos = 0;
   lNomDelay = 0;
   lModDepth = 0;
   pMod = NULL;
}

void MModDelayLine::SetNomDelay(long delay)
{
   lNomDelay = delay;

   SetMaxDelay(lNomDelay+lModDepth); 

   // update output position (buffer size has changed)
   fModOutPos = (float)(pfInPos - pfBuffer - lNomDelay);
   if(fModOutPos < 0)
      fModOutPos += lBufferSize;
}

void MModDelayLine::SetModDepth(long depth)
{
   if(depth>=lNomDelay){
      DebugBreak();
      depth = lNomDelay-1;
      return;
   }

   lModDepth = depth;

   SetMaxDelay(lNomDelay+lModDepth);

   // update output position (buffer size has changed)
   fModOutPos = (float)(pfInPos - pfBuffer - lNomDelay);
   if(fModOutPos < 0)
      fModOutPos += lBufferSize;
}

void MModDelayLine::SetModulator(MModulator* mod)
{
   assert(mod!=NULL);
   pMod = mod; 
}

//-----------------------------------------------------------------------------
//   Get Functions

long MModDelayLine::GetNomDelay(void)
{
   return lNomDelay;
}

long MModDelayLine::GetModDepth(void)
{
   return lModDepth;
}

MModulator* MModDelayLine::GetModulator(void)
{
   return pMod;
}




//-----------------------------------------------------------------------------
// Simplified 1st order All-Pass Modulated Delay Line Module
//-----------------------------------------------------------------------------

float MSAP1ModDelayLine::GetCurValue(void)
{
   float fOne_m_D, fOutIndex;
   long  lIndex, lIndex_p_1;
   float out;
   
   fOutIndex = (float)(fModOutPos + pMod->GetCurValue()*lModDepth);

   if(fOutIndex>=0)
      lIndex = (long)(fOutIndex);
   else
      lIndex = (long)(fOutIndex-1);

   lIndex_p_1 = lIndex + 1;
   fOne_m_D = fOutIndex - lIndex;

   // Boundary check
   if(lIndex_p_1 >= lBufferSize){
      lIndex_p_1 -= lBufferSize;
      if(lIndex >= lBufferSize)
         lIndex -= lBufferSize;
   }
   else if(lIndex < 0){
      lIndex += lBufferSize;
      if(lIndex_p_1 < 0){
         lIndex_p_1 += lBufferSize;
      }
   }

   // Interpolation
   out =  *(pfBuffer + lIndex);
   out += fOne_m_D * (*(pfBuffer + lIndex_p_1) - buffer);
   buffer = out;

   if(++fModOutPos >= lBufferSize)
      fModOutPos -= lBufferSize;

   return out;
}




//-----------------------------------------------------------------------------
// 1st order All-Pass Modulated Delay Line Module
//-----------------------------------------------------------------------------

float MAP1ModDelayLine::GetCurValue(void)
{
   float fOne_m_D, fOne_p_D, fOutIndex;
   long  lIndex, lIndex_p_1;
   float out;
   
   fOutIndex = (float)(fModOutPos + pMod->GetCurValue()*lModDepth);

   if(fOutIndex>=0)
      lIndex = (long)(fOutIndex);
   else
      lIndex = (long)(fOutIndex-1);

   lIndex_p_1 = lIndex + 1;
   fOne_m_D = fOutIndex - lIndex;
   fOne_p_D = 1 + lIndex_p_1 - fOutIndex;
   
   // Boundary check
   if(lIndex_p_1 >= lBufferSize){
      lIndex_p_1 -= lBufferSize;
      if(lIndex >= lBufferSize)
         lIndex -= lBufferSize;
   }
   else if(lIndex < 0){
      lIndex += lBufferSize;
      if(lIndex_p_1 < 0){
         lIndex_p_1 += lBufferSize;
      }
   }

   // Interpolation
   out =  *(pfBuffer + lIndex);
   out += fOne_m_D / fOne_p_D * (*(pfBuffer + lIndex_p_1) - buffer);
   buffer = out;

   if(++fModOutPos >= lBufferSize)
      fModOutPos -= lBufferSize;

   return out;
}





//-----------------------------------------------------------------------------
// 1st order Low-Pass Modulated Delay Line Module
//-----------------------------------------------------------------------------

float MLP1ModDelayLine::GetCurValue(void)
{
   float fD, fOutIndex;
   long  lIndex, lIndex_p_1;
   float out;
   
   fOutIndex = (float)(fModOutPos + pMod->GetCurValue()*lModDepth);

   if(fOutIndex>=0)
      lIndex = (long)(fOutIndex);
   else
      lIndex = (long)(fOutIndex-1);

   lIndex_p_1 = lIndex + 1;
   fD = fOutIndex - lIndex;

   // Boundary check
   if(lIndex_p_1 >= lBufferSize){
      lIndex_p_1 -= lBufferSize;
      if(lIndex >= lBufferSize)
         lIndex -= lBufferSize;
   }
   else if(lIndex < 0){
      lIndex += lBufferSize;
      if(lIndex_p_1 < 0){
         lIndex_p_1 += lBufferSize;
      }
   }

/*   if ((lIndex<0) || (lIndex>=lBufferSize)
      || (lIndex_p_1<0) || (lIndex_p_1>=lBufferSize))
      DebugBreak();*/

   // Interpolation
   out =  fD * (*(pfBuffer + lIndex_p_1));
   out += (1-fD) * (*(pfBuffer + lIndex));

   if(++fModOutPos >= lBufferSize)
      fModOutPos -= lBufferSize;

   return out;
}




//-----------------------------------------------------------------------------
// 2nd order Low-Pass Modulated Delay Line Module
//-----------------------------------------------------------------------------

float MLP2ModDelayLine::GetCurValue(void)
{
   float fD, fOutIndex;
   long  lIndex, lIndex_p_1, lIndex_p_2;
   float out;
   
   fOutIndex = (float)(fModOutPos + pMod->GetCurValue()*lModDepth);

   if(fOutIndex>=0)
      lIndex = (long)(fOutIndex);
   else
      lIndex = (long)(fOutIndex-1);

   lIndex_p_1 = lIndex + 1;
   lIndex_p_2 = lIndex_p_1 + 1;
   fD = fOutIndex - lIndex;

   // Boundary check
   if(lIndex_p_2 >= lBufferSize){
      lIndex_p_2 -= lBufferSize;
      if(lIndex_p_1 >= lBufferSize){
         lIndex_p_1 -= lBufferSize;
         if(lIndex >= lBufferSize)
            lIndex -= lBufferSize;
      }
   }
   else if(lIndex < 0){
      lIndex += lBufferSize;
      if(lIndex_p_1 < 0){
         lIndex_p_1 += lBufferSize;
         if(lIndex_p_2 < 0){
            lIndex_p_2 += lBufferSize;
         }
      }
   }

   float ppf0 = fD;
   float ppf1 = ppf0*(fD-1);

   float ppr0 = (fD-2);
   float ppr1 = ppr0*(fD-1);

   // Interpolation
   out = ppr1*(float)0.5 * (*(pfBuffer + lIndex));
   out -= (ppf0*ppr0) * (*(pfBuffer + lIndex_p_1));
   out += ppf1*(float)0.5 * (*(pfBuffer + lIndex_p_2));

   if(++fModOutPos >= lBufferSize)
      fModOutPos -= lBufferSize;

   return out;
}




//-----------------------------------------------------------------------------
// 3rd order Low-Pass Modulated Delay Line Module
//-----------------------------------------------------------------------------

float MLP3ModDelayLine::GetCurValue(void)
{
   float fD, fOutIndex;
   long  lIndex, lIndex_p_1, lIndex_p_2, lIndex_p_3;
   float out;
   
   fOutIndex = (float)(fModOutPos + pMod->GetCurValue()*lModDepth);

   if(fOutIndex>=0)
      lIndex_p_1 = (long)(fOutIndex);
   else
      lIndex_p_1 = (long)(fOutIndex-1);

   lIndex = lIndex_p_1 - 1;
   lIndex_p_2 = lIndex_p_1 + 1;
   lIndex_p_3 = lIndex_p_1 + 2;
   fD = fOutIndex - lIndex;

   // Boundary check
   if(lIndex_p_3 >= lBufferSize){
      lIndex_p_3 -= lBufferSize;
      if(lIndex_p_2 >= lBufferSize){
         lIndex_p_2 -= lBufferSize;
         if(lIndex_p_1 >= lBufferSize){
            lIndex_p_1 -= lBufferSize;
            if(lIndex >= lBufferSize)
               lIndex -= lBufferSize;
         }
      }
   }
   else if(lIndex < 0){
      lIndex += lBufferSize;
      if(lIndex_p_1 < 0){
         lIndex_p_1 += lBufferSize;
         if(lIndex_p_2 < 0){
            lIndex_p_2 += lBufferSize;
            if(lIndex_p_3 < 0)
               lIndex_p_3 += lBufferSize;
         }
      }
   }

   float ppf0 = fD;
   float ppf1 = ppf0*(fD-1);
   float ppf2 = ppf1*(fD-2);

   float ppr0 = (fD-3);
   float ppr1 = ppr0*(fD-2);
   float ppr2 = ppr1*(fD-1);

   out = ppr2*(float)(-0.166666666666666667) * (*(pfBuffer + lIndex));
   out += (ppr2+ppr1)*(float)0.5 * (*(pfBuffer + lIndex_p_1));
   out -= (ppf2-ppf1)*(float)0.5 * (*(pfBuffer + lIndex_p_2));
   out += ppf2*(float)0.166666666666666667 * (*(pfBuffer + lIndex_p_3));

   // 6 2 2 6

   if(++fModOutPos >= lBufferSize)
      fModOutPos -= lBufferSize;

   return out;
}





//-----------------------------------------------------------------------------
// 4th order Low-Pass Modulated Delay Line Module
//-----------------------------------------------------------------------------

float MLP4ModDelayLine::GetCurValue(void)
{
   float fD, fOutIndex;
   long  lIndex, lIndex_p_1, lIndex_p_2, lIndex_p_3, lIndex_p_4;
   float out;
   
   fOutIndex = (float)(fModOutPos + pMod->GetCurValue()*lModDepth);

   if(fOutIndex>=0)
      lIndex_p_1 = (long)(fOutIndex);
   else
      lIndex_p_1 = (long)(fOutIndex-1);

   lIndex = lIndex_p_1 - 1;
   lIndex_p_2 = lIndex_p_1 + 1;
   lIndex_p_3 = lIndex_p_1 + 2;
   lIndex_p_4 = lIndex_p_1 + 3;
   fD = fOutIndex - lIndex;

   // Boundary check
   if(lIndex_p_4 >= lBufferSize){
      lIndex_p_4 -= lBufferSize;
      if(lIndex_p_3 >= lBufferSize){
         lIndex_p_3 -= lBufferSize;
         if(lIndex_p_2 >= lBufferSize){
            lIndex_p_2 -= lBufferSize;
            if(lIndex_p_1 >= lBufferSize){
               lIndex_p_1 -= lBufferSize;
               if(lIndex >= lBufferSize)
                  lIndex -= lBufferSize;
            }
         }
      }
   }
   else if(lIndex < 0){
      lIndex += lBufferSize;
      if(lIndex_p_1 < 0){
         lIndex_p_1 += lBufferSize;
         if(lIndex_p_2 < 0){
            lIndex_p_2 += lBufferSize;
            if(lIndex_p_3 < 0){
               lIndex_p_3 += lBufferSize;
               if(lIndex_p_4 < 0)
                  lIndex_p_4 += lBufferSize;
            }
         }
      }
   }

   float ppf0 = fD;
   float ppf1 = ppf0*(fD-1);
   float ppf2 = ppf1*(fD-2);
   float ppf3 = ppf2*(fD-3);

   float ppr0 = (fD-4);
   float ppr1 = ppr0*(fD-3);
   float ppr2 = ppr1*(fD-2);
   float ppr3 = ppr2*(fD-1);

   // Interpolation
   out  = ppr3*(float)0.04166666666666666667 * (*(pfBuffer + lIndex));
   out -= (ppr3+ppr2)*(float)0.166666666666666667 * (*(pfBuffer + lIndex_p_1));
   out += (ppf1*ppr1)*(float)0.25 * (*(pfBuffer + lIndex_p_2));
   out -= (ppf3-ppf2)*(float)0.166666666666666667 * (*(pfBuffer + lIndex_p_3));
   out += ppf3*(float)0.04166666666666666667 * (*(pfBuffer + lIndex_p_4));

   // 24 6 4 6 24

   if(++fModOutPos >= lBufferSize)
      fModOutPos -= lBufferSize;

   return out;
}




//-----------------------------------------------------------------------------
// Simplified Variable Rate 1st order All-Pass Modulated Delay Line Module
//-----------------------------------------------------------------------------

void MSVRAP1ModDelayLine::SetModRate(long rate)
{
   assert(rate<=lBufferSize);
   lModRate = rate;
}

long MSVRAP1ModDelayLine::GetModRate(void)
{
   return lModRate;
}

float MSVRAP1ModDelayLine::GetCurValue(void)
{
   float fOutIndex, out;
   long lIndex;
   
   if(((index++)%lModRate) == 0){
      fOutIndex = (float)(fModOutPos + pMod->GetCurValue()*lModDepth);

      if(fOutIndex >= 0.0f)
         lIndex = (long)(fOutIndex);
      else
         lIndex = (long)(fOutIndex-1);

      pfOutPos   = pfBuffer + lIndex;

      // Boundary check
      if(pfOutPos < pfBuffer)
         pfOutPos += lBufferSize;
      if(pfOutPos >= pfBufferEnd)
         pfOutPos -= lBufferSize;

      fOne_m_D = fOutIndex - lIndex;

      fModOutPos += lModRate;
      if(fModOutPos >= lBufferSize)
         fModOutPos -= lBufferSize;
   }

   // Interpolation
   out = *(pfOutPos++);

   // Boundary check
   if(pfOutPos >= pfBufferEnd)
      pfOutPos -= lBufferSize;

   // Interpolation
   out += fOne_m_D * (*(pfOutPos) - buffer);
   buffer = out;

   return out;
}