/*

  Vocoder.C - Vocoder effect
  Author: Ryam Billing & Josep Andreu

  Adapted effect structure of ZynAddSubFX - a software synthesizer
  Author: Nasca Octavian Paul

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

#ifdef SHR3D_SFX_CORE_RAKARRACK

#ifdef SHR3D_SFX_CORE_RAKARRACK_LIBSAMPLERATE

#include "../common/Resample.h"

#include <string.h>

#define CLAMP(x, low, high)  (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))

Vocoder::Vocoder(f32* auxresampled_, i32 bands, i32 DS, i32 uq, i32 dq)
  : vlp(2, 4000.0f, 1.0f, 1)
  , vhp(3, 200.0f, 0.707f, 1)
{
  adjust(DS, blockSize());

  VOC_BANDS = bands;

  auxresampled = auxresampled_;
  //default values
  Pvolume = 50;
  Plevel = 0;
  Pinput = 0;
  Ppanning = 64;
  Plrcross = 100;

  filterbank = (fbank*)malloc(sizeof(fbank) * VOC_BANDS);
  tmpl = (f32*)malloc(sizeof(f32) * nPERIOD);
  tmpr = (f32*)malloc(sizeof(f32) * nPERIOD);
  tsmpsl = (f32*)malloc(sizeof(f32) * nPERIOD);
  tsmpsr = (f32*)malloc(sizeof(f32) * nPERIOD);
  tmpaux = (f32*)malloc(sizeof(f32) * nPERIOD);

  Pmuffle = 10;
  f32 tmp = 0.01f;  //10 ms decay time on peak detectors
  alpha = ncSAMPLE_RATE / (ncSAMPLE_RATE + tmp);
  beta = 1.0f - alpha;
  prls = beta;
  gate = 0.005f;

  tmp = 0.05f; //50 ms att/rel on compressor
  calpha = ncSAMPLE_RATE / (ncSAMPLE_RATE + tmp);
  cbeta = 1.0f - calpha;
  cthresh = 0.25f;
  cpthresh = cthresh; //dynamic threshold
  cratio = 0.25f;

  A_Resample = new Resample(dq);
  U_Resample = new Resample(dq);
  D_Resample = new Resample(uq);

  for (i32 i = 0; i < VOC_BANDS; i++)
  {
    const f32 center = (f32)i * 20000.0f / ((f32)VOC_BANDS);
    const f32 qq = 60.0f;

    filterbank[i].l = new AnalogFilter(4, center, qq, 0);
    filterbank[i].l->setSR(nSAMPLE_RATE);
    filterbank[i].r = new AnalogFilter(4, center, qq, 0);
    filterbank[i].r->setSR(nSAMPLE_RATE);
    filterbank[i].aux = new AnalogFilter(4, center, qq, 0);
    filterbank[i].aux->setSR(nSAMPLE_RATE);
  }

  vlp.setSR(nSAMPLE_RATE);
  vhp.setSR(nSAMPLE_RATE);

  setbands(VOC_BANDS, 200.0f, 4000.0f);
  setpreset(0);
}

void Vocoder::cleanup()
{
  for (i32 k = 0; k < VOC_BANDS; k++)
  {
    filterbank[k].l->cleanup();
    filterbank[k].r->cleanup();
    filterbank[k].aux->cleanup();
    filterbank[k].speak = 0.0f;
    filterbank[k].gain = 0.0f;
    filterbank[k].oldgain = 0.0f;
  }
  vhp.cleanup();
  vlp.cleanup();

  compeak = compg = compenv = oldcompenv = 0.0f;
}

void Vocoder::adjust(i32 DS, const i32 blockSize)
{
  DS_state = DS;

  switch (DS)
  {
  case 0:
    nPERIOD = blockSize;
    nSAMPLE_RATE = SAMPLE_RATE;
    nfSAMPLE_RATE = fSAMPLE_RATE;
    break;
  case 1:
    nPERIOD = lrintf(f32(blockSize) * 96000.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 96000;
    nfSAMPLE_RATE = 96000.0f;
    break;
  case 2:
    nPERIOD = lrintf(f32(blockSize) * 48000.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 48000;
    nfSAMPLE_RATE = 48000.0f;
    break;
  case 3:
    nPERIOD = lrintf(f32(blockSize) * 44100.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 44100;
    nfSAMPLE_RATE = 44100.0f;
    break;
  case 4:
    nPERIOD = lrintf(f32(blockSize) * 32000.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 32000;
    nfSAMPLE_RATE = 32000.0f;
    break;
  case 5:
    nPERIOD = lrintf(f32(blockSize) * 22050.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 22050;
    nfSAMPLE_RATE = 22050.0f;
    break;
  case 6:
    nPERIOD = lrintf(f32(blockSize) * 16000.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 16000;
    nfSAMPLE_RATE = 16000.0f;
    break;
  case 7:
    nPERIOD = lrintf(f32(blockSize) * 12000.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 12000;
    nfSAMPLE_RATE = 12000.0f;
    break;
  case 8:
    nPERIOD = lrintf(f32(blockSize) * 8000.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 8000;
    nfSAMPLE_RATE = 8000.0f;
    break;
  case 9:
    nPERIOD = lrintf(f32(blockSize) * 4000.0f / fSAMPLE_RATE);
    nSAMPLE_RATE = 4000;
    nfSAMPLE_RATE = 4000.0f;
    break;
  }

  ncSAMPLE_RATE = 1.0f / nfSAMPLE_RATE;

  u_up = (f64)nPERIOD / (f64)blockSize;
  u_down = (f64)blockSize / (f64)nPERIOD;
}

void Vocoder::processBlock(f32** inBlock, f32** outBlock, const i32 blockSize)
{
  f32 tempgain;
  f32 maxgain = 0.0f;
  f32 auxtemp, tmpgain;

  if (DS_state != 0)
    A_Resample->mono_out(auxresampled, tmpaux, blockSize, u_up, nPERIOD);
  else
    memcpy(tmpaux, auxresampled, sizeof(f32) * nPERIOD);

  for (i32 i = 0; i < nPERIOD; i++)    //apply compression to auxresampled
  {
    auxtemp = input * tmpaux[i];
    if (fabs(auxtemp > compeak)) compeak = fabs(auxtemp);   //First do peak detection on the signal
    compeak *= prls;
    compenv = cbeta * oldcompenv + calpha * compeak;       //Next average into envelope follower
    oldcompenv = compenv;

    if (compenv > cpthresh)                                //if envelope of signal exceeds thresh, then compress
    {
      compg = cpthresh + cpthresh * (compenv - cpthresh) / compenv;
      cpthresh = cthresh + cratio * (compg - cpthresh);   //cpthresh changes dynamically
      tmpgain = compg / compenv;
    }
    else
    {
      tmpgain = 1.0f;
    }

    if (compenv < cpthresh)
      cpthresh = compenv;
    if (cpthresh < cthresh)
      cpthresh = cthresh;

    tmpaux[i] = auxtemp * tmpgain;

    tmpaux[i] = vlp.filterout_s(tmpaux[i]);
    tmpaux[i] = vhp.filterout_s(tmpaux[i]);
  }

  //End compression

  auxtemp = 0.0f;

  if (DS_state != 0)
  {
    f32* tsmps[2] = { tsmpsl, tsmpsr };
    U_Resample->processBlock(inBlock, tsmps, blockSize, u_up);
  }
  else
  {
    memcpy(tsmpsl, inBlock[0], sizeof(f32) * nPERIOD);
    memcpy(tsmpsr, inBlock[1], sizeof(f32) * nPERIOD);
  }

  memset(tmpl, 0, sizeof(f32) * nPERIOD);
  memset(tmpr, 0, sizeof(f32) * nPERIOD);

  for (i32 j = 0; j < VOC_BANDS; j++)
  {
    for (i32 i = 0; i < nPERIOD; i++)
    {
      auxtemp = tmpaux[i];

      if (filterbank[j].speak < gate) filterbank[j].speak = 0.0f;  //gate 
      if (auxtemp > maxgain) maxgain = auxtemp; //vu meter level.    

      auxtemp = filterbank[j].aux->filterout_s(auxtemp);
      if (fabs(auxtemp) > filterbank[j].speak) filterbank[j].speak = fabs(auxtemp);  //Leaky Peak detector

      filterbank[j].speak *= prls;

      filterbank[j].gain = beta * filterbank[j].oldgain + alpha * filterbank[j].speak;
      filterbank[j].oldgain = filterbank[j].gain;


      tempgain = (1.0f - ringworm) * filterbank[j].oldgain + ringworm * auxtemp;

      tmpl[i] += filterbank[j].l->filterout_s(tsmpsl[i]) * tempgain;
      tmpr[i] += filterbank[j].r->filterout_s(tsmpsr[i]) * tempgain;
    }
  }

  for (i32 i = 0; i < nPERIOD; i++)
  {
    tmpl[i] *= lpanning * level;
    tmpr[i] *= rpanning * level;
  }

  if (DS_state != 0)
  {
    f32* temp[2] = { tmpl, tmpr };
    D_Resample->processBlock(temp, outBlock, nPERIOD, u_down);
  }
  else
  {
    memcpy(outBlock[0], tmpl, sizeof(f32) * nPERIOD);
    memcpy(outBlock[1], tmpr, sizeof(f32) * nPERIOD);
  }

  vulevel = (f32)CLAMP(rap2dB(maxgain), -48.0, 15.0);
}

void Vocoder::setbands(i32 numbands, f32 startfreq, f32 endfreq)
{
  const f32 start = startfreq; //useful variables
  const f32 endband = endfreq;
  const f32 fnumbands = (f32)numbands;
  f32 output[128 + 1];

  //calculate intermediate values
  const f32 pwer = logf(endband / start) / log(2.0f);

  for (i32 k = 0; k <= VOC_BANDS; k++)
    output[k] = start * powf(2.0f, ((f32)k) * pwer / fnumbands);

  for (i32 k = 0; k < VOC_BANDS; k++)
  {
    filterbank[k].sfreq = output[k] + (output[k + 1] - output[k]) * 0.5f;
    filterbank[k].sq = filterbank[k].sfreq / (output[k + 1] - output[k]);

    filterbank[k].l->setfreq_and_q(filterbank[k].sfreq, filterbank[k].sq);
    filterbank[k].r->setfreq_and_q(filterbank[k].sfreq, filterbank[k].sq);
    filterbank[k].aux->setfreq_and_q(filterbank[k].sfreq, filterbank[k].sq);
  }
  cleanup();
}

void Vocoder::setvolume(i32 Pvolume_)
{
  Pvolume = Pvolume_;
  outvolume = (f32)Pvolume_ / 127.0f;
  if (Pvolume_ == 0)
    cleanup();
}

void Vocoder::setpanning(i32 Ppanning_)
{
  Ppanning = Ppanning_;
  lpanning = ((f32)Ppanning_ + 0.5f) / 127.0f;
  rpanning = 1.0f - lpanning;
}

void Vocoder::init_filters()
{
  for (i32 ii = 0; ii < VOC_BANDS; ii++)
  {
    const f32 ff = filterbank[ii].sfreq;
    const f32 qq = filterbank[ii].sq;
    filterbank[ii].l->setfreq_and_q(ff, qq);
    filterbank[ii].r->setfreq_and_q(ff, qq);
    filterbank[ii].aux->setfreq_and_q(ff, qq);
  }
}

void Vocoder::adjustq(f32 q)
{
  for (i32 ii = 0; ii < VOC_BANDS; ii++)
  {
    filterbank[ii].l->setq(q);
    filterbank[ii].r->setq(q);
    filterbank[ii].aux->setq(q);
  }
}

void Vocoder::setpreset(i32 npreset)
{
  const i32 PRESET_SIZE = 7;
  i32 presets[][PRESET_SIZE] = {
    //Vocoder 1
    {0, 64, 10, 70, 70, 40, 0},
    //Vocoder 2
    {0, 64, 14, 80, 70, 40, 32},
    //Vocoder 3
    {0, 64, 20, 90, 70, 40, 64},
    //Vocoder 4
    {0, 64, 30, 100, 70, 40, 127}
  };

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

void Vocoder::changepar(i32 npar, i32 value)
{
  f32 tmp = 0;
  switch (npar)
  {
  case 0:
    setvolume(value);
    return;
  case 1:
    setpanning(value);
    return;
  case 2:
    Pmuffle = value;
    tmp = (f32)Pmuffle;
    tmp *= 0.0001f + tmp / 64000;
    alpha = ncSAMPLE_RATE / (ncSAMPLE_RATE + tmp);
    beta = 1.0f - alpha;
    return;
  case 3:
    Pqq = value;
    tmp = (f32)value;
    adjustq(tmp);
    return;
  case 4:
    Pinput = value;
    input = dB2rap(75.0f * (f32)Pinput / 127.0f - 40.0f);
    return;
  case 5:
    Plevel = value;
    level = dB2rap(60.0f * (f32)Plevel / 127.0f - 40.0f);
    return;
  case 6:
    Pring = value;
    ringworm = (f32)Pring / 127.0f;
    return;
  }
  ASSERT(false);
}

i32 Vocoder::getpar(i32 npar)
{
  switch (npar)
  {
  case 0:
    return Pvolume;
  case 1:
    return Ppanning;
  case 2:
    return Pmuffle;
  case 3:
    return Pqq;
  case 4:
    return Pinput;
  case 5:
    return Plevel;
  case 6:
    return Pring;
  }
  ASSERT(false);
  return 0;
}

#endif // SHR3D_SFX_CORE_RAKARRACK_LIBSAMPLERATE

#endif // SHR3D_SFX_CORE_RAKARRACK
