/*
  ZynAddSubFX - a software synthesizer

  FilterParams.C - Parameters for filter
  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 "FilterParams.h"

#ifdef SHR3D_SFX_CORE_RAKARRACK

FilterParams::FilterParams(u8 Ptype_, u8 Pfreq_, u8 Pq_)
  : Dtype(Ptype_)
  , Dfreq(Pfreq_)
  , Dq(Pq_)
{
  // setpresettype("Pfilter");
  defaults();
}

void FilterParams::defaults()
{
  Ptype = Dtype;
  Pfreq = Dfreq;
  Pq = Dq;

  Pstages = 0;
  Pfreqtrack = 64;
  Pgain = 64;
  Pcategory = 0;

  Pnumformants = 3;
  Pformantslowness = 64;

  for (i32 j = 0; j < FF_MAX_VOWELS; j++)
  {
    defaults(j);
  }

  Psequencesize = 3;
  for (i32 i = 0; i < FF_MAX_SEQUENCE; i++)
    Psequence[i].nvowel = (u8)i % FF_MAX_VOWELS;

  Psequencestretch = 40;
  Psequencereversed = 0;
  Pcenterfreq = 64;		//1 kHz
  Poctavesfreq = 64;
  Pvowelclearness = 64;
}

void FilterParams::defaults(i32 n)
{
  for (i32 i = 0; i < FF_MAX_FORMANTS; i++)
  {
    Pvowels[n].formants[i].freq = (u8)(RND * 127.0);	//some random freqs
    Pvowels[n].formants[i].q = 64;
    Pvowels[n].formants[i].amp = 127;
  }
}

void FilterParams::getfromFilterParams(FilterParams* pars)
{
  defaults();

  if (pars == NULL)
    return;

  Ptype = pars->Ptype;
  Pfreq = pars->Pfreq;
  Pq = pars->Pq;

  Pstages = pars->Pstages;
  Pfreqtrack = pars->Pfreqtrack;
  Pgain = pars->Pgain;
  Pcategory = pars->Pcategory;

  Pnumformants = pars->Pnumformants;
  Pformantslowness = pars->Pformantslowness;

  for (i32 j = 0; j < FF_MAX_VOWELS; j++)
  {
    for (i32 i = 0; i < FF_MAX_FORMANTS; i++)
    {
      Pvowels[j].formants[i].freq = pars->Pvowels[j].formants[i].freq;
      Pvowels[j].formants[i].q = pars->Pvowels[j].formants[i].q;
      Pvowels[j].formants[i].amp = pars->Pvowels[j].formants[i].amp;
    }
  }

  Psequencesize = pars->Psequencesize;

  for (i32 i = 0; i < FF_MAX_SEQUENCE; i++)
    Psequence[i].nvowel = pars->Psequence[i].nvowel;

  Psequencestretch = pars->Psequencestretch;
  Psequencereversed = pars->Psequencereversed;
  Pcenterfreq = pars->Pcenterfreq;
  Poctavesfreq = pars->Poctavesfreq;
  Pvowelclearness = pars->Pvowelclearness;
}

/*
 * Parameter control
 */
f32 FilterParams::getfreq()
{
  return (((f32)Pfreq / 64.0f - 1.0f) * 5.0f);
}

f32 FilterParams::getq()
{
  return (expf(powf((f32)Pq / 127.0f, 2) * logf(1000.0f)) - 0.9f);
}

f32 FilterParams::getfreqtracking(f32 notefreq)
{
  return (logf(notefreq / 440.0f) * ((f32)Pfreqtrack - 64.0f) / (64.0f * LOG_2));
}

f32 FilterParams::getgain()
{
  return (((f32)Pgain / 64.0f - 1.0f) * 30.0f);	//-30..30dB
}

/*
 * Get the center frequency of the formant's graph
 */
f32 FilterParams::getcenterfreq()
{
  return (10000.0f * powf(10.0f, -(1.0f - (f32)Pcenterfreq / 127.0f) * 2.0f));
}

/*
 * Get the number of octave that the formant functions applies to
 */
f32 FilterParams::getoctavesfreq()
{
  return (0.25f + 10.0f * (f32)Poctavesfreq / 127.0f);
}

/*
 * Get the frequency from x, where x is [0..1]
 */
f32 FilterParams::getfreqx(f32 x)
{
  if (x > 1.0)
    x = 1.0f;
  f32 octf = powf(2.0f, getoctavesfreq());
  return (getcenterfreq() / sqrtf(octf) * powf(octf, x));
}

/*
 * Get the x coordinate from frequency (used by the UI)
 */
f32 FilterParams::getfreqpos(f32 freq)
{
  return ((logf(freq) - logf(getfreqx(0.0))) / logf(2.0f) / getoctavesfreq());
}

/*
 * Get the freq. response of the formant filter
 */
void FilterParams::formantfilterH(i32 nvowel, i32 nfreqs, f32* freqs)
{
  f32 c[3], d[3];
  f32 filter_freq, filter_q, filter_amp;
  f32 omega, sn, cs, alpha;

  for (i32 i = 0; i < nfreqs; i++)
    freqs[i] = 0.0;

  //for each formant...
  for (i32 nformant = 0; nformant < Pnumformants; nformant++)
  {
    //compute formant parameters(frequency,amplitude,etc.)
    filter_freq = getformantfreq(Pvowels[nvowel].formants[nformant].freq);
    filter_q = getformantq(Pvowels[nvowel].formants[nformant].q) * getq();
    if (Pstages > 0)
      filter_q =
      (filter_q > 1.0 ? powf(filter_q, 1.0f / ((f32)Pstages + 1)) : filter_q);

    filter_amp = getformantamp(Pvowels[nvowel].formants[nformant].amp);


    if (filter_freq <= (SAMPLE_RATE / 2 - 100.0))
    {
      omega = 2.0f * PI_ * filter_freq / fSAMPLE_RATE;
      sn = sinf(omega);
      cs = cosf(omega);
      alpha = sn / (2.0f * filter_q);
      f32 tmp = 1.0f + alpha;
      c[0] = alpha / tmp * sqrtf(filter_q + 1.0f);
      c[1] = 0;
      c[2] = -alpha / tmp * sqrtf(filter_q + 1.0f);
      d[1] = -2.0f * cs / tmp * (-1.0f);
      d[2] = (1.0f - alpha) / tmp * (-1.0f);
    }
    else
    {
      continue;
    }

    for (i32 i = 0; i < nfreqs; i++)
    {
      f32 freq = getfreqx((f32)i / (f32)nfreqs);
      if (freq > SAMPLE_RATE / 2)
      {
        for (i32 tmp = i; tmp < nfreqs; tmp++)
          freqs[tmp] = 0.0;
        break;
      }
      f32 fr = freq / fSAMPLE_RATE * PI_ * 2.0f;
      f32 x = c[0], y = 0.0f;
      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.0f;
      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);

      freqs[i] += powf(h, ((f32)Pstages + 1.0f) / 2.0f) * filter_amp;
    }
  }
  for (i32 i = 0; i < nfreqs; i++)
  {
    if (freqs[i] > 0.000000001f)
      freqs[i] = rap2dB(freqs[i]) + getgain();
    else
      freqs[i] = -90.0f;
  }
}

/*
 * Transforms a parameter to the real value
 */
f32 FilterParams::getformantfreq(u8 freq)
{
  const f32 result = getfreqx((f32)freq / 127.0f);
  return result;
}

f32 FilterParams::getformantamp(u8 amp)
{
  const f32 result = powf(0.1f, (1.0f - (f32)amp / 127.0f) * 4.0f);
  return result;
}

f32 FilterParams::getformantq(u8 q)
{
  //temp
  const f32 result = powf(25.0f, ((f32)q - 32.0f) / 64.0f);
  return result;
}

#endif // SHR3D_SFX_CORE_RAKARRACK
