#include "synth_engine.h"
#include "synth_math.h"
#include "lowpass.h"
#include "filter.h"
#include "control.h"
#include "sound.h"
#include "wavetable.h"
#include "osc.h"

#include <string.h>

float
sin_sample(float amp, float freq, unsigned long long phase, unsigned int sample_rate)
{
  return amp * sinf(2.0 * M_PI * freq * ((float)phase / (float)sample_rate));
}

float
saw_sample(float amp, float freq, unsigned long long phase, unsigned int sample_rate)
{
  return amp * (0.17 * (1.0 - (2.0 * M_PI * freq * fmod((float)phase, (float)(sample_rate / (freq)))) / (float)sample_rate));
}

float
sawX_sample(float amp, float freq, float sm, unsigned long long phase, unsigned int sample_rate)
{
  float dOutput = 0.0;
  for (float n = 1.0; n < sm; n++)
    dOutput += (sinf(n * 2.0 * M_PI * freq * ((float)phase / (float)sample_rate))) / n;
  return 0.5 * amp * dOutput;
}

float
sqr_sample(float amp, float freq, float duty_cycle, unsigned long long phase, unsigned int sample_rate)
{
  if (duty_cycle < 0.0001 || duty_cycle > 0.9999) {
    duty_cycle = 0.5;
  }

  return (fmod((float)phase / (float)sample_rate, 1.0 / freq) < duty_cycle * (1.0 / freq)) ? amp : -amp;
}

float
gen10(float f, unsigned long long phase, float x, unsigned int sample_rate)
{
  return fmodf(phase, sample_rate/f) / (sample_rate/f);
}

float
gen110(float f, unsigned long long phase, float x, unsigned int sample_rate)
{
  float a = fmodf(phase, sample_rate/f);
  return a > (sample_rate/f) / 2.0f ? 0.9999f : -0.9999f;
}

float
gen0(float f, midi_note_t * midi_note, float x, unsigned int sample_rate)
{
  return sin_sample(1, f, midi_note->elapsed, sample_rate);
  //return chirp(midi_note->elapsed, f);
  /* return sqr_sample(0.1, f,  0.3,             midi_note->elapsed, sample_rate) */
  /*        + sqr_sample(0.1, f * 3.0 / 2.0 , 0.5, midi_note->elapsed, sample_rate) */
  /*        + saw_sample(0.3, f,                   midi_note->elapsed, sample_rate) */
  /*        + sin_sample(0.1, f,                   midi_note->elapsed, sample_rate) */
  /*        + sin_sample(0.1, f * 5,               midi_note->elapsed, sample_rate) */
  /*        /\* + sin_sample(0.1, freq * 50 * 1021,       midi_note->elapsed, sample_rate) *\/ */
  /*        /\* + sin_sample(0.1, freq * 50 * 3531021,    midi_note->elapsed, sample_rate) *\/ */
  /*        + sin_sample(0.1, f * 7,               midi_note->elapsed, sample_rate); */
}

float
gen1(float f, midi_note_t * midi_note, float x, unsigned int sample_rate)
{
  //return saw_sample(1, f, midi_note->elapsed, sample_rate);
  return sawX_sample(0.5, f, 5, midi_note->elapsed, sample_rate)
         + saw_sample(0.3, 2 * f / 5, midi_note->elapsed, sample_rate)
         + sin_sample(0.2, f * 5.0 / 7.0 , midi_note->elapsed, sample_rate);
}

float
gen2(float f, midi_note_t * midi_note, float x, unsigned int sample_rate)
{
  /* return sin_sample(0.5, f * sqrt(2) , midi_note->elapsed, sample_rate) */
  /*   + sin_sample(0.5, f, midi_note->elapsed, sample_rate); */

  return sawX_sample(1, f, 15, midi_note->elapsed, sample_rate);
}

float
gen3(float f, midi_note_t * midi_note, float x, unsigned int sample_rate)
{
  //return sqr_sample(1, f, .5, midi_note->elapsed, sample_rate);

  //return wvt_next(wvt_sound, f, sample_rate, &midi_note->wvt_index);
  float sample = osc_sound(midi_note->wvt_index);
  midi_note->wvt_index = osc_sound_next(f, midi_note->wvt_index);
  return sample;
  
  /* return sawX_sample(0.7, f, 5, midi_note->elapsed, sample_rate) */
  /*   + sin_sample(0.3, 4.0/17.0*f, midi_note->elapsed, sample_rate); */
}

float
gen4(float f, midi_note_t * midi_note, float x, unsigned int sample_rate)
{
  float sample = osc_tri(midi_note->wvt_index);
  midi_note->wvt_index = osc_tri_next(f, midi_note->wvt_index);
  return sample;
  //return wvt_next(wvt_tri, f, sample_rate, &midi_note->wvt_index);
}

float
gen5(float f, midi_note_t * midi_note, float x, unsigned int sample_rate)
{
  return 0.8 * wvt_next(wvt_saw, f, sample_rate, &midi_note->wvt_index) +
         .2 * sin_sample(1, f/2, midi_note->elapsed, sample_rate);
}

float
gen6(float f, midi_note_t * midi_note, float x, unsigned int sample_rate)
{
  return wvt_next(wvt_sqr, f, sample_rate, &midi_note->wvt_index);
}

float
lfo(float f, unsigned long long phase, unsigned int sample_rate) {
  return sin_sample(1, f, phase, sample_rate);
}

int
notes_active(synth_t *synth)
{
  int flag = 0;
  
  for (int i = 0; i < MIDI_NOTES; i++) {
    if (!synth->midi_note[i].active)
      continue;

    if (!fix_adsr(&synth->adsr,
                  (float)synth->midi_note[i].noteOn,
                  (float)synth->midi_note[i].noteOff,
                  synth->midi_note[i].elapsed,
                  synth->midi_note[i].noteOffSample)
        && synth->midi_note[i].noteOff != 0) {
      synth->midi_note[i].freq = 0;
      synth->midi_note[i].channel = -1;
      synth->midi_note[i].noteOn = -1;
      synth->midi_note[i].noteOff = -1;
      synth->midi_note[i].wvt_index = 0;
      synth->midi_note[i].lfo_index = 0;
      synth->midi_note[i].velocity = -1;
      synth->midi_note[i].elapsed = -1;
      synth->midi_note[i].noteOffSample = 0;
      synth->midi_note[i].active = 0;
    } else {
      flag++;
    }
  }

  return flag;
}

float lfo_index;

float
make_sample(void *synthData, unsigned int sample_rate, int frame)
{
  synth_t *synth = (synth_t*)synthData;
  float sample = 0.0f;

  float n = notes_active(synth);
  if (n == 0) return sample;

  float rms = 0;
  for (int i = 0; i < MIDI_NOTES; i++) {
    if (!synth->midi_note[i].active)
      continue;

    rms += synth->midi_note[i].velocity * synth->midi_note[i].velocity;
  }

  // if rms == 0 we can deduce there are no notes however notes_active handles
  // stopping any already playing notes and if we remove it we need to make
  // sure that notes stop some other way (it laso happens every frame)
  rms = sqrt(rms / n);

  for (int i = 0; i < MIDI_NOTES; i++) {
    if (!synth->midi_note[i].active)
      continue;

    float adsr = fix_adsr(&synth->adsr,
                                synth->midi_note[i].noteOn,
                                synth->midi_note[i].noteOff,
                          synth->midi_note[i].elapsed,
                  synth->midi_note[i].noteOffSample);

    float targ_freq = synth->midi_note[i].freq * cc_iget(&synth->cc_pitch, frame, FRAMES_PER_BUFFER);
    targ_freq = targ_freq + targ_freq *
      cc_iget(&synth->cc_lfo_amp, frame, FRAMES_PER_BUFFER) *
      // (1.05946309436/24.0) *
      (wvt_next(wvt_tri, (wvt_next(wvt_sound, 2 * cc_iget(&synth->cc_lfo_freq, frame, FRAMES_PER_BUFFER), SAMPLE_RATE, &lfo_index) / 2.0 + 0.5) * cc_iget(&synth->cc_lfo_freq, frame, FRAMES_PER_BUFFER), SAMPLE_RATE, &synth->midi_note[i].lfo_index) / 2.0 + 0.5);
    
    float synth_sample = synth->osctri->ops->sample(synth->osctri, synth->midi_note[i].wvt_index);
    synth->midi_note[i].wvt_index = synth->osctri->ops->next(synth->osctri, targ_freq, synth->midi_note[i].wvt_index);
    /* float synth_sample = synth->gen[synth->geni](targ_freq, */
    /*                                              &synth->midi_note[i], */
    /*                                              synth->x, */
    /*                                              sample_rate); */

    sample += 0.2 * rms * adsr * synth_sample;
  }

  /* filter */
  if (synth->filter) {
    // ALLL THE FILTERS
    float cutoff = cc_iget(&synth->cc_cutoff, frame, FRAMES_PER_BUFFER);
    float reso = round(cc_iget(&synth->cc_resonance, frame, FRAMES_PER_BUFFER));

    //cutoff = cutoff + cutoff * (synth->lfo.amp) * lfo(cc_iget(&synth->cc_lfo_freq, frame, FRAMES_PER_BUFFER), synth->lfo.elapsed, sample_rate);
    if (cutoff == 0) cutoff = 0.001;
    LowPass_Update(reso, cutoff, sample_rate);
    sample = LowPass_Filter(sample);

    update_bw_low_pass_filter(synth->fff, SAMPLE_RATE,cutoff, reso);
    sample = bw_low_pass(synth->fff, sample);
  }

  sample = synth->gain * sample;
    
  // band stop for high freqs
  sample = bw_band_stop(synth->fff2, sample);
  
  if (synth->clamp) sample = clamp(sample, -1, 1);
  
  return sample;
}

void
add_to_viz(synth_t *synth, float sample)
{
  synth->viz.wave[synth->viz.wi++] = sample;

  if (synth->viz.wi >= VIZ_BUF) {
    synth->viz.wi = 0;
  }
}

void
increment_synth(synth_t *synth)
{
  synth->lfo.elapsed++;
  synth->adsr.elapsed++;
  for (int i = 0; i < MIDI_NOTES; i++) {
    if (!synth->midi_note[i].active)
      continue;

    synth->midi_note[i].elapsed++;
  }
}

void
get_frame(void *outputBuffer, synth_t *synth, int i)
{
  float *out = (float*)outputBuffer + 2 * i;
  float s = 0.0f;

  
  if (!synth->active) {
    *out++ = 0.0f;
    *out++ = 0.0f;

    add_to_viz(synth, 0.0f);
    lfo_index = 0;
    return;
  }

  if (!notes_active(synth)) {
    synth->active = 0;
    *out++ = 0.0f;
    *out++ = 0.0f;
    add_to_viz(synth, 0.0f);
    lfo_index = 0;
    return;
  }

  s = make_sample(synth, SAMPLE_RATE, i);
  *out++ = s;
  *out++ = s;

  // move time
  increment_synth(synth);

  // viz
  add_to_viz(synth, s);
}

#include "fftw3.h"

int
process(float * buffer)
{
  int len = FRAMES_PER_BUFFER;

  float buf[FRAMES_PER_BUFFER];

  for( unsigned long i=0; i < len * 2; i += 2 ) {
    buf[i / 2] = buffer[i];
  }  
  
  fftwf_complex* frequency_signal = (fftwf_complex*)fftwf_malloc(sizeof(fftwf_complex) * (len / 2 + 1));
  //double* time_signal = (double*)fftwf_malloc(sizeof(double) * len);
    
  fftwf_plan forward_plan = fftwf_plan_dft_r2c_1d(len, buf, frequency_signal, FFTW_ESTIMATE);
  fftwf_plan backward_plan = fftwf_plan_dft_c2r_1d(len, frequency_signal, buf, FFTW_ESTIMATE);

  fftwf_execute(forward_plan);

  /* for (int i=0; i < (len / 2 + 1); i++ ) { */
  /*   if (frequency_signal[i][0] > 1) { */
  /*     frequency_signal[i][0] = 0; */
  /*   } */
  /*   if (frequency_signal[i][1] > 1) { */
  /*     frequency_signal[i][1] = 0; */
  /*   } */
  /*      printf("%f %f | ", sqrt(frequency_signal[i][0] * frequency_signal[i][0] + frequency_signal[i][1] * frequency_signal[i][1]), atan2(frequency_signal[i][1], frequency_signal[i][0])); */

  /*   float f = atan2(frequency_signal[i][1], frequency_signal[i][0]); */
  /*   if (f > M_PI / 2 && f < 2 * M_PI / 3) { */
  /*     frequency_signal[i][0] = 0.0; */
  /*     frequency_signal[i][1] = 0.0; */
  /*   } */
  /* } */

  fftwf_execute(backward_plan);

  for (unsigned long i=0; i < len * 2; i += 2) {
    buffer[i] = buf[i / 2] / len ;
    buffer[i + 1] = buf[i / 2] / len ;
  }

  fftwf_destroy_plan(forward_plan);
  fftwf_destroy_plan(backward_plan);
  fftwf_free(frequency_signal);
  //fftwf_free(time_signal);

  /* for( unsigned long i=0; i < len * 2; i += 2 ) { */
  /*   buf[i / 2] = buffer[i]; */
  /* } */
  /* float kernel[7] = {.01, .88, .21, .36, .21, .13, .01}; */
  /* float res[len + 7 - 1]; */
  /* convole(buf, kernel, len, 7, res); */

  /* int N = (len + 7 - 1); */
  /* int M = len; */
  
  /* float ratio = (float) N / M; */

  /* for (int i = 0; i < M; i++) { */
  /*   int index = (int)(i * ratio); */
  /*   buf[i] = res[index]; */
  /* } */
  
  /* for (unsigned long i=0; i < len * 2; i += 2) { */
  /*   buffer[i] = buf[i / 2] ; */
  /*   buffer[i + 1] = buf[i / 2] ; */
  /* } */
  
  return 0;
}

int
sound_gen(const void *inputBuffer, void *outputBuffer,
          unsigned long framesPerBuffer,
          const PaStreamCallbackTimeInfo* timeInfo,
          PaStreamCallbackFlags statusFlags,
          void *synthData)
{
  synth_t *synth = (synth_t*)synthData;
  float *out = (float*)outputBuffer;
  
  float buffer[2 * FRAMES_PER_BUFFER];

  (void) timeInfo;
  (void) statusFlags;
  (void) inputBuffer;

  // get_changes();
  for (int i = 0; i < synth->cci; i++) {
    cc_prep(synth->ccs[i]);
  }
  // fill buffer
  for( unsigned long i=0; i<framesPerBuffer; i++ ) {
    // use iget inside
    get_frame(buffer, synth, i);
  }
  // process buffer
  if (synth->multi) process(buffer);

  // output buffer
  for( unsigned long i=0; i<framesPerBuffer * 2; i += 2 ) {
    // use iget inside
    *out++ = buffer[i];
    *out++ = buffer[i+1];
  }
  // finalize_changes();
  for (int i = 0; i < synth->cci; i++) {
    cc_fix(synth->ccs[i]);
  }
  return paContinue;
}

void
init_synth(synth_t * synth)
{
  synth->cci = 0;
  CC(synth->cc_cutoff, "cutoff", 10, 5000, 30, 5000);
  CC(synth->cc_resonance, "resonance", 1, 10, .02, 1);
  //CC(synth->cc_lfo_freq, "lfo_freq", .01, 10, .02, 1);
  CC(synth->cc_lfo_freq, "lfo_freq", 1, 1000, 2, 1);
  CC(synth->cc_lfo_amp, "lfo_amp", 0, 1, .01, 0);
  CC(synth->cc_pitch, "pitch", -3, 4, 0.01, 1);

  synth->modi = 0;

  synth->gain = 1;
  synth->x = 1;
  
  synth->adsr.a = 0.0;
  synth->adsr.peak = 1.0f;
  synth->adsr.d = 0.3;
  synth->adsr.s = 0.7;
  synth->adsr.r = 0.4;
  synth->adsr.elapsed = 0;

  synth->lfo.freq = 1.0f;
  synth->lfo.amp = 0.0f;
  synth->lfo.elapsed = 0;
  
  for (int i =0; i<MIDI_NOTES; i++) {
    synth->midi_note[i].freq = 0;
    synth->midi_note[i].channel = -1;
    synth->midi_note[i].noteOn = -1;
    synth->midi_note[i].noteOff = -1;
    synth->midi_note[i].velocity = -1;
    synth->midi_note[i].elapsed = -1;
    synth->midi_note[i].active = 0;
    synth->midi_note[i].wvt_index = 0;
    synth->midi_note[i].lfo_index = 0;
    synth->midi_note[i].adsr = &(synth->adsr);
  }

  synth->octave = 3;

  synth->poly = 0;
  synth->multi = 0;
  synth->filter = 1;
  synth->cutoff = 22000.0f;
  synth->clamp = 1;

  synth->gen[0] = gen0;
  synth->gen[1] = gen1;
  synth->gen[2] = gen2;
  synth->gen[3] = gen3;
  synth->gen[4] = gen4;
  synth->gen[5] = gen5;
  synth->gen[6] = gen6;
  synth->geni = 3;

  synth->active = 0;
  
  synth->viz.sample_rate_divider = 1;
  synth->viz.wi = 0;

  LowPass_Init();
  synth->fff = create_bw_low_pass_filter(2, SAMPLE_RATE, 400);
  synth->fff2 = create_bw_band_stop_filter(8, SAMPLE_RATE, 15000, 22000);

  init_sound(synth, sound_gen);
  wvt_init();

  synth->osctri = make_tri("triangle");
}

void
free_synth(synth_t * synth)
{
  destroy_sound(synth);

  free_bw_low_pass(synth->fff);
  free_bw_band_stop(synth->fff2);
}