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

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 = 1.0;

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

  /* sawX_sample(1, synth->freq, 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 sawX_sample(0.5, f * (1 + sqrt(5)) / 2, 5, 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); */
}



/* 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];
    }
  }
}

void
low_pass_filter(float* signal, int length, float cutoff, float resonance, float* out) {
    float c = 1.0f / tanf(M_PI * cutoff); // calculate filter constant
    float a1 = 1.0f / (1.0f + resonance * c + c * c); // calculate filter coefficients
    float a2 = 2.0f * a1;
    float a3 = a1;
    float b1 = 2.0f * (1.0f - c * c) * a1;
    float b2 = (1.0f - resonance * c + c * c) * a1;
    float prev_input = 0.0f, prev_output = 0.0f; // initialize previous input and output to zero
    for (int i = 0; i < length; i++) {
        float input = signal[i];
        float output = a1 * input + a2 * prev_input + a3 * prev_output - (i >= 1 ? b1 * out[i-1] : 0.0f) - (i >= 2 ? b2 * out[i-2] : 0.0f);
        out[i] = output;
        prev_input = input;
        prev_output = output;
    }
}


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


  int n = 1;
  if (1 /* !synth->filter */) {
    for (int i = 0; i < n; i++) {
      sample += (1.0 / n) * 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) * 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; //synth->gen[synth->geni](synth->n.freq + synth->freq_offset, phase, synth->x);
    
    if (synth->clamp && sample >= 1) sample = 0.99;
    if (synth->clamp && sample <= -1) sample = -0.99;
  } else {
    // get sample array s[]
    int samples = 30;
    float s[samples];
    
    if (synth->adsr.elapsed < samples) {
      for (int i = 0; i < samples; i++) {
        s[i] = synth->gain * adsr_amplitude(synth, i) * synth->gen[synth->geni](synth->n.freq + synth->freq_offset, i, synth->x, sample_rate);
      }
    } else {
      for (int i = 0; i < samples; i++) {
        s[i] = synth->gain * adsr_amplitude(synth, synth->adsr.elapsed - 50 + i) * synth->gen[synth->geni](synth->n.freq + synth->freq_offset, phase - 50 + i, synth->x, sample_rate);
      }
    }

    // process s[]


    // return s[50]
  }
  
  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;

  if (1) {
    float s;
    for( unsigned long i=0; i<framesPerBuffer; i++ ) {
      //get_portaudio_frame(outputBuffer, synth);
      s = make_sample(synth->n.elapsed, synth, SAMPLE_RATE, 0);
      *out++ = s;
      *out++ = s;
      synth->adsr.elapsed++;
      synth->n.elapsed++;
      if (!synth->multi) {
        if (synth->n.elapsed >= (1.0 / synth->n.freq) * SAMPLE_RATE) synth->n.elapsed = 0;
      } else {
      
      }
    }
  } else {
    float s[FRAMES_PER_BUFFER * 5];

    if (synth->adsr.elapsed < framesPerBuffer * 5) {
      for (unsigned long long i = 0; i < framesPerBuffer * 5; i++) {
        s[i] = make_sample(i, synth, SAMPLE_RATE, 0);
        synth->adsr.elapsed++;
        synth->n.elapsed++;
        if (!synth->multi) {
          if (synth->n.elapsed >= (1.0 / synth->n.freq) * SAMPLE_RATE) synth->n.elapsed = 0;
        } else {
        }
      }
    } else {
      for (unsigned long long i = 0; i < framesPerBuffer * 5; i++) {
        s[i] = make_sample(synth->n.elapsed - framesPerBuffer * 2 + i, synth, SAMPLE_RATE, 0);
        synth->adsr.elapsed++;
        synth->n.elapsed++;
        if (!synth->multi) {
          if (synth->n.elapsed >= (1.0 / synth->n.freq) * SAMPLE_RATE) synth->n.elapsed = 0;
        } else {
        }
      }
    }

    // filter

    // output

    if (synth->adsr.elapsed < framesPerBuffer * 5) {
      for( unsigned long i=0; i<framesPerBuffer; i++ ) {
        *out++ = s[i];
        *out++ = s[i];
      }
    } else {
      for( unsigned long i=0; i<framesPerBuffer; i++ ) {
        *out++ = s[i + (2 * framesPerBuffer)];
        *out++ = s[i + (2 * framesPerBuffer)];
      }
    }
    synth->adsr.elapsed-= 4*framesPerBuffer;

    if (!synth->multi) {
      for (int i = 0; i < framesPerBuffer * 4; i++) {
        synth->n.elapsed--;
        if (synth->n.elapsed == 0) synth->n.elapsed = (int)(SAMPLE_RATE / synth->n.freq);
      }
    } else {
      synth->n.elapsed-= 4*framesPerBuffer;
    }
  }

  return paContinue;
}

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

  synth->n.freq    = 0; 
  synth->n.noteOn  = 0; 
  synth->n.noteOff = 1; 
  synth->n.key     = 0; 
  synth->n.elapsed = 0; 


  synth->adsr.a = 0.001;
  synth->adsr.d = 0.3;
  synth->adsr.s = 0.7;
  synth->adsr.r = 0.4;
  synth->adsr.elapsed = 0;

  synth->octave = 3;

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

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

  synth->viz.sample_rate_divider = 1;

  LowPass_Init();
  synth->fff = create_bw_low_pass_filter(2, SAMPLE_RATE, 400);
}