/*
  rakarrack - a guitar effects software

 Vibe.C  -  Optical tremolo effect

  Copyright (C) 2008-2010 Ryan Billing
  Author: 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
 (version2)  along with this program; if not, write to the Free Software
 Foundation,
 Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA

*/

#include "Vibe.h"

#ifdef SHR3D_SFX_CORE_RAKARRACK

Vibe::Vibe()
{
  //Swing was measured on operating device of: 10K to 250k.  
  //400K is reported to sound better for the "low end" (high resistance) 
  //Because of time response, Rb needs to be driven further.
  //End resistance will max out to around 10k for most LFO freqs.
  //pushing low end a little lower for kicks and giggles
  Ra = 500000.0f;  //Cds cell dark resistance.
  Ra = logf(Ra);		//this is done for clarity 
  Rb = 600.0f;         //Cds cell full illumination
  b = exp(Ra / logf(Rb)) - CNST_E;
  dTC = 0.085f;
  dRCl = dTC;
  dRCr = dTC;   //Right & left channel dynamic time contsants
  minTC = logf(0.005f / dTC);
  alphal = 1.0f - cSAMPLE_RATE / (dRCl + cSAMPLE_RATE);
  alphar = alphal;
  dalphal = dalphar = alphal;
  lampTC = cSAMPLE_RATE / (0.02f + cSAMPLE_RATE);  //guessing 10ms
  ilampTC = 1.0f - lampTC;
  lstep = 0.0f;
  rstep = 0.0f;
  Pdepth = 127;
  Ppanning = 64;
  lpanning = 1.0f;
  rpanning = 1.0f;
  fdepth = 1.0f;
  oldgl = 0.0f;
  oldgr = 0.0f;
  gl = 0.0f;
  gr = 0.0f;
  for (i32 jj = 0; jj < 8; jj++)
    oldcvolt[jj] = 0.0f;
  cperiod = 1.0f / fPERIOD;

  init_vibes();
}

void Vibe::processBlock(const f32* const* inBlock, f32** outBlock, const i32 blockSize)
{
  f32 lfol, lfor;
  lfo.effectlfoout(lfol, lfor);

  lfol = fdepth + lfol * fwidth;
  lfor = fdepth + lfor * fwidth;

  if (lfol > 1.0f)
    lfol = 1.0f;
  else if (lfol < 0.0f)
    lfol = 0.0f;
  if (lfor > 1.0f)
    lfor = 1.0f;
  else if (lfor < 0.0f)
    lfor = 0.0f;

  lfor = 2.0f - 2.0f / (lfor + 1.0f);   // 
  lfol = 2.0f - 2.0f / (lfol + 1.0f); //emulate lamp turn on/off characteristic by typical curves 

  for (i32 i = 0; i < blockSize; i++)
  {
    //Left Lamp
    gl = lfol * lampTC + oldgl * ilampTC;
    oldgl = gl;
    //Right Lamp
    gr = lfor * lampTC + oldgr * ilampTC;
    oldgr = gr;

    //Left Cds   
    stepl = gl * alphal + dalphal * oldstepl;
    oldstepl = stepl;
    dRCl = dTC * expf(stepl * minTC);
    alphal = cSAMPLE_RATE / (dRCl + cSAMPLE_RATE);
    dalphal = 1.0f - cSAMPLE_RATE / (0.5f * dRCl + cSAMPLE_RATE);     //different attack & release character
    const f32 xl = CNST_E + stepl * b;
    const f32 fxl = expf(Ra / logf(xl));

    //Right Cds   
    stepr = gr * alphar + dalphar * oldstepr;
    oldstepr = stepr;
    dRCr = dTC * expf(stepr * minTC);
    alphar = cSAMPLE_RATE / (dRCr + cSAMPLE_RATE);
    dalphar = 1.0f - cSAMPLE_RATE / (0.5f * dRCr + cSAMPLE_RATE);      //different attack & release character
    const f32 xr = CNST_E + stepr * b;
    const f32 fxr = expf(Ra / logf(xr));

    if (i % 16 == 0)
      modulate(fxl, fxr);

    //Left Channel  

//    input = bjt_shape(fbl + inBlock[0][i]);  

    f32 vin = 7.5f * (1.0f + fbl + inBlock[0][i]);
    if (vin < 0.0f)
      vin = 0.0f;
    if (vin > 15.0f)
      vin = 15.0f;
    f32 vbe = 0.8f - 0.8f / (vin + 1.0f);  //really rough, simplistic bjt turn-on emulator
    f32 input = vin - vbe;
    input = input * 0.1333333333f - 0.90588f;  //some magic numbers to return gain to unity & zero the DC

    f32 emitterfb = 25.0f / fxl;
    f32 ocvolt = 0.0f;
    for (i32 j = 0; j < 4; j++) //4 stages phasing
    {
      f32 y0 = 0.0f;
      y0 = input * ecvc[j].n0 + ecvc[j].x1 * ecvc[j].n1 - ecvc[j].y1 * ecvc[j].d1;
      ecvc[j].y1 = y0 + DENORMAL_GUARD;
      ecvc[j].x1 = input;

      f32 x0 = 0.0f;
      f32 data = input + emitterfb * oldcvolt[j];
      x0 = data * vc[j].n0 + vc[j].x1 * vc[j].n1 - vc[j].y1 * vc[j].d1;
      vc[j].y1 = x0 + DENORMAL_GUARD;
      vc[j].x1 = data;

      const f32 cvolt = y0 + x0;

      ocvolt = cvolt * vcvo[j].n0 + vcvo[j].x1 * vcvo[j].n1 - vcvo[j].y1 * vcvo[j].d1;
      vcvo[j].y1 = ocvolt + DENORMAL_GUARD;
      vcvo[j].x1 = cvolt;

      oldcvolt[j] = ocvolt;

      const f32 evolt = input * vevo[j].n0 + vevo[j].x1 * vevo[j].n1 - vevo[j].y1 * vevo[j].d1;
      vevo[j].y1 = evolt + DENORMAL_GUARD;
      vevo[j].x1 = input;

      vin = 7.5f * (1.0f + ocvolt + evolt);
      if (vin < 0.0f)
        vin = 0.0f;
      if (vin > 15.0f)
        vin = 15.0f;
      vbe = 0.8f - 0.8f / (vin + 1.0f);  //really rough, simplistic bjt turn-on emulator
      input = vin - vbe;
      input = input * 0.1333333333f - 0.90588f;  //some magic numbers to return gain to unity & zero the DC

      //    cvolt = vibefilter(input,ecvc,j) + vibefilter(input + emitterfb*oldcvolt[j],vc,j);
      //    ocvolt = vibefilter(cvolt,vcvo,j);
      //    oldcvolt[j] = ocvolt;
      //    evolt = vibefilter(input, vevo,j);

      //    input = bjt_shape(ocvolt + evolt);
    }
    fbl = fb * ocvolt;
    const f32 outl = lpanning * input;

    //Right channel

    vin = 7.5f * (1.0f + fbr + inBlock[1][i]);
    if (vin < 0.0f)
      vin = 0.0f;
    if (vin > 15.0f)
      vin = 15.0f;
    vbe = 0.8f - 0.8f / (vin + 1.0f);  //really rough, simplistic bjt turn-on emulator
    input = vin - vbe;
    input = input * 0.1333333333f - 0.90588f;  //some magic numbers to return gain to unity & zero the DC

    //    input = bjt_shape(fbr + inBlock[1][i]);  

    emitterfb = 25.0f / fxr;
    for (i32 j = 4; j < 8; j++) //4 stages phasing
    {
      f32 y0 = 0.0f;
      y0 = input * ecvc[j].n0 + ecvc[j].x1 * ecvc[j].n1 - ecvc[j].y1 * ecvc[j].d1;
      ecvc[j].y1 = y0 + DENORMAL_GUARD;
      ecvc[j].x1 = input;

      f32 x0 = 0.0f;
      f32 data = input + emitterfb * oldcvolt[j];
      x0 = data * vc[j].n0 + vc[j].x1 * vc[j].n1 - vc[j].y1 * vc[j].d1;
      vc[j].y1 = x0 + DENORMAL_GUARD;
      vc[j].x1 = data;

      const f32 cvolt = y0 + x0;

      ocvolt = cvolt * vcvo[j].n0 + vcvo[j].x1 * vcvo[j].n1 - vcvo[j].y1 * vcvo[j].d1;
      vcvo[j].y1 = ocvolt + DENORMAL_GUARD;
      vcvo[j].x1 = cvolt;

      oldcvolt[j] = ocvolt;

      const f32 evolt = input * vevo[j].n0 + vevo[j].x1 * vevo[j].n1 - vevo[j].y1 * vevo[j].d1;
      vevo[j].y1 = evolt + DENORMAL_GUARD;
      vevo[j].x1 = input;

      vin = 7.5f * (1.0f + ocvolt + evolt);
      if (vin < 0.0f)
        vin = 0.0f;
      if (vin > 15.0f)
        vin = 15.0f;
      vbe = 0.8f - 0.8f / (vin + 1.0f);  //really rough, simplistic bjt turn-on emulator
      input = vin - vbe;
      input = input * 0.1333333333f - 0.90588f;  //some magic numbers to return gain to unity & zero the DC

      //    cvolt = vibefilter(input,ecvc,j) + vibefilter(input + emitterfb*oldcvolt[j],vc,j);
      //    ocvolt = vibefilter(cvolt,vcvo,j);
      //    oldcvolt[j] = ocvolt;
      //    evolt = vibefilter(input, vevo,j);

      //    input = bjt_shape(ocvolt + evolt);
    }

    fbr = fb * ocvolt;
    const f32 outr = rpanning * input;

    outBlock[0][i] = outl * fcross + outr * flrcross;
    outBlock[1][i] = outr * fcross + outl * flrcross;
  }
}

f32 Vibe::vibefilter(f32 data, fparams* ftype, i32 stage)
{
  f32 y0 = 0.0f;
  y0 = data * ftype[stage].n0 + ftype[stage].x1 * ftype[stage].n1 - ftype[stage].y1 * ftype[stage].d1;
  ftype[stage].y1 = y0 + DENORMAL_GUARD;
  ftype[stage].x1 = data;
  return y0;
}

f32 Vibe::bjt_shape(f32 data)
{
  f32 vbe, vout;
  f32 vin = 7.5f * (1.0f + data);
  if (vin < 0.0f) vin = 0.0f;
  if (vin > 15.0f) vin = 15.0f;
  vbe = 0.8f - 0.8f / (vin + 1.0f);  //really rough, simplistic bjt turn-on emulator
  vout = vin - vbe;
  vout = vout * 0.1333333333f - 0.90588f;  //some magic numbers to return gain to unity & zero the DC
  return vout;
}

void Vibe::init_vibes()
{
  k = 2.0f * fSAMPLE_RATE;
  f32 tmpgain = 1.0f;
  R1 = 4700.0f;
  Rv = 4700.0f;
  C2 = 1e-6f;
  beta = 150.0f;  //transistor forward gain.
  gain = -beta / (beta + 1.0f);

  //Univibe cap values 0.015uF, 0.22uF, 470pF, and 0.0047uF
  C1[0] = 0.015e-6f;
  C1[1] = 0.22e-6f;
  C1[2] = 470e-12f;
  C1[3] = 0.0047e-6f;
  C1[4] = 0.015e-6f;
  C1[5] = 0.22e-6f;
  C1[6] = 470e-12f;
  C1[7] = 0.0047e-6f;

  for (i32 i = 0; i < 8; i++)
  {
    //Vo/Ve driven from emitter
    en1[i] = k * R1 * C1[i];
    en0[i] = 1.0f;
    ed1[i] = k * (R1 + Rv) * C1[i];
    ed0[i] = 1.0f + C1[i] / C2;

    // Vc~=Ve/(Ic*Re*alpha^2) collector voltage from current input.  
    //Output here represents voltage at the collector

    cn1[i] = k * gain * Rv * C1[i];
    cn0[i] = gain * (1.0f + C1[i] / C2);
    cd1[i] = k * (R1 + Rv) * C1[i];
    cd0[i] = 1.0f + C1[i] / C2;

    //Contribution from emitter load through passive filter network
    ecn1[i] = k * gain * R1 * (R1 + Rv) * C1[i] * C2 / (Rv * (C2 + C1[i]));
    ecn0[i] = 0.0f;
    ecd1[i] = k * (R1 + Rv) * C1[i] * C2 / (C2 + C1[i]);
    ecd0[i] = 1.0f;

    // %Represents Vo/Vc.  Output over collector voltage
    on1[i] = k * Rv * C2;
    on0[i] = 1.0f;
    od1[i] = k * Rv * C2;
    od0[i] = 1.0f + C2 / C1[i];

    //%Bilinear xform stuff
    tmpgain = 1.0f / (cd1[i] + cd0[i]);
    vc[i].n1 = tmpgain * (cn0[i] - cn1[i]);
    vc[i].n0 = tmpgain * (cn1[i] + cn0[i]);
    vc[i].d1 = tmpgain * (cd0[i] - cd1[i]);
    vc[i].d0 = 1.0f;

    tmpgain = 1.0f / (ecd1[i] + ecd0[i]);
    ecvc[i].n1 = tmpgain * (ecn0[i] - ecn1[i]);
    ecvc[i].n0 = tmpgain * (ecn1[i] + ecn0[i]);
    ecvc[i].d1 = tmpgain * (ecd0[i] - ecd1[i]);
    ecvc[i].d0 = 1.0f;

    tmpgain = 1.0f / (od1[i] + od0[i]);
    vcvo[i].n1 = tmpgain * (on0[i] - on1[i]);
    vcvo[i].n0 = tmpgain * (on1[i] + on0[i]);
    vcvo[i].d1 = tmpgain * (od0[i] - od1[i]);
    vcvo[i].d0 = 1.0f;

    tmpgain = 1.0f / (ed1[i] + ed0[i]);
    vevo[i].n1 = tmpgain * (en0[i] - en1[i]);
    vevo[i].n0 = tmpgain * (en1[i] + en0[i]);
    vevo[i].d1 = tmpgain * (ed0[i] - ed1[i]);
    vevo[i].d0 = 1.0f;

    // bootstrap[i].n1
    // bootstrap[i].n0
    // bootstrap[i].d1
  }
}

void Vibe::modulate(f32 ldrl, f32 ldrr)
{
  f32 tmpgain;
  f32 R1pRv;
  f32 C2pC1;
  Rv = 4700.0f + ldrl;
  R1pRv = R1 + Rv;

  for (i32 i = 0; i < 8; i++)
  {
    if (i == 4) {
      Rv = 4700.0f + ldrr;
      R1pRv = R1 + Rv;
    }

    C2pC1 = C2 + C1[i];
    //Vo/Ve driven from emitter
    ed1[i] = k * (R1pRv)*C1[i];
    //ed1[i] = R1pRv*kC1[i];

    // Vc~=Ve/(Ic*Re*alpha^2) collector voltage from current input.  
    //Output here represents voltage at the collector
    cn1[i] = k * gain * Rv * C1[i];
    //cn1[i] = kgainCl[i]*Rv;
    //cd1[i] = (R1pRv)*C1[i];
    cd1[i] = ed1[i];

    //Contribution from emitter load through passive filter network
    ecn1[i] = k * gain * R1 * cd1[i] * C2 / (Rv * (C2pC1));
    //ecn1[i] = iC2pC1[i]*kgainR1C2*cd1[i]/Rv;
    ecd1[i] = k * cd1[i] * C2 / (C2pC1);
    //ecd1[i] = iC2pC1[i]*k*cd1[i]*C2/(C2pC1);

    // %Represents Vo/Vc.  Output over collector voltage
    on1[i] = k * Rv * C2;
    od1[i] = on1[i];

    //%Bilinear xform stuff
    tmpgain = 1.0f / (cd1[i] + cd0[i]);
    vc[i].n1 = tmpgain * (cn0[i] - cn1[i]);
    vc[i].n0 = tmpgain * (cn1[i] + cn0[i]);
    vc[i].d1 = tmpgain * (cd0[i] - cd1[i]);

    tmpgain = 1.0f / (ecd1[i] + ecd0[i]);
    ecvc[i].n1 = tmpgain * (ecn0[i] - ecn1[i]);
    ecvc[i].n0 = tmpgain * (ecn1[i] + ecn0[i]);
    ecvc[i].d1 = tmpgain * (ecd0[i] - ecd1[i]);
    ecvc[i].d0 = 1.0f;

    tmpgain = 1.0f / (od1[i] + od0[i]);
    vcvo[i].n1 = tmpgain * (on0[i] - on1[i]);
    vcvo[i].n0 = tmpgain * (on1[i] + on0[i]);
    vcvo[i].d1 = tmpgain * (od0[i] - od1[i]);

    tmpgain = 1.0f / (ed1[i] + ed0[i]);
    vevo[i].n1 = tmpgain * (en0[i] - en1[i]);
    vevo[i].n0 = tmpgain * (en1[i] + en0[i]);
    vevo[i].d1 = tmpgain * (ed0[i] - ed1[i]);
  }
}

void Vibe::setpanning(i32 value)
{
  Ppanning = value;
  rpanning = ((f32)Ppanning) / 64.0f;
  lpanning = 2.0f - rpanning;
  lpanning = 10.0f * powf(lpanning, 4);
  rpanning = 10.0f * powf(rpanning, 4);
  lpanning = 1.0f - 1.0f / (lpanning + 1.0f);
  rpanning = 1.0f - 1.0f / (rpanning + 1.0f);
  lpanning *= 1.3f;
  rpanning *= 1.3f;
}

void Vibe::setvolume(i32 value)
{
  Pvolume = value;
  outvolume = (f32)Pvolume / 127.0f;
}

void Vibe::setpreset(i32 npreset)
{
  const i32 PRESET_SIZE = 10;
  i32 presets[][PRESET_SIZE] = {
    //Classic
    {35, 120, 10, 0, 64, 64, 64, 64, 3, 64},
    //Stereo Classic
    {35, 120, 10, 0, 48, 64, 64, 64, 3, 64},
    //Wide Vibe
    {127, 80, 10, 0, 0, 64, 64, 64, 0, 64},
    //Classic Chorus
    {35, 360, 10, 0, 48, 64, 0, 64, 3, 64},
    //Vibe Chorus
    {75, 330, 10, 0, 50, 64, 0, 64, 17, 64},
    //Lush Chorus
    {55, 260, 10, 0, 64, 70, 0, 49, 20, 48},
    //Sick Phaser
    {110, 75, 10, 0, 32, 64, 64, 14, 0, 30},
    //Warble
    {127, 360, 10, 0, 0, 64, 0, 0, 0, 37}

  };

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

void Vibe::changepar(i32 npar, i32 value)
{
  switch (npar)
  {
  case 0:
    Pwidth = value;
    fwidth = ((f32)Pwidth) / 90.0f;
    return;
  case 1:
    lfo.Pfreq = value;
    lfo.updateparams();
    return;
  case 2:
    lfo.Prandomness = value;
    lfo.updateparams();
    return;
  case 3:
    lfo.PLFOtype = value;
    lfo.updateparams();
    return;
  case 4:
    lfo.Pstereo = value;
    lfo.updateparams();
    return;
  case 5: // pan
    setpanning(value);
    return;
  case 6:
    setvolume(value);
    return;
  case 7: //fb
    Pfb = value;
    fb = ((f32)(Pfb - 64)) / 65.0f;
    return;
  case 8: //depth
    Pdepth = value;
    fdepth = ((f32)Pdepth) / 127.0f;
    return;
  case 9: //lrcross
    Plrcross = value;
    flrcross = ((f32)(Plrcross - 64)) / 64.0f;
    fcross = 1.0f - fabsf(flrcross);
    return;
  }
  ASSERT(false);
}

i32 Vibe::getpar(i32 npar)
{
  switch (npar)
  {
  case 0:
    return Pwidth;
  case 1:
    return lfo.Pfreq;
  case 2:
    return lfo.Prandomness;
  case 3:
    return lfo.PLFOtype;
  case 4:
    return lfo.Pstereo;
  case 5:
    return Ppanning; //pan
  case 6:
    return Pvolume;
  case 7:
    return Pfb;
  case 8:
    return Pdepth;
  case 9:
    return Plrcross;
  }
  ASSERT(false);
  return 0;
}

#endif // SHR3D_SFX_CORE_RAKARRACK
