/*
  ZynAddSubFX - a software synthesizer

  FormantFilter.C - formant filters
  Copyright (C) 2002-2005 Nasca Octavian Paul
  Author: Nasca Octavian Paul

  Modified for rakarrack by 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 "FormantFilter.h"

#ifdef SHR3D_SFX_CORE_RAKARRACK

#define AMPLITUDE_INTERPOLATION_THRESHOLD 0.0001f
#define ABOVE_AMPLITUDE_THRESHOLD(a,b) ( ( 2.0f*fabs( (b) - (a) ) / ( fabs( (b) + (a) + 0.0000000001f) ) ) > AMPLITUDE_INTERPOLATION_THRESHOLD )
#define INTERPOLATE_AMPLITUDE(a,b,x,size) ( (a) + ( (b) - (a) ) * (f32)(x) / (f32) (size) )

FormantFilter::FormantFilter(FilterParams& pars)
{
  numformants = pars.Pnumformants;
  for (i32 i = 0; i < numformants; i++)
    formant[i] = new AnalogFilter(4 /*BPF*/, 1000.0f, 10.0f, pars.Pstages);
  cleanup();

  for (i32 j = 0; j < FF_MAX_VOWELS; j++)
  {
    for (i32 i = 0; i < numformants; i++)
    {
      formantpar[j][i].freq =
        pars.getformantfreq(pars.Pvowels[j].formants[i].freq);
      formantpar[j][i].amp =
        pars.getformantamp(pars.Pvowels[j].formants[i].amp);
      formantpar[j][i].q =
        pars.getformantq(pars.Pvowels[j].formants[i].q);
    }
  }

  for (i32 i = 0; i < FF_MAX_FORMANTS; i++)
    oldformantamp[i] = 1.0;

  for (i32 i = 0; i < numformants; i++)
  {
    currentformants[i].freq = 1000.0f;
    currentformants[i].amp = 1.0f;
    currentformants[i].q = 2.0f;
  }

  formantslowness = powf(1.0f - ((f32)pars.Pformantslowness / 128.0f), 3.0f);

  sequencesize = pars.Psequencesize;
  if (sequencesize == 0)
    sequencesize = 1;
  for (i32 k = 0; k < sequencesize; k++)
    sequence[k].nvowel = pars.Psequence[k].nvowel;

  vowelclearness = powf(10.0f, ((f32)pars.Pvowelclearness - 32.0f) / 48.0f);

  sequencestretch = powf(0.1f, ((f32)pars.Psequencestretch - 32.0f) / 48.0f);
  if (pars.Psequencereversed)
    sequencestretch *= -1.0f;

  outgain = dB2rap(pars.getgain());

  oldinput = -1.0f;
  Qfactor = 1.0f;
  oldQfactor = Qfactor;
  firsttime = 1;
};

FormantFilter::~FormantFilter()
{
  for (i32 i = 0; i < numformants; i++)
    delete (formant[i]);
}

void FormantFilter::cleanup()
{
  for (i32 i = 0; i < numformants; i++)
    formant[i]->cleanup();
}

void FormantFilter::setpos(f32 input)
{
  i32 p1, p2;

  if (firsttime != 0)
    slowinput = input;
  else
    slowinput = slowinput * (1.0f - formantslowness) + input * formantslowness;

  if ((fabsf(oldinput - input) < 0.001)
    && (fabsf(slowinput - input) < 0.001)
    && (fabsf(Qfactor - oldQfactor) < 0.001))
  {
    //      oldinput=input; daca setez asta, o sa faca probleme la schimbari foarte lente
    firsttime = 0;
    return;
  }
  else
  {
    oldinput = input;
  }


  f32 pos = fmodf(input * sequencestretch, 1.0f);
  if (pos < 0.0)
    pos += 1.0f;

  F2I(pos * (f32)sequencesize, p2);
  p1 = p2 - 1;
  if (p1 < 0)
    p1 += sequencesize;

  pos = fmodf(pos * (f32)sequencesize, 1.0f);
  if (pos < 0.0)
    pos = 0.0f;
  else if (pos > 1.0)
    pos = 1.0f;
  pos =
    (atanf((pos * 2.0f - 1.0f) * vowelclearness) / atanf(vowelclearness) +
      1.0f) * 0.5f;

  p1 = sequence[p1].nvowel;
  p2 = sequence[p2].nvowel;

  if (firsttime != 0)
  {
    for (i32 i = 0; i < numformants; i++)
    {
      currentformants[i].freq =
        formantpar[p1][i].freq * (1.0f - pos) +
        formantpar[p2][i].freq * pos;
      currentformants[i].amp =
        formantpar[p1][i].amp * (1.0f - pos) + formantpar[p2][i].amp * pos;
      currentformants[i].q =
        formantpar[p1][i].q * (1.0f - pos) + formantpar[p2][i].q * pos;
      formant[i]->setfreq_and_q(currentformants[i].freq,
        currentformants[i].q * Qfactor);
      oldformantamp[i] = currentformants[i].amp;
    };
    firsttime = 0;
  }
  else
  {
    for (i32 i = 0; i < numformants; i++)
    {
      currentformants[i].freq =
        currentformants[i].freq * (1.0f - formantslowness) +
        (formantpar[p1][i].freq * (1.0f - pos) +
          formantpar[p2][i].freq * pos) * formantslowness;

      currentformants[i].amp =
        currentformants[i].amp * (1.0f - formantslowness) +
        (formantpar[p1][i].amp * (1.0f - pos) +
          formantpar[p2][i].amp * pos) * formantslowness;

      currentformants[i].q =
        currentformants[i].q * (1.0f - formantslowness) +
        (formantpar[p1][i].q * (1.0f - pos) +
          formantpar[p2][i].q * pos) * formantslowness;

      formant[i]->setfreq_and_q(currentformants[i].freq,
        currentformants[i].q * Qfactor);
    }
  }

  oldQfactor = Qfactor;
}

void FormantFilter::setfreq(f32 frequency)
{
  setpos(frequency);
}

void FormantFilter::setq(f32 q_)
{
  Qfactor = q_;
  for (i32 i = 0; i < numformants; i++)
    formant[i]->setq(Qfactor * currentformants[i].q);
}

void FormantFilter::setgain(f32 /*dBgain*/)
{
}

void FormantFilter::setfreq_and_q(f32 frequency, f32 q_)
{
  Qfactor = q_;
  setpos(frequency);
}

void FormantFilter::filterout(f32* smp, const i32 blockSize)
{
  for (i32 i = 0; i < blockSize; i++)
  {
    inbuffer[i] = smp[i];
    smp[i] = 0.0;
  }

  for (i32 j = 0; j < numformants; j++)
  {
    for (i32 i = 0; i < blockSize; i++)
      tmpbuf[i] = inbuffer[i] * outgain;
    formant[j]->filterout(tmpbuf, blockSize);

    if (ABOVE_AMPLITUDE_THRESHOLD
    (oldformantamp[j], currentformants[j].amp))
      for (i32 i = 0; i < blockSize; i++)
        smp[i] +=
        tmpbuf[i] * INTERPOLATE_AMPLITUDE(oldformantamp[j],
          currentformants[j].amp, i,
          blockSize);
    else
      for (i32 i = 0; i < blockSize; i++)
        smp[i] += tmpbuf[i] * currentformants[j].amp;
    oldformantamp[j] = currentformants[j].amp;
  }
}

#endif // SHR3D_SFX_CORE_RAKARRACK
