/*

  APhaser.C  - Approximate digital model of an analog JFET phaser.
  Analog modeling implemented by Ryan Billing aka Transmogrifox.
  November, 2009

  Credit to:
  ///////////////////
  ZynAddSubFX - a software synthesizer

  Phaser.C - Phaser effect
  Copyright (C) 2002-2005 Nasca Octavian Paul
  Author: Nasca Octavian Paul

  Modified for rakarrack by Josep Andreu

  DSP analog modeling theory & practice largely influenced by various CCRMA publications, particularly works by Julius O. Smith.
  ////////////////////


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

#ifdef SHR3D_SFX_CORE_RAKARRACK


#define PHASER_LFO_SHAPE 2
#define ONE_  0.99999f        // To prevent LFO ever reaching 1.0 for filter stability purposes
#define ZERO_ 0.00001f        // Same idea as above.

AnalogPhaser::AnalogPhaser()
{
  CFs = 2.0f * fSAMPLE_RATE * C;
  invperiod = 1.0f / fPERIOD;

  setpreset(0);
  cleanup();
}

void AnalogPhaser::processBlock(const f32* const* inBlock, f32** outBlock, const i32 blockSize)
{
  i32 i, j;
  f32 lfol, lfor, lgain, rgain, bl, br, gl, gr, rmod, lmod, d, hpfr, hpfl;
  lgain = 0.0;
  rgain = 0.0;

  //initialize hpf
  hpfl = 0.0;
  hpfr = 0.0;

  lfo.effectlfoout(lfol, lfor);
  lmod = lfol * width + depth;
  rmod = lfor * width + depth;

  if (lmod > ONE_)
    lmod = ONE_;
  else if (lmod < ZERO_)
    lmod = ZERO_;
  if (rmod > ONE_)
    rmod = ONE_;
  else if (rmod < ZERO_)
    rmod = ZERO_;

  if (Phyper != 0)
  {
    lmod *= lmod;  //Triangle wave squared is approximately sin on bottom, tri on top
    rmod *= rmod;  //Result is exponential sweep more akin to filter in synth with exponential generator circuitry.
  }

  lmod = sqrtf(1.0f - lmod);  //gl,gr is Vp - Vgs. Typical FET drain-source resistance follows constant/[1-sqrt(Vp - Vgs)] 
  rmod = sqrtf(1.0f - rmod);

  rdiff = (rmod - oldrgain) * invperiod;
  ldiff = (lmod - oldlgain) * invperiod;

  gl = oldlgain;
  gr = oldrgain;

  oldlgain = lmod;
  oldrgain = rmod;

  for (i = 0; i < blockSize; i++)
  {
    gl += ldiff;	// Linear interpolation between LFO samples
    gr += rdiff;

    f32 lxn = inBlock[0][i];
    f32 rxn = inBlock[1][i];

    if (barber) {
      gl = fmodf((gl + 0.25f), ONE_);
      gr = fmodf((gr + 0.25f), ONE_);
    }

    //Left channel
    for (j = 0; j < Pstages; j++)
    {			//Phasing routine
      mis = 1.0f + offsetpct * offset[j];
      d = (1.0f + 2.0f * (0.25f + gl) * hpfl * hpfl * distortion) * mis;  //This is symmetrical. FET is not, so this deviates slightly, however sym dist. is better sounding than a real FET.
      Rconst = 1.0f + mis * Rmx;
      bl = (Rconst - gl) / (d * Rmin);  // This is 1/R. R is being modulated to control filter fc.
      lgain = (CFs - bl) / (CFs + bl);

      lyn1[j] = lgain * (lxn + lyn1[j]) - lxn1[j];
      lyn1[j] += DENORMAL_GUARD;
      hpfl = lyn1[j] + (1.0f - lgain) * lxn1[j];  //high pass filter -- Distortion depends on the high-pass part of the AP stage. 

      lxn1[j] = lxn;
      lxn = lyn1[j];
      if (j == 1)
        lxn += fbl;  //Insert feedback after first phase stage
    }

    //Right channel
    for (j = 0; j < Pstages; j++)
    {			//Phasing routine
      mis = 1.0f + offsetpct * offset[j];
      d = (1.0f + 2.0f * (0.25f + gr) * hpfr * hpfr * distortion) * mis;   // distortion
      Rconst = 1.0f + mis * Rmx;
      br = (Rconst - gr) / (d * Rmin);
      rgain = (CFs - br) / (CFs + br);

      ryn1[j] = rgain * (rxn + ryn1[j]) - rxn1[j];
      ryn1[j] += DENORMAL_GUARD;
      hpfr = ryn1[j] + (1.0f - rgain) * rxn1[j];  //high pass filter

      rxn1[j] = rxn;
      rxn = ryn1[j];
      if (j == 1) rxn += fbr;  //Insert feedback after first phase stage
    }

    fbl = lxn * fb;
    fbr = rxn * fb;
    outBlock[0][i] = lxn;
    outBlock[1][i] = rxn;
  }

  if (Poutsub != 0)
  {
    for (i = 0; i < blockSize; i++)
    {
      outBlock[0][i] *= -1.0f;
      outBlock[1][i] *= -1.0f;
    }
  }
}

void AnalogPhaser::cleanup()
{
  fbl = 0.0;
  fbr = 0.0;
  oldlgain = 0.0;
  oldrgain = 0.0;
  for (i32 i = 0; i < Pstages; i++)
  {
    lxn1[i] = 0.0;

    lyn1[i] = 0.0;

    rxn1[i] = 0.0;

    ryn1[i] = 0.0;

  }
}

void AnalogPhaser::setwidth(i32 Pwidth_)
{
  Pwidth = Pwidth_;
  width = ((f32)Pwidth_ / 127.0f);
}

void AnalogPhaser::setfb(i32 Pfb_)
{
  Pfb = Pfb_;
  fb = (f32)(Pfb_ - 64) / 64.2f;
}

void AnalogPhaser::setvolume(i32 Pvolume_)
{
  Pvolume = Pvolume_;
  // outvolume is needed in calling program
  outvolume = (f32)Pvolume_ / 127.0f;
}

void AnalogPhaser::setdistortion(i32 Pdistortion_)
{
  Pdistortion = Pdistortion_;
  distortion = (f32)Pdistortion_ / 127.0f;
}

void AnalogPhaser::setoffset(i32 Poffset_)
{
  Poffset = Poffset_;
  offsetpct = (f32)Poffset_ / 127.0f;
}

void AnalogPhaser::setstages(i32 Pstages_)
{
  if (Pstages_ >= MAX_PHASER_STAGES)
    Pstages_ = MAX_PHASER_STAGES;
  Pstages = Pstages_;

  cleanup();
}

void AnalogPhaser::setdepth(i32 Pdepth_)
{
  Pdepth = Pdepth_;
  depth = (f32)(Pdepth_ - 64) / 127.0f;  //Pdepth input should be 0-127.  depth shall range 0-0.5 since we don't need to shift the full spectrum.
}

void AnalogPhaser::setpreset(i32 npreset)
{
  const i32 PRESET_SIZE = 13;
  i32 presets[][PRESET_SIZE] = {
    //Phaser1
    {64, 20, 14, 0, 1, 64, 110, 40, 4, 10, 0, 64, 1},
    //Phaser2
    {64, 20, 14, 5, 1, 64, 110, 40, 6, 10, 0, 70, 1},
    //Phaser3
    {64, 20, 9, 0, 0, 64, 40, 40, 8, 10, 0, 60, 0},
    //Phaser4
    {64, 20, 14, 10, 0, 64, 110, 80, 7, 10, 1, 45, 1},
    //Phaser5
    {25, 20, 240, 10, 0, 64, 25, 16, 8, 100, 0, 25, 0},
    //Phaser6
    {64, 20, 1, 10, 1, 64, 110, 40, 12, 10, 0, 70, 1}
  };

  for (i32 n = 0; n < PRESET_SIZE; n++)
    changepar(n, presets[npreset][n]);
}

void AnalogPhaser::changepar(i32 npar, i32 value)
{
  switch (npar)
  {
  case 0:
    setvolume(value);
    return;
  case 1:
    setdistortion(value);
    return;
  case 2:
    lfo.Pfreq = value;
    lfo.updateparams();
    return;
  case 3:
    lfo.Prandomness = value;
    lfo.updateparams();
    return;
  case 4:
    lfo.PLFOtype = value;
    lfo.updateparams();
    barber = 0;
    if (value == 2) barber = 1;
    return;
  case 5:
    lfo.Pstereo = value;
    lfo.updateparams();
    return;
  case 6:
    setwidth(value);
    return;
  case 7:
    setfb(value);
    return;
  case 8:
    setstages(value);
    return;
  case 9:
    setoffset(value);
    return;
  case 10:
    if (value > 1)
      value = 1;
    Poutsub = value;
    return;
  case 11:
    setdepth(value);
    return;
  case 12:
    if (value > 1)
      value = 1;
    Phyper = value;
    return;
  }
  ASSERT(false);
}

i32 AnalogPhaser::getpar(i32 npar)
{
  switch (npar)
  {
  case 0:
    return Pvolume;
  case 1:
    return Pdistortion;
  case 2:
    return lfo.Pfreq;
  case 3:
    return lfo.Prandomness;
  case 4:
    return lfo.PLFOtype;
  case 5:
    return lfo.Pstereo;
  case 6:
    return Pwidth;
  case 7:
    return Pfb;
  case 8:
    return Pstages;
  case 9:
    return Poffset;
  case 10:
    return Poutsub;
  case 11:
    return Pdepth;
  case 12:
    return Phyper;
  }
  ASSERT(false);
  return 0;
};

#endif // SHR3D_SFX_CORE_RAKARRACK
