#include <stdint.h>
#include <stdio.h>
#include <math.h>

struct apf
{
  int32_t    length;    // delay line lenth;
  int32_t    wpos;      // position within ringbuffer
  float      k;         // allpass coefficient;
  float    * buffer;
};

static float g_k = 0;

void init_apf (int32_t l, float k, apf &filter)
///////////////////////////////////////////////
{
  filter.k=k;
  filter.wpos = 0;
  filter.length = l;
  filter.buffer = new float[l];

  for (int i=0; i<l; i++)
    filter.buffer[i]=0;
}

float apply_apf (float in, apf &filter)
///////////////////////////////////////
{
  float z    = filter.buffer[filter.wpos];
  float out  = z;

  in  -= z  * filter.k;
  out += in * filter.k;
  filter.buffer[filter.wpos] = in;

  if (filter.wpos++ == filter.length)
    filter.wpos = 0;

  return out;
}

float apply_predelay (float in, apf &filter)
////////////////////////////////////////////
{
  float z    = filter.buffer[filter.wpos];
  filter.buffer[filter.wpos] = in;

  if (filter.wpos++ == filter.length)
    filter.wpos = 0;

  return z;
}


static apf filters[8];
static apf prefltr[2];
static apf predelay;

void init (float scale)
////////////////
{
  // 8 cascaded allpass stages.
  init_apf (floor( 159.0*scale), g_k, filters[0]);
  init_apf (floor( 277.0*scale), g_k, filters[1]);
  init_apf (floor( 433.0*scale), g_k, filters[2]);
  init_apf (floor( 919.0*scale), g_k, filters[3]);
  init_apf (floor(1259.0*scale), g_k, filters[4]);
  init_apf (floor(1447.0*scale), g_k, filters[5]);
  init_apf (floor(1741.0*scale), g_k, filters[6]);
  init_apf (floor(2213.0*scale), g_k, filters[7]);

  // two pre-loop diffusors
  init_apf ( 617, g_k, prefltr[0]);
  init_apf ( 214, g_k, prefltr[1]);

  init_apf ( 1000, 0, predelay);

}

float g_prev = 0;
float kLowpass = 0;

float run (float in, float krf, float &l, float &r)
///////////////////////////////////////////////////
{
  float sec1, sec2, sec3, sec4;

  // two allpass filters for pre-diffusion:
  in = apply_apf (in, prefltr[0]);
//  in = apply_apf (in, prefltr[1]);

  // some hefty pre-delay.
  in = apply_predelay (in, predelay);

  // add in feedback:
  in += g_prev;

  // four pairs of apf in the loop.
  sec1 = apply_apf (in,       filters[0]);
  sec1 = apply_apf (sec1,     filters[1]);

  sec2 = apply_apf (sec1*krf, filters[2]);
  sec2 = apply_apf (sec2,     filters[3]);

  sec3 = apply_apf (sec2*krf, filters[4]);
  sec3 = apply_apf (sec3,     filters[5]);

  sec4 = apply_apf (sec3*krf, filters[6]);
  sec4 = apply_apf (sec4,     filters[7]);

  // simple output tapping:
  l = sec1 *0.5 + sec2 * 0.1 + sec3 * 0.3 + sec4 * 0.1;
  r = sec2 *0.2 + sec2 * 0.4 + sec3 * 0.1 + sec4 * 0.3;

  l = sec2;
  r = sec3;

  // apply lowpass with cheap lpf
  float lpFiltered = ((sec4 * krf)-g_prev) * kLowpass + g_prev;

  g_prev = lpFiltered;
}

void setlowpass (float f)
{
  kLowpass = exp(-2.0f*M_PI*f/44100.0f);
}


int main (int argc, char **args)
/////////////////////////////////
{
  g_k = 0.8;

  init(3.5f);
  setlowpass (18000);


  FILE *f = fopen ("guitar.pcm","rb");
  FILE *o = fopen ("output.pcm","wb");

  fseek (f, 0, SEEK_END);
  long l = ftell(f);
  fseek (f, 0, SEEK_SET);

  for (int i=0; i<l/2; i++)
  {
    short sample_in;
    float l,r;
    fread (&sample_in, 2,1,f);
    float in = (float)sample_in / 32768.0f;
    run (in, 0.96f, l, r);

    fwrite (&l, 4,1,o);
    fwrite (&r, 4,1,o);
  }

  for (int i=0; i<44100*8; i++)
  {
    float l,r;
    run (0, 0.96f, l, r);
    fwrite (&r, 4,1,o);
    fwrite (&l, 4,1,o);
  }
  fclose (f);
  fclose (o);

  return 0;
}