#include "resobass.h"
#include "ms20filter.h"
#include "moogfilter.h"
#include <math.h>

struct synth {
  struct ms20filter filter;
  //struct moogfilter filter;
  double hpstate1;
  double hpstate2;
  double hpcoeff;
  double phase;
  double phaseinc;
  double samplerate;
  double gate;
  double ampenv;
  double cutoffenv;
  int lastkey;
  float freqtable[128];
  double mod;
  double reso;
};

static void init(struct synth* restrict s, float samplerate, float freq[128]) {
  s->phase = 0;
  ms20filter_init(&s->filter);
  //moogfilter_init(&s->filter);
  s->hpstate1 = 0;
  s->hpstate2 = 0;
  s->hpcoeff = 10.0*2*3.141592/samplerate;
  s->samplerate = samplerate;
  s->phaseinc = 440.0/samplerate;
  s->gate = 0;
  s->ampenv=0;
  s->cutoffenv=0;
  s->lastkey = -1;
  for(int i=0;i<128;i++)
    s->freqtable[i]=freq[i];
  s->mod = 0.5;
  s->reso = 0.8;
}

static void retune(struct synth* restrict s, float freq[128]) {
  for(int i=0;i<128;i++)
    s->freqtable[i]=freq[i];
}

static void finalize(struct synth* restrict s) {
}
static void process(struct synth* restrict s, int length, float const* restrict const* in, float * restrict const* out) {
  float* restrict outL = out[0];
  float* restrict outR = out[1];
  double phase = s->phase;
  double phaseinc = s->phaseinc;
  double hpstate1 = s->hpstate1;
  double hpstate2 = s->hpstate2;
  double hpcoeff = s->hpcoeff;
  double gate = s->gate;
  for(int i=0;i<length;i++) {
    double v1 = phase*phase;
    phase += phaseinc;
    if (phase >= 0.5)
      phase -= 1.0;
    double v2 = phase*phase;
    double osc = (v2-v1)*(1/phaseinc);
    osc -= hpstate1; hpstate1 += osc*hpcoeff;
    double cutoff = s->cutoffenv * (s->mod*12000 * 2 * 3.141592 / s->samplerate);
    if (gate > 0) {
      s->cutoffenv -= 4*s->cutoffenv*s->cutoffenv/s->samplerate;
    }
    else {
      s->cutoffenv -= 16*s->cutoffenv*s->cutoffenv/s->samplerate;
    }
    double sound = ms20filter_tick(&s->filter, osc, 0, cutoff, s->reso);
    //double sound = moogfilter_tick(&s->filter, osc, cutoff, s->reso);
    sound += 1.0;
    sound *= 1.0;
    sound *= 1/sqrt(1+sound*sound);
    sound -= hpstate2; hpstate2 += sound*hpcoeff;
    double g = s->ampenv * 0.25;
    if (gate > 0) {
      s->ampenv *= 1 - 1/s->samplerate;
    }
    else {
      s->ampenv *= 1 - 4/s->samplerate;
    }
    sound *= g;
    outL[i] = sound;
    outR[i] = sound;
  }
  s->phase = phase;
  s->hpstate1 = hpstate1;
  s->hpstate2 = hpstate2;
}
static void keydown(struct synth* restrict s, int key, float velocity) {
  s->phaseinc = s->freqtable[key]/s->samplerate;
  s->ampenv = sqrt(s->ampenv*s->ampenv+velocity*velocity);
  //s->cutoffenv = sqrt(s->cutoffenv*s->cutoffenv+velocity*velocity);
  //s->ampenv += velocity;
  s->cutoffenv += velocity;
  s->gate = velocity;
  s->lastkey = key;
}
static void keyup(struct synth* restrict s, int key) {
  if (key == s->lastkey)
    s->gate = 0;
}
static void vol(struct synth* restrict s, float vol) {
  s->reso = vol*0.99;
}

static void mod(struct synth* restrict s, float mod) {
  s->mod = mod*mod;
}
static void pitchbend(struct synth* restrict s, float cents) {
}
static struct command commands[] = {
  { }
};

static struct paramdesc params[] = {
  { }
};

struct synthdesc resobassdesc = {
  .size=sizeof(struct synth),
  .commands=&commands,
  .params=params,
  .init=&init,
  .finalize=&finalize,
  .process=&process,
  .keydown=&keydown,
  .keyup=&keyup,
  .pitchbend=&pitchbend,
  .mod=&mod,
  .vol=&vol,
  .retune=&retune,
};