/*
  ZynAddSubFX - a software synthesizer

  AnalogFilter.C - Several analog filters (lowpass, highpass...)
  Copyright (C) 2002-2005 Nasca Octavian Paul
  Author: Nasca Octavian Paul

  Modified for rakarrack by Ryan Billing and Josep Andreu

  This program is free software; you can redistribute it and/or modify
  it under the terms of version 2 of the GNU General Public License
  as published by the Free Software Foundation.

  This program is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License (version 2) for more details.

  You should have received a copy of the GNU General Public License (version 2)
  along with this program; if not, write to the Free Software Foundation,
  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA

*/

#include "AnalogFilter.h"

#ifdef SHR3D_SFX_CORE_RAKARRACK

AnalogFilter::AnalogFilter(u8 Ftype, f32 Ffreq, f32 Fq, u8 Fstages)
  : stages(Fstages)
  , type(Ftype)
  , freq(Ffreq)
  , q(Fq)
{
  iSAMPLE_RATE = SAMPLE_RATE;
  ifSAMPLE_RATE = fSAMPLE_RATE;

  if (stages >= MAX_FILTER_STAGES)
    stages = MAX_FILTER_STAGES;
  cleanup();
  firsttime = 0;
  abovenq = 0;
  oldabovenq = 0;
  setfreq_and_q(Ffreq, Fq);
  firsttime = 1;
  d[0] = 0;			//this is not used
  outgain = 1.0f;
}

void AnalogFilter::cleanup()
{
  for (i32 i = 0; i < MAX_FILTER_STAGES + 1; i++)
  {
    x[i].c1 = 0.0f;
    x[i].c2 = 0.0f;
    y[i].c1 = 0.0f;
    y[i].c2 = 0.0f;
    oldx[i] = x[i];
    oldy[i] = y[i];
  }
  needsinterpolation = 0;
}

void AnalogFilter::computefiltercoefs()
{
  i32 zerocoefs = 0;		//this is used if the freq is too high
  f32 tmp;
  f32 omega, sn, cs, alpha, beta;

  //do not allow frequencies bigger than samplerate/2
  f32 freq = this->freq;
  if (freq > (iSAMPLE_RATE / 2 - 500.0))
  {
    freq = ifSAMPLE_RATE * .5f - 500.0f;
    zerocoefs = 1;
  };
  if (freq < 0.1)
    freq = 0.1f;
  //do not allow bogus Q
  if (q < 0.0)
    q = 0.0f;

  f32 tmpq;
  f32 tmpgain;
  if (stages == 0)
  {
    tmpq = q;
    tmpgain = gain;
  }
  else
  {
    tmpq = (q > 1.0 ? powf(q, 1.0f / (f32)(stages + 1)) : q);
    tmpgain = powf(gain, 1.0f / (f32)(stages + 1));
  }

  //most of theese are implementations of 
  //the "Cookbook formulae for audio EQ" by Robert Bristow-Johnson
  //The original location of the Cookbook is:
  //http://www.harmony-central.com/Computer/Programming/Audio-EQ-Cookbook.txt
  switch (type)
  {
  case 0:			//LPF 1 pole         
    if (zerocoefs == 0)
      tmp = expf(-D_PI * freq / ifSAMPLE_RATE);
    else
      tmp = 0.0f;
    c[0] = 1.0f - tmp;
    c[1] = 0.0;
    c[2] = 0.0;
    d[1] = tmp;
    d[2] = 0.0;
    order = 1;
    return;
  case 1:			//HPF 1 pole
    if (zerocoefs == 0)
      tmp = expf(-D_PI * freq / ifSAMPLE_RATE);
    else
      tmp = 0.0f;
    c[0] = (1.0f + tmp) * .5f;
    c[1] = -(1.0f + tmp) * .5f;
    c[2] = 0.0f;
    d[1] = tmp;
    d[2] = 0.0f;
    order = 1;
    return;
  case 2:			//LPF 2 poles 
    if (zerocoefs == 0)
    {
      omega = D_PI * freq / ifSAMPLE_RATE;
      sn = sinf(omega);
      cs = cosf(omega);
      alpha = sn / (2.0f * tmpq);
      tmp = 1 + alpha;
      c[0] = (1.0f - cs) * .5f / tmp;
      c[1] = (1.0f - cs) / tmp;
      c[2] = (1.0f - cs) * .5f / tmp;
      d[1] = -2.0f * cs / tmp * (-1.0f);
      d[2] = (1.0f - alpha) / tmp * (-1.0f);
    }
    else
    {
      c[0] = 1.0;
      c[1] = 0.0;
      c[2] = 0.0;
      d[1] = 0.0;
      d[2] = 0.0;
    }
    order = 2;
    return;
  case 3:			//HPF 2 poles 
    if (zerocoefs == 0)
    {
      omega = D_PI * freq / ifSAMPLE_RATE;
      sn = sinf(omega);
      cs = cosf(omega);
      alpha = sn / (2.0f * tmpq);
      tmp = 1.0f + alpha;
      c[0] = (1.0f + cs) / 2.0f / tmp;
      c[1] = -(1.0f + cs) / tmp;
      c[2] = (1.0f + cs) / 2.0f / tmp;
      d[1] = -2.0f * cs / tmp * (-1.0f);
      d[2] = (1.0f - alpha) / tmp * (-1.0f);
    }
    else
    {
      c[0] = 0.0;
      c[1] = 0.0;
      c[2] = 0.0;
      d[1] = 0.0;
      d[2] = 0.0;
    }
    order = 2;
    return;
  case 4:			//BPF 2 poles 
    if (zerocoefs == 0)
    {
      omega = D_PI * freq / ifSAMPLE_RATE;
      sn = sinf(omega);
      cs = cosf(omega);
      alpha = sn / (2.0f * tmpq);
      tmp = 1.0f + alpha;
      c[0] = alpha / tmp * sqrtf(tmpq + 1.0f);
      c[1] = 0.0f;
      c[2] = -alpha / tmp * sqrtf(tmpq + 1.0f);
      d[1] = -2.0f * cs / tmp * (-1.0f);
      d[2] = (1.0f - alpha) / tmp * (-1.0f);
    }
    else
    {
      c[0] = 0.0;
      c[1] = 0.0;
      c[2] = 0.0;
      d[1] = 0.0;
      d[2] = 0.0;
    }
    order = 2;
    return;
  case 5:			//NOTCH 2 poles 
    if (zerocoefs == 0)
    {
      omega = D_PI * freq / ifSAMPLE_RATE;
      sn = sinf(omega);
      cs = cosf(omega);
      alpha = sn / (2.0f * sqrtf(tmpq));
      tmp = 1.0f + alpha;
      c[0] = 1.0f / tmp;
      c[1] = -2.0f * cs / tmp;
      c[2] = 1.0f / tmp;
      d[1] = -2.0f * cs / tmp * (-1.0f);
      d[2] = (1.0f - alpha) / tmp * (-1.0f);
    }
    else
    {
      c[0] = 1.0;
      c[1] = 0.0;
      c[2] = 0.0;
      d[1] = 0.0;
      d[2] = 0.0;
    }
    order = 2;
    return;
  case 6:			//PEAK (2 poles)
    if (zerocoefs == 0)
    {
      omega = D_PI * freq / ifSAMPLE_RATE;
      sn = sinf(omega);
      cs = cosf(omega);
      tmpq *= 3.0f;
      alpha = sn / (2.0f * tmpq);
      tmp = 1.0f + alpha / tmpgain;
      c[0] = (1.0f + alpha * tmpgain) / tmp;
      c[1] = (-2.0f * cs) / tmp;
      c[2] = (1.0f - alpha * tmpgain) / tmp;
      d[1] = -2.0f * cs / tmp * (-1.0f);
      d[2] = (1.0f - alpha / tmpgain) / tmp * (-1.0f);
    }
    else
    {
      c[0] = 1.0;
      c[1] = 0.0;
      c[2] = 0.0;
      d[1] = 0.0;
      d[2] = 0.0;
    }
    order = 2;
    return;
  case 7:			//Low Shelf - 2 poles
    if (zerocoefs == 0)
    {
      omega = D_PI * freq / ifSAMPLE_RATE;
      sn = sinf(omega);
      cs = cosf(omega);
      tmpq = sqrtf(tmpq);
      alpha = sn / (2.0f * tmpq);
      beta = sqrtf(tmpgain) / tmpq;
      tmp = (tmpgain + 1.0f) + (tmpgain - 1.0f) * cs + beta * sn;

      c[0] =
        tmpgain * ((tmpgain + 1.0f) - (tmpgain - 1.0f) * cs +
          beta * sn) / tmp;
      c[1] =
        2.0f * tmpgain * ((tmpgain - 1.0f) - (tmpgain + 1.0f) * cs) / tmp;
      c[2] =
        tmpgain * ((tmpgain + 1.0f) - (tmpgain - 1.0f) * cs -
          beta * sn) / tmp;
      d[1] = -2.0f * ((tmpgain - 1.0f) + (tmpgain + 1.0f) * cs) / tmp * (-1.0f);
      d[2] =
        ((tmpgain + 1.0f) + (tmpgain - 1.0f) * cs - beta * sn) / tmp * (-1.0f);
    }
    else
    {
      c[0] = tmpgain;
      c[1] = 0.0;
      c[2] = 0.0;
      d[1] = 0.0;
      d[2] = 0.0;
    }
    order = 2;
    return;
  case 8:			//High Shelf - 2 poles
    if (zerocoefs == 0)
    {
      omega = D_PI * freq / ifSAMPLE_RATE;
      sn = sinf(omega);
      cs = cosf(omega);
      tmpq = sqrtf(tmpq);
      alpha = sn / (2.0f * tmpq);
      beta = sqrtf(tmpgain) / tmpq;
      tmp = (tmpgain + 1.0f) - (tmpgain - 1.0f) * cs + beta * sn;

      c[0] =
        tmpgain * ((tmpgain + 1.0f) + (tmpgain - 1.0f) * cs +
          beta * sn) / tmp;
      c[1] =
        -2.0f * tmpgain * ((tmpgain - 1.0f) + (tmpgain + 1.0f) * cs) / tmp;
      c[2] =
        tmpgain * ((tmpgain + 1.0f) + (tmpgain - 1.0f) * cs -
          beta * sn) / tmp;
      d[1] = 2.0f * ((tmpgain - 1.0f) - (tmpgain + 1.0f) * cs) / tmp * (-1.0f);
      d[2] =
        ((tmpgain + 1.0f) - (tmpgain - 1.0f) * cs - beta * sn) / tmp * (-1.0f);
    }
    else
    {
      c[0] = 1.0;
      c[1] = 0.0;
      c[2] = 0.0;
      d[1] = 0.0;
      d[2] = 0.0;
    }
    order = 2;
    return;
  default:			//wrong type
    type = 0;
    computefiltercoefs();
    return;
  }
}

void AnalogFilter::setfreq(f32 frequency)
{
  if (frequency < 0.1)
    frequency = 0.1f;
  f32 rap = freq / frequency;
  if (rap < 1.0)
    rap = 1.0f / rap;

  oldabovenq = abovenq;
  abovenq = frequency > (iSAMPLE_RATE / 2 - 500.0);

  i32 nyquistthresh = (abovenq ^ oldabovenq);

  if ((rap > 3.0) || (nyquistthresh != 0))
  {				//if the frequency is changed fast, it needs interpolation (now, filter and coeficients backup)
    for (i32 i = 0; i < 3; i++)
    {
      oldc[i] = c[i];
      oldd[i] = d[i];
    }
    for (i32 i = 0; i < MAX_FILTER_STAGES + 1; i++)
    {
      oldx[i] = x[i];
      oldy[i] = y[i];
    }
    if (firsttime == 0)
      needsinterpolation = 1;
  }
  freq = frequency;
  computefiltercoefs();
  firsttime = 0;
}

void AnalogFilter::setSR(u32 value)
{
  iSAMPLE_RATE = value;
  ifSAMPLE_RATE = f32(iSAMPLE_RATE);
  computefiltercoefs();
}

void AnalogFilter::setfreq_and_q(f32 frequency, f32 q_)
{
  q = q_;
  setfreq(frequency);
}

void AnalogFilter::setq(f32 q_)
{
  q = q_;
  computefiltercoefs();
}

void AnalogFilter::settype(i32 type_)
{
  type = type_;
  computefiltercoefs();
}

void AnalogFilter::setgain(f32 dBgain)
{
  gain = dB2rap(dBgain);
  computefiltercoefs();
}

void AnalogFilter::setstages(i32 stages_)
{
  if (stages_ >= MAX_FILTER_STAGES)
    stages_ = MAX_FILTER_STAGES - 1;
  stages = stages_;
  cleanup();
  computefiltercoefs();
}

void AnalogFilter::singlefilterout(f32* smp, fstage& x, fstage& y, f32* c, f32* d, const i32 blockSize)
{
  i32 i;
  f32 y0;
  if (order == 1)
  {				//First order filter
    for (i = 0; i < blockSize; ++i)
    {

      y0 = smp[i] * c[0] + x.c1 * c[1] + y.c1 * d[1];
      y.c1 = y0 + DENORMAL_GUARD;
      x.c1 = smp[i];
      //output
      smp[i] = y0;
    }
  }
  if (order == 2)
  {				//Second order filter
    for (i = 0; i < blockSize; ++i)
    {
      y0 =
        (smp[i] * c[0]) + (x.c1 * c[1]) + (x.c2 * c[2]) + (y.c1 * d[1]) +
        (y.c2 * d[2]);
      y.c2 = y.c1;
      y.c1 = y0 + DENORMAL_GUARD;
      x.c2 = x.c1;
      x.c1 = smp[i];
      //output
      smp[i] = y0;
    }
  }
}

void AnalogFilter::filterout(f32* smp, const i32 blockSize)
{
  f32* ismp = NULL;	//used if it needs interpolation
  if (needsinterpolation != 0)
  {
    ismp = new f32[blockSize];
    for (i32 i = 0; i < blockSize; i++)
      ismp[i] = smp[i];
    for (i32 i = 0; i < stages + 1; i++)
      singlefilterout(ismp, oldx[i], oldy[i], oldc, oldd, blockSize);
  }

  for (i32 i = 0; i < stages + 1; i++)
    singlefilterout(smp, x[i], y[i], c, d, blockSize);

  if (needsinterpolation != 0)
  {
    for (i32 i = 0; i < blockSize; i++)
    {
      f32 x = f32(i) / blockSize;
      smp[i] = ismp[i] * (1.0f - x) + smp[i] * x;
    }
    delete (ismp);
    needsinterpolation = 0;
  }
}

f32 AnalogFilter::filterout_s(f32 smp)
{
  if (needsinterpolation != 0)
  {
    for (i32 i = 0; i < stages + 1; i++)
      smp = singlefilterout_s(smp, oldx[i], oldy[i], oldc, oldd);
  }

  for (i32 i = 0; i < stages + 1; i++)
    smp = singlefilterout_s(smp, x[i], y[i], c, d);

  return(smp);
}

f32 AnalogFilter::singlefilterout_s(f32 smp, fstage& x, fstage& y, f32* c, f32* d)
{
  f32 y0;
  if (order == 1)
  {				//First order filter
    y0 = smp * c[0] + x.c1 * c[1] + y.c1 * d[1];
    y.c1 = y0;
    x.c1 = smp + DENORMAL_GUARD;
    //output
    smp = y0;

  }
  if (order == 2)
  {				//Second order filter
    y0 =
      (smp * c[0]) + (x.c1 * c[1]) + (x.c2 * c[2]) + (y.c1 * d[1]) +
      (y.c2 * d[2]);
    y.c2 = y.c1;
    y.c1 = y0 + DENORMAL_GUARD;
    x.c2 = x.c1;
    x.c1 = smp;
    //output
    smp = y0;
  }
  
  return smp;
}

void AnalogFilter::reversecoeffs()
{
  const f32 tmpd1 = -1.0f * d[1];
  const f32 tmpd2 = -1.0f * d[2];

  const f32 tmpc0 = 10.0f * c[0];

  c[0] = tmpc0;
  d[1] = -1.0f * c[1] * tmpc0;
  d[2] = -1.0f * c[2] * tmpc0;
  c[1] = tmpd1 * tmpc0;
  c[2] = tmpd2 * tmpc0;
}

f32 AnalogFilter::H(f32 freq)
{
  const f32 fr = freq / ifSAMPLE_RATE * D_PI;
  f32 x = c[0];
  f32 y = 0.0;
  for (i32 n = 1; n < 3; n++)
  {
    x += cosf((f32)n * fr) * c[n];
    y -= sinf((f32)n * fr) * c[n];
  }
  f32 h = x * x + y * y;
  x = 1.0;
  y = 0.0;
  for (i32 n = 1; n < 3; n++)
  {
    x -= cosf((f32)n * fr) * d[n];
    y += sinf((f32)n * fr) * d[n];
  }
  h = h / (x * x + y * y);
  return (powf(h, (f32)(stages + 1) / 2.0f));
}

#endif // SHR3D_SFX_CORE_RAKARRACK
