#include "plucksynth.h"
#include "macros.h"

#include <strings.h> // bzero
#include <stdlib.h>
#include <stdint.h>
//#include "twopole.h"

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

struct plucksynthvoice {
  double phase0;
  double phase1;
  double cutoff0;
  double cutoff1;
  double state00;
  double state01;
  double state10;
  double state11;
  double drift;
  double gate;
  double phaseinc;
  double invphaseinc;
  double amp;
  double smoothedamp;
  int active;
};

struct plucksynth {
  struct plucksynthvoice voice[128];
  double driftdepth;
  double ampattack;
  double ampdecay;
  double amprelease;
  double whitenoiseamp; // depends on samplerate
  double noiselowpasscoeff;
  double samplerate;
  uint32_t rng_state;
  double bend;
  double invbend;
  double cutoffscaling;
  double kgain;
  int waveform;
  double decayhfdamping0;
  double decayhfdamping1;
  double releasehfdamping;
  double reso0;
  double reso1;
};

void plucksynth_init(struct plucksynth* restrict s, float samplerate, float* restrict freq);
void plucksynth_finalize(struct plucksynth* restrict s);
void plucksynth_keydown(struct plucksynth* restrict s, int key, float velocity);
void plucksynth_keyup(struct plucksynth* restrict s, int key);
float lerp(float v0, float v1, float x);
float eerp(float v0, float v1, float x);
void plucksynth_vol(struct plucksynth* restrict s, float vol);
void plucksynth_mod(struct plucksynth* restrict s, float mod);
void plucksynth_pitchbend(struct plucksynth* restrict s, float bend);
void plucksynth_attack(struct plucksynth* restrict s, float seconds);
void plucksynth_release(struct plucksynth* restrict s, float seconds);

struct command plucksynth_commands[];

void plucksynth_retune(struct plucksynth* restrict s, float freq[128]) {
  double samplerate = s->samplerate;
  for(int i=0;i<128;i++) {
    struct plucksynthvoice* restrict v = &s->voice[i];
    v->phaseinc=freq[i]/samplerate;
    v->invphaseinc = 440.0/44100/v->phaseinc;
  }
}

void plucksynth_init(struct plucksynth* restrict s, float samplerate, float freq[128]) {
  for(int i=0;i<128;i++) {
    struct plucksynthvoice* restrict v = &s->voice[i];
    v->gate=0;
    v->phaseinc=0;
    v->invphaseinc=0;
    // phaseinhc and invphaseinc set by retune call below.
    v->phase0=rand()*(1.0/(RAND_MAX+1.0));
    v->phase1=rand()*(1.0/(RAND_MAX+1.0));
    v->drift=0;
    v->cutoff0 = 2000;
    v->cutoff1 = 2000;
    v->state00 = 0;
    v->state01 = 0;
    v->state10 = 0;
    v->state11 = 0;
    v->amp=1.0e-5;
    v->active = 0;
  }
  s->samplerate = samplerate;
  plucksynth_retune(s,freq);
  s->ampattack = 1000;
  s->ampdecay = 0.1;
  s->amprelease = 50;
  s->whitenoiseamp = sqrt(samplerate/44100);
  s->noiselowpasscoeff = 0.1*3.141592*2/samplerate;
  s->bend = 1.0;
  s->invbend = 1.0;
  s->cutoffscaling = 2*3.141592 / samplerate;
  s->kgain=3.141592*2;
  s->waveform = 1;
  s->decayhfdamping0=0.0009;
  s->decayhfdamping1=0.0003;
  s->releasehfdamping=0.020;
  s->reso0 = 0.0;
  s->reso1 = 0.0;
}

void plucksynth_process(struct plucksynth* restrict s, int length, float const * const restrict * restrict in, float * const restrict * restrict out) {
  float * restrict outleft = out[0];
  float * restrict outright = out[1];
  double whitenoiseamp = s->whitenoiseamp;
  double noiselowpasscoeff = s->noiselowpasscoeff;
  double driftdepth = s->driftdepth;
  double drift_ingain = whitenoiseamp * noiselowpasscoeff * (1.0/32768.0/65536.0);
  double drift_fbgain = 1-noiselowpasscoeff;
  double bend = s->bend;
  double invbend = s->invbend;
  int rng_state = s->rng_state;
  double cutoffscaling = s->cutoffscaling;
  double kgain = s->kgain;
  int waveform = s->waveform;
  double decayhfdampingcoeff0 = s->decayhfdamping0 / s->samplerate;
  double decayhfdampingcoeff1 = s->decayhfdamping1 / s->samplerate;
  double releasehfdampingcoeff = s->releasehfdamping / s->samplerate;
  double ampattackcoeff = 1-exp(-s->ampattack/s->samplerate-0.0000001);
  double ampdecaycoeff = 1-exp(-s->ampdecay/s->samplerate-0.0000001);
  double ampreleasecoeff = exp(-s->amprelease/s->samplerate-0.0000001);
  double reso0 = s->reso0;
  double reso1 = s->reso1;
  for(int i=0;i<length;i++) {
    outleft[i]=1.0e-5;
    outright[i]=1.0e-5;
  }
  
  for(int i=0;i<128;i++) {
    struct plucksynthvoice* restrict v = &s->voice[i];
    if(v->active) {
      for(int j=0;j<length;j++) {
        double oscsL = 1.0e-5;
        double oscsR = 1.0e-5;
        float k = kgain;
        if (k < 2.0)
          k = 2.0;
        double dc = 1.0/k;
        double g = sqrt(dc)*0.25;
        double phaseinc = v->phaseinc * bend;
        double const sqrtinvphaseinc = sqrt(v->invphaseinc * invbend);
        
        double phase0 = v->phase0;
        double phase1 = v->phase1;
        double drift = v->drift;
        drift = (int)rng_state * drift_ingain + drift * drift_fbgain;
        rng_state = (rng_state * 196314165u) + 907633515u;

        double phaseinc0 = phaseinc*(0.999+drift*driftdepth);
        double phaseinc1 = phaseinc*(1.001+drift*driftdepth);

        double p = 1.0/2.0/3.141592;
 
        double osc0before = phase0 < p ? phase0/p : (1-phase0)/(1-p);
        double osc1before = phase1 < p ? phase1/p : (1-phase1)/(1-p);

        phase0 += phaseinc0;
        phase1 += phaseinc1;

        v->drift = drift;
        if (phase0 >= 1.0) phase0 -= 1;
        v->phase0 = phase0;
        if (phase1 >= 1.0) phase1 -= 1;
        v->phase1 = phase1;

        double osc0after = phase0 < p ? phase0/p : (1-phase0)/(1-p);
        double osc1after = phase1 < p ? phase1/p : (1-phase1)/(1-p);
        
        double osc0=0;
        double osc1=0;
        switch(waveform) {
        case 0:
          osc0 = (osc0after-0.5)*sqrtinvphaseinc;
          osc1 = (osc1after-0.5)*sqrtinvphaseinc;
          break;
        case 1:
          osc0 = (osc0after-osc0before)/phaseinc0*0.25;
          osc1 = (osc1after-osc1before)/phaseinc1*0.25;
          break;
        };
        
        double cutoff0 = v->cutoff0;
        double cutoffgain0 = 1-cutoff0 *
          (v->gate > 0.0001 ? decayhfdampingcoeff0 : releasehfdampingcoeff);
        if (cutoffgain0 < 0.8) {
          cutoffgain0=0.8;
        }
        cutoff0 *= cutoffgain0;
        v->cutoff0 = cutoff0;
        double c0 = cutoff0 * cutoffscaling;
        
        
        double cutoff1 = v->cutoff1;
        double cutoffgain1 = 1-cutoff1 *
          (v->gate > 0.0001 ? decayhfdampingcoeff1 : releasehfdampingcoeff);
        if (cutoffgain1 < 0.8) {
          cutoffgain1=0.8;
        }
        cutoff1 *= cutoffgain1;
        v->cutoff1 = cutoff1;
        double c1 = cutoff1 * cutoffscaling;
        
        if(c0 > 0.9)
          c0 = 0.9;
        double r0= reso0*(2.0-c0);
        double s00 = v->state00;
        double s01 = v->state01;
        double feed00 = osc0 - s01*r0;
        double feed01 = s00  + s01*r0;
        s00 = s00 + c0*(feed00-s00);
        s01 = s01 + c0*(feed01-s01);
        v->state00 = s00;
        v->state01 = s01;
        
        
        if(c1 > 0.9)
          c1 = 0.9;
        double r1= reso1*(2.0-c1);
        double s10 = v->state10;
        double s11 = v->state11;
        double feed10 = osc1 - s11*r1; // !!!! should be osc1
        double feed11 = s10  + s11*r1;
        s10 = s10 + c1*(feed10-s10);
        s11 = s11 + c1*(feed11-s11);
        v->state10 = s10;
        v->state11 = s11;
        
        double osc = s01+s11;
        
        oscsL += osc*g;
        oscsR += osc*g;
        double smoothedamp = v->smoothedamp;
        double smoothedampdiff = (v->gate+1.0e-6-smoothedamp);
        v->gate *= 1-ampdecaycoeff;
        double env = smoothedamp+=smoothedampdiff*(1-ampattackcoeff);
        v->smoothedamp = smoothedamp;
        outleft[j] += env * oscsL;
        outright[j] += env * oscsR;
        if (v->gate < 1.0e-4 && smoothedamp < 1.0e-4) {
          v->active = 0;
          break;
        }
      }
    }
    s->rng_state = rng_state;
  }
}
void plucksynth_finalize(struct plucksynth* restrict s) {
  bzero(s,sizeof(*s));
}

void plucksynth_keydown(struct plucksynth* restrict s, int key, float velocity) {
  s->voice[key].gate=sqrt(velocity);
  s->voice[key].active=1;
  s->voice[key].cutoff0=12000*velocity;
  s->voice[key].cutoff1=12000*velocity;
  //  s->voice[key].phase0=0;
  //  s->voice[key].phase1=0;
};

void plucksynth_keyup(struct plucksynth* restrict s, int key) {
  s->voice[key].gate=0;
};

float lerp(float v0, float v1, float x) {
  return v0*(1-x)+v1*x;
}

float eerp(float v0, float v1, float x) {
  return v0*pow(v1/v0,x);
}

void plucksynth_vol(struct plucksynth* restrict s, float vol) {
  s->kgain=2*vol;
}

void plucksynth_mod(struct plucksynth* restrict s, float mod) {
  s->driftdepth = mod*2.0;
}
void plucksynth_pitchbend(struct plucksynth* restrict s, float bend) {
  s->bend = pow(9.0/8.0,bend);
  s->invbend = 1.0/s->bend;
}

void plucksynth_attack(struct plucksynth* restrict s, float seconds) {
  s->ampattack = seconds;
}
void plucksynth_release(struct plucksynth* restrict s, float seconds) {
  s->amprelease = seconds;
}

void plucksynth_decayhfdamping0(struct plucksynth* restrict s, float decayhfdamping) { s->decayhfdamping0=decayhfdamping; }
void plucksynth_decayhfdamping1(struct plucksynth* restrict s, float decayhfdamping) { s->decayhfdamping1=decayhfdamping; }
void plucksynth_releasehfdamping(struct plucksynth* restrict s, float releasehfdamping) { s->releasehfdamping=releasehfdamping; }
void plucksynth_reso0(struct plucksynth* restrict s, float reso0) { s->reso0 = reso0; }
void plucksynth_reso1(struct plucksynth* restrict s, float reso1) { s->reso1 = reso1; }

int plucksynth_cmd_values(void* actiondata, void* v, char* line) {
  CAST(struct plucksynth*,s,v);
  printf("attack: %lf (s)\n",s->ampattack);
  printf("release: %lf (s)\n",s->amprelease);
  printf("decayhfdamping0: %lf\n",s->decayhfdamping0);
  printf("decayhfdamping1: %lf\n",s->decayhfdamping1);
  printf("releasehfdamping: %lf\n",s->releasehfdamping);
  printf("reso0: %lf\n",s->reso0);
  printf("reso1: %lf\n",s->reso1);
  return 0;
}

struct command plucksynth_commands[] = {
  { "attack", &cmd_callwithfloat, &plucksynth_attack, "set attack time (s)" },
  { "release", &cmd_callwithfloat, &plucksynth_release, "set release time (s)" },
  { "decayhfdamping0", &cmd_callwithfloat, &plucksynth_decayhfdamping0, "set decay high freq damping during of partial 0 (s)" },
  { "decayhfdamping1", &cmd_callwithfloat, &plucksynth_decayhfdamping1, "set decay high freq damping during of partial 1 (s)" },
  { "releasehfdamping", &cmd_callwithfloat, &plucksynth_releasehfdamping, "set release high freq damping during(s)" },
  { "reso0", &cmd_callwithfloat, &plucksynth_reso0, "set resonance on filter 0 (-0.25..1.0)" },
  { "reso1", &cmd_callwithfloat, &plucksynth_reso1, "set resonance on filter 1 (-0.25..1.0)" },
  { "values", &plucksynth_cmd_values, NULL, "show values of all parameters" },
  { "help", &cmd_help, plucksynth_commands, "show availible commands"},
  { "exit", &cmd_exit, NULL, "leave plucksynth"},
  { NULL, NULL, NULL }
};

struct synthdesc plucksynthdesc = {
  .size = sizeof(struct plucksynth),
  .init = &plucksynth_init,
  .finalize = &plucksynth_finalize,
  .process = &plucksynth_process,
  .keydown = &plucksynth_keydown,
  .keyup = &plucksynth_keyup,
  .commands = plucksynth_commands,
  .retune = &plucksynth_retune,
};