#include "formanter.h"
#include "twopole.h"
#include "math.h"

#define FORMANTS 3

struct formanter {
  struct twopole formant[3];
  float samplerate;
};

static double widthtoradius(double width) {
  return exp(-width); // ugh, a guess
};

static void init(struct formanter* f, float samplerate, float freq[129]) {
  f->samplerate=samplerate;
  double center[FORMANTS] = { 592, 1766, 3500 };
  double width[FORMANTS]  = { 400, 400, 400 };
  double twopi = 2*3.141592;
  double k = twopi/samplerate;
  for(int i=0;i<FORMANTS;i++) {
    twopole_init(&f->formant[i],k*center[i],widthtoradius(k*width[i]));
  }
}

static void finalize(struct formanter* f) {
}

static void process(struct formanter* f, int length, float const* restrict const* in, float * restrict const* out) {
  twopole_process(&f->formant[0],length,in[0],in[1],out[0],out[1]);
  for(int i=1;i<FORMANTS;i++) {
    twopole_processadding(&f->formant[i],length,1.0,in[0],in[1],out[0],out[1]);
  }
}

struct synthdesc formanterdesc = {
  .size=sizeof(struct formanter),
  .commands=NULL,
  .params=NULL,
  .init=&init,
  .finalize=&finalize,
  .process=&process,
  .keydown=NULL,
  .keyup=NULL,
  .retune=NULL,
};