#include "synth_engine.h"
#include "lowpass.h"
#include "filter.h"

/* 1d convolution */
void
convole(float *signal, float *filter, size_t signal_size, size_t filter_size, float *out) {
  for (size_t i = 0; i < filter_size + signal_size; i++) {
    size_t kmin, kmax, k;
    out[i] = 0;
    /* find overlap */
    kmin = (i >= filter_size - 1) ? i - (filter_size - 1) : 0;
    kmax = (i < signal_size - 1) ? i : signal_size - 1;

    /* Add the overlaping values */
    for (k = kmin; k <= kmax; k++) {
      out[i] += signal[k] * filter[i - k];
    }
  }
}

float
clamp(float f)
{
  if (f <= -1) return -0.9999;
  if (f >= 1)  return  0.9999;
  return f;
}

float
adsr_amplitude(void *synthData, unsigned long long elapsed)
{
  synth_t *synth = (synth_t*)synthData;

  float dAmplitude = 0.0;
  float dReleaseAmplitude = 0.0;
  float dStartAmplitude = synth->adsr.peak;

  float dLifeTime = (elapsed * (1.0 / (float)SAMPLE_RATE));

  if (synth->n.noteOn != 0 && synth->n.noteOff == 0) {
    if (dLifeTime < synth->adsr.a)
      dAmplitude = (dLifeTime / synth->adsr.a)*(dLifeTime / synth->adsr.a) * dStartAmplitude;

    if (dLifeTime >=  synth->adsr.a && dLifeTime <= ( synth->adsr.a +  synth->adsr.d))
      dAmplitude = ((dLifeTime - synth->adsr.a) / synth->adsr.d) * (synth->adsr.s - dStartAmplitude) + dStartAmplitude;

    if (dLifeTime > (synth->adsr.a + synth->adsr.d))
      dAmplitude = synth->adsr.s;
  }
  else { // Note is off
    if (dLifeTime < synth->adsr.a)
      dReleaseAmplitude = (dLifeTime / synth->adsr.a)*(dLifeTime / synth->adsr.a) * dStartAmplitude;

    if (dLifeTime >= synth->adsr.a && dLifeTime <= (synth->adsr.a + synth->adsr.d))
      dReleaseAmplitude = ((dLifeTime - synth->adsr.a) / synth->adsr.d) * (synth->adsr.s - dStartAmplitude) + dStartAmplitude;

    if (dLifeTime > (synth->adsr.a + synth->adsr.d))
      dReleaseAmplitude = synth->adsr.s;

    dAmplitude = (((synth->n.noteOn + dLifeTime) - synth->n.noteOff) / synth->adsr.r) * (0.0 - dReleaseAmplitude) + dReleaseAmplitude;

    if (synth->adsr.r < 0) {
      dAmplitude = synth->adsr.s;
    }
  }
  // Amplitude should not be negative
  if (dAmplitude <= 0.000)
    dAmplitude = 0.0;

  return clamp(dAmplitude);
}


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
gen0(float f, unsigned long long phase, float x, unsigned int sample_rate)
{
  return sqr_sample(0.1, f,  0.3,             phase, sample_rate)
    + sqr_sample(0.1, f * 3.0 / 2.0 , 0.5, phase, sample_rate)
    + saw_sample(0.3, f,                   phase, sample_rate)
    + sin_sample(0.1, f,                   phase, sample_rate)
    + sin_sample(0.1, f * 5,               phase, sample_rate)
    /* + sin_sample(0.1, freq * 50 * 1021,       phase, sample_rate) */
    /* + sin_sample(0.1, freq * 50 * 3531021,    phase, sample_rate) */
    + sin_sample(0.1, f * 7,               phase, sample_rate);
}

float
gen1(float f, unsigned long long phase, float x, unsigned int sample_rate)
{
  return sawX_sample(0.5, f, 5, phase, sample_rate)
    + saw_sample(0.3, 2 * f / 5, phase, sample_rate)
    + sin_sample(0.2, f * 5.0 / 7.0 , phase, sample_rate);
}

float
gen2(float f, unsigned long long phase, float x, unsigned int sample_rate)
{
  /* return sin_sample(0.5, f * sqrt(2) , phase, sample_rate) */
  /*   + sin_sample(0.5, f, phase, sample_rate); */

  return sawX_sample(1, f, 5, phase, sample_rate);
}

float
gen3(float f, unsigned long long phase, float x, unsigned int sample_rate)
{
  /* return sawX_sample(0.7, f, 5, phase, sample_rate) */
  /*   + sin_sample(0.3, 4.0/17.0*f, phase, sample_rate); */
  return saw_sample(0.5, f * (1 + sqrt(5)) / 2, phase, sample_rate)
    + sin_sample(0.3, f * x, phase, sample_rate)
    + sqr_sample(0.2, f * x, 0.2 * x * x, phase, sample_rate);
}

float
make_sample(unsigned long long phase, void *synthData, unsigned int sample_rate, int viz)
{
  synth_t *synth = (synth_t*)synthData;
  float sample = 0;

  //LFO!
  //if (synth->adsr.elapsed > SAMPLE_RATE / 2) synth->adsr.elapsed = 0;

  if (synth->poly) {
    int n = synth->notes_active;
    for (int i = 0; i < n; i++) {
      sample += (1.0 / n) * synth->gen[synth->geni](synth->freq[i] + synth->freq_offset, synth->freq_count[i], synth->x, sample_rate);
    }
  } else {
      sample = synth->gen[synth->geni](synth->n.freq + synth->freq_offset, phase, synth->x, sample_rate);
  }

  if (!viz && synth->filter) {
    // ALLL THE FILTERS
    LowPass_Update(synth->resonance, (adsr_amplitude(synth, synth->adsr.elapsed) + 0.1) * round(synth->cutoff) + 1, sample_rate);
    sample = LowPass_Filter(sample);

    update_bw_low_pass_filter(synth->fff, SAMPLE_RATE, (adsr_amplitude(synth, synth->adsr.elapsed) + 0.1) * synth->cutoff, synth->resonance);
    sample = bw_low_pass(synth->fff, sample);
  }

  sample = synth->gain * adsr_amplitude(synth, synth->adsr.elapsed) * sample;

    
  // band stop for high freqs
  if (!viz)
    sample = bw_band_stop(synth->fff2, sample);

  
  if (synth->clamp) sample = clamp(sample);
  
  return sample;
}

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;

  (void) timeInfo; /* Prevent unused variable warnings. */
  (void) statusFlags;
  (void) inputBuffer;

  float s;
  for( unsigned long i=0; i<framesPerBuffer; i++ ) {
    //get_portaudio_frame(outputBuffer, synth);
    if (!synth->active) {
      *out++ = 0.0f;
      *out++ = 0.0f;
      continue;
    }
    if (adsr_amplitude(synth, synth->adsr.elapsed) == 0 && synth->n.noteOff != 0) {
      //printf("SYNTH OPFF\n");
      synth->active = 0;
      *out++ = 0.0f;
      *out++ = 0.0f;
      continue;
    }
    s = make_sample(synth->n.elapsed, synth, SAMPLE_RATE, 0);
    *out++ = s;
    *out++ = s;
    synth->adsr.elapsed++;
    synth->n.elapsed++;
    for (int j = 0; j < synth->notes_active; j++) {
      synth->freq_count[j]++;
    }
    if (!synth->multi) {
      for (int j = 0; j < synth->notes_active; j++) {
        if (synth->freq_count[j] >= (1.0 / synth->freq[i]) * SAMPLE_RATE) synth->freq_count[j] = 0;
      }
      if (synth->n.elapsed >= (1.0 / synth->n.freq) * SAMPLE_RATE) synth->n.elapsed = 0;
    } else {
      
    }
  }

  return paContinue;
}

void
init_synth(synth_t * synth)
{
  synth->freq_offset = 0;
  synth->gain = 1;
  synth->x = 1;

  synth->notes_active = 0;
  for (int i = 0; i<100;i++) {
    synth->freq[i] = 0;
    synth->freq_count[i] = 0;
  }
  
  synth->n.freq    = 0; 
  synth->n.noteOn  = 0; 
  synth->n.noteOff = 1; 
  synth->n.key     = 0; 
  synth->n.elapsed = 0; 

  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->octave = 3;

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

  synth->gen[0] = gen0;
  synth->gen[1] = gen1;
  synth->gen[2] = gen2;
  synth->gen[3] = gen3;
  synth->geni = 0;

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

  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);
}