/*

  HarmonicEnhancer.C  -  Class
  This file is based in the harmonic_gen_1220.c by Steve Harris
  Copyright (C) 2008-2009 Josep Andreu (Holborn)
  Author: Josep Andreu
  Based on Steve Harris LADSPA harmonic.

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

#ifdef SHR3D_SFX_CORE_RAKARRACK

#include <string.h>

HarmEnhancer::HarmEnhancer(f32* Rmag, f32 hfreq, f32 lfreq, f32 gain)
  : hpfl(3, hfreq, 1, 0)
  , hpfr(3, hfreq, 1, 0)
  , lpfl(2, lfreq, 1, 0)
  , lpfr(2, lfreq, 1, 0)
{
  set_vol(0, gain);
  realvol = gain;
  itm1l = 0.0f;
  itm1r = 0.0f;
  otm1l = 0.0f;
  otm1r = 0.0f;

  hpffreq = hfreq;
  lpffreq = lfreq;

  limiter.setpreset(4);
  calcula_mag(Rmag);
}

void HarmEnhancer::cleanup()
{
  lpfl.cleanup();
  hpfl.cleanup();
  lpfr.cleanup();
  hpfr.cleanup();
  limiter.cleanup();
}

void HarmEnhancer::set_vol(i32 mode, f32 gain)
{
  if (!mode)
    vol = gain;
  else
    vol = realvol + gain;

  vol *= 2.0f;
}

void HarmEnhancer::set_freqh(i32 mode, f32 freq)
{
  if (!mode)
  {
    hpffreq = freq;
    freq = 0.0;
  }

  hpfl.setfreq(hpffreq + freq);
  hpfr.setfreq(hpffreq + freq);
}

void HarmEnhancer::set_freql(i32 mode, f32 freq)
{
  if (!mode)
  {
    lpffreq = freq;
    freq = 0.0;
  }

  lpfl.setfreq(lpffreq + freq);
  lpfr.setfreq(lpffreq + freq);
}

/* Calculate Chebychev coefficents from partial magnitudes, adapted from
 * example in Num. Rec. */
void HarmEnhancer::chebpc(f32 c[], f32 d[])
{
  f32 dd[HARMONICS];

  for (i32 j = 0; j < HARMONICS; j++)
    d[j] = dd[j] = 0.0;

  d[0] = c[HARMONICS - 1];

  for (i32 j = HARMONICS - 2; j >= 1; j--)
  {
    for (i32 k = HARMONICS - j; k >= 1; k--)
    {
      const f32 sv = d[k];
      d[k] = 2.0f * d[k - 1] - dd[k];
      dd[k] = sv;
    }
    const f32 sv = d[0];
    d[0] = -dd[0] + c[j];
    dd[0] = sv;
  }

  for (i32 j = HARMONICS - 1; j >= 1; j--)
    d[j] = d[j - 1] - dd[j];

  d[0] = -dd[0] + 0.5f * c[0];
}

void HarmEnhancer::calcula_mag(f32* Rmag)
{
  f32 mag[HARMONICS] =
  {
    0.0f, Rmag[0], Rmag[1], Rmag[2], Rmag[3], Rmag[4], Rmag[5],
    Rmag[6], Rmag[7], Rmag[8], Rmag[9]
  };

  // Normalise magnitudes

  f32 mag_fix = 0.0f;
  for (i32 i = 0; i < 10; i++)
    mag_fix += fabs(Rmag[i]);

  if (mag_fix < 1.0f)
    mag_fix = 1.0f;
  else
    mag_fix = 1.0f / mag_fix;

  for (i32 i = 0; i < HARMONICS; i++)
    mag[i] *= mag_fix;

  // Calculate polynomial coefficients, using Chebychev aproximation
  chebpc(mag, p);
}

void HarmEnhancer::harm_out(f32** inOutBlock, const i32 blockSize)
{
  memcpy(inputl, inOutBlock[0], sizeof(f32) * blockSize);
  memcpy(inputr, inOutBlock[1], sizeof(f32) * blockSize);

  hpfl.filterout(inputl, blockSize);
  hpfr.filterout(inputr, blockSize);

  f32* input[2] = { inputl, inputr };
  limiter.processBlock(input, blockSize);

  for (i32 i = 0; i < blockSize; i++)
  {
    f32 xl = inputl[i];
    f32 xr = inputr[i];
    f32 yl = 0.0f;
    f32 yr = 0.0f;

    for (i32 j = 10; j > 0; j--)
    {
      yl = (yl + p[j]) * xl;
      yr = (yr + p[j]) * xr;
    }
    yl += p[0];
    yr += p[0];

    otm1l = 0.999f * otm1l + yl - itm1l;
    itm1l = yl;
    otm1r = 0.999f * otm1r + yr - itm1r;
    itm1r = yr;

    otm1l = yl;
    otm1r = yr;

    inputl[i] = otm1l;
    inputr[i] = otm1r;
  }

  lpfl.filterout(inputl, blockSize);
  lpfr.filterout(inputr, blockSize);

  for (i32 i = 0; i < blockSize; i++)
  {
    inOutBlock[0][i] = inOutBlock[0][i] + inputl[i] * vol;
    inOutBlock[1][i] = inOutBlock[1][i] + inputr[i] * vol;
  }
}

#endif // SHR3D_SFX_CORE_RAKARRACK
