/*
  ZynAddSubFX - a software synthesizer

  RBFilter.C - Several state-variable filters
  Copyright (C) 2002-2005 Nasca Octavian Paul
  Author: Nasca Octavian Paul

  Modified for rakarrack by Ryan Billing

  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 "RBFilter.h"

#ifdef SHR3D_SFX_CORE_RAKARRACK

RBFilter::RBFilter(i32 Ftype, f32 Ffreq, f32 Fq, i32 Fstages)
{
  stages = Fstages;
  type = Ftype;
  freq = Ffreq;
  q = Fq;
  qmode = 0;
  gain = 1.0f;
  outgain = 1.0f;
  needsinterpolation = 0;
  firsttime = 1;
  en_mix = 0;
  oldq = 0.0f;
  oldsq = 0.0f;
  oldf = 0.0f;
  hpg = lpg = bpg = 0.0f;
  if (stages >= MAX_FILTER_STAGES)
    stages = MAX_FILTER_STAGES;
  cleanup();
  setfreq_and_q(Ffreq, Fq);
  iper = 1.0f / fPERIOD;
  a_smooth_tc = cSAMPLE_RATE / (cSAMPLE_RATE + 0.01f);  //10ms time constant for averaging coefficients
  b_smooth_tc = 1.0f - a_smooth_tc;
}

void RBFilter::cleanup()
{
  for (i32 i = 0; i < MAX_FILTER_STAGES + 1; i++)
  {
    st[i].low = 0.0;
    st[i].high = 0.0;
    st[i].band = 0.0;
    st[i].notch = 0.0;
  }
  oldabovenq = 0;
  abovenq = 0;
}

void RBFilter::computefiltercoefs()
{
  par.f = 2.0f * sinf(PI_ * freq / fSAMPLE_RATE);
  if (par.f > 0.99999)
    par.f = 0.99999f;
  par.q = 1.0f - atanf(sqrtf(q)) * 2.0f / PI_;
  par.q = powf(par.q, 1.0f / (f32)(stages + 1));
  par.q_sqrt = sqrtf(par.q);
}

void RBFilter::computefiltercoefs_hiQ()  //potentially unstable at some settings, but better sound
{
  par.f = 2.0f * sinf(PI_ * freq / fSAMPLE_RATE);
  if (par.f > 0.99999)
    par.f = 0.99999f;
  if (q < 0.5f)
    q = 0.5f;
  par.q = 1.0f / q;
  par.q = powf(par.q, 1.0f / (f32)(stages + 1));
  par.q_sqrt = 1.0f;
}

void RBFilter::setmode(i32 mode)
{
  if (mode)
    qmode = 1;
  else
    qmode = 0;
}

void RBFilter::setfreq(f32 frequency)
{
  if (frequency > (fSAMPLE_RATE / 2.0f - 500.0f))
    frequency = fSAMPLE_RATE / 2.0f - 500.0f;
  if (frequency < 0.1)
    frequency = 0.1f;
  f32 rap = freq / frequency;
  if (rap < 1.0)
    rap = 1.0f / rap;

  oldabovenq = abovenq;
  abovenq = frequency > (fSAMPLE_RATE / 2.0f - 500.0f);

  i32 nyquistthresh = (abovenq ^ oldabovenq);

  if ((rap > 3.0) || (nyquistthresh != 0))
  {				//if the frequency is changed fast, it needs interpolation (now, filter and coeficients backup)
    if (firsttime == 0)
      needsinterpolation = 1;
    ipar = par;
  }
  freq = frequency;

  if (!qmode)
    computefiltercoefs();
  else
    computefiltercoefs_hiQ();
  firsttime = 0;
}

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

void RBFilter::setq(f32 q_)
{
  q = q_;
  if (!qmode)
    computefiltercoefs();
  else
    computefiltercoefs_hiQ();
}

void RBFilter::settype(i32 type_)
{
  type = type_;
  if (!qmode)
    computefiltercoefs();
  else
    computefiltercoefs_hiQ();
}

void RBFilter::setgain(f32 dBgain)
{
  gain = dB2rap(dBgain);
  if (!qmode)
    computefiltercoefs();
  else
    computefiltercoefs_hiQ();
}

void RBFilter::setstages(i32 stages_)
{
  if (stages_ >= MAX_FILTER_STAGES)
    stages_ = MAX_FILTER_STAGES - 1;
  stages = stages_;
  cleanup();
  if (!qmode)
    computefiltercoefs();
  else
    computefiltercoefs_hiQ();
}

void RBFilter::setmix(i32 mix, f32 lpmix, f32 bpmix, f32 hpmix)
{
  if (mix)
    en_mix = 1;
  else
    en_mix = 0;

  lpg = lpmix;
  bpg = bpmix;
  hpg = hpmix;
}

void RBFilter::singlefilterout(f32* smp, fstage& x, parameters& par_, const i32 blockSize)
{
  f32* out = NULL;
  switch (type)
  {
  case 0:
    out = &x.low;
    break;
  case 1:
    out = &x.high;
    break;
  case 2:
    out = &x.band;
    break;
  case 3:
    out = &x.notch;
    break;
  }

  f32 tmpq, tmpsq, tmpf, qdiff, sqdiff, fdiff;
  qdiff = (par_.q - oldq) * iper;
  sqdiff = (par_.q_sqrt - oldsq) * iper;
  fdiff = (par_.f - oldf) * iper;
  tmpq = oldq;
  tmpsq = oldsq;
  tmpf = oldf;

  for (i32 i = 0; i < blockSize; i++)
  {
    tmpq += qdiff;
    tmpsq += sqdiff;
    tmpf += fdiff;   //Modulation interpolation

    x.low = x.low + tmpf * x.band;
    x.high = tmpsq * smp[i] - x.low - tmpq * x.band;
    //x.high = smp[i] - x.low - tmpq * x.band;     
    x.band = tmpf * x.high + x.band;

    if (en_mix)
    {
      smp[i] = lpg * x.low + hpg * x.high + bpg * x.band;
    }
    else
    {
      x.notch = x.high + x.low;
      smp[i] = *out;
    }
  }

  oldf = par_.f;
  oldq = par_.q;
  oldsq = par_.q_sqrt;
}

void RBFilter::filterout(f32* smp, const i32 blockSize)
{
  if (needsinterpolation != 0)
  {
    f32* 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, st[i], ipar, blockSize);

    delete[] ismp;
    needsinterpolation = 0;
  }

  for (i32 i = 0; i < stages + 1; i++)
    singlefilterout(smp, st[i], par, blockSize);

  for (i32 i = 0; i < blockSize; i++)
    smp[i] *= outgain;
}

f32 RBFilter::filterout_s(f32 smp)
{
  if (needsinterpolation != 0)
  {
    for (i32 i = 0; i < stages + 1; i++)
      smp = singlefilterout_s(smp, st[i], ipar);
    needsinterpolation = 0;
  }

  for (i32 i = 0; i < stages + 1; i++)
    smp = singlefilterout_s(smp, st[i], par);

  return (smp *= outgain);
}

f32 RBFilter::singlefilterout_s(f32 smp, fstage& x, parameters& par_)
{
  f32* out = NULL;
  switch (type)
  {
  case 0:
    out = &x.low;
    break;
  case 1:
    out = &x.high;
    break;
  case 2:
    out = &x.band;
    break;
  case 3:
    out = &x.notch;
    break;
  }

  oldq = b_smooth_tc * oldq + a_smooth_tc * par_.q;
  oldsq = b_smooth_tc * oldsq + a_smooth_tc * par_.q_sqrt;
  oldf = b_smooth_tc * oldf + a_smooth_tc * par_.f;   //modulation interpolation

  x.low = x.low + oldf * x.band;
  x.high = oldsq * smp - x.low - oldq * x.band;
  //x.high = smp - x.low - oldq * x.band;
  x.band = oldf * x.high + x.band;

  if (en_mix)
  {
    smp = lpg * x.low + hpg * x.high + bpg * x.band;
  }
  else
  {
    x.notch = x.high + x.low;
    smp = *out;
  }

  oldf = par_.f;
  oldq = par_.q;
  oldsq = par_.q_sqrt;

  return smp;
}

#endif // SHR3D_SFX_CORE_RAKARRACK
