#include "adsr.h"
#include "synth_engine.h"
#include "synth_math.h"

float
adsr_amplitude(adsr_t *adsr, float noteOn, float noteOff, unsigned long long elapsed)
{
  float mod = 0.0;
  float release_amplitude = 0.0;

  float lifetime = elapsed / (float)SAMPLE_RATE;
  //float time_active = (noteOn  + lifetime) - noteOff;
  float time_active = noteOn - noteOff + lifetime;

  if (noteOn != 0 && noteOff == 0) {
    if (lifetime < adsr->a)
      mod = (lifetime / adsr->a)*(lifetime / adsr->a) * adsr->peak;

    if (lifetime >=  adsr->a && lifetime <= ( adsr->a +  adsr->d))
      mod = ((lifetime - adsr->a) / adsr->d) * (adsr->s - adsr->peak) + adsr->peak;

    if (lifetime > (adsr->a + adsr->d))
      mod = adsr->s;
  }
  else { // Note is off
    if (lifetime < adsr->a)
      release_amplitude = (lifetime / adsr->a)*(lifetime / adsr->a) * adsr->peak;

    if (lifetime >= adsr->a && lifetime <= (adsr->a + adsr->d))
      release_amplitude = ((lifetime - adsr->a) / adsr->d) * (adsr->s - adsr->peak) + adsr->peak;

    if (lifetime > (adsr->a + adsr->d))
      release_amplitude = adsr->s;

    mod = (time_active / adsr->r) * (0.0 - release_amplitude) + release_amplitude;

    if (adsr->r < 0) {
      mod = adsr->s;
    }

  }
  // Amplitude should not be negative
  if (mod <= 0.000)
    mod = 0.0;

  return clamp(mod, -1, 1);
}

float
fix_adsr(adsr_t *adsr, float noteOn, float noteOff, unsigned long long elapsed, unsigned long long noteOffSample)
{
  // convert to samples
  unsigned long long attack = adsr->a * SAMPLE_RATE + 1;
  unsigned long long decay = adsr->d * SAMPLE_RATE + 1;
  unsigned long long sustain = adsr->s * SAMPLE_RATE + 1;
  unsigned long long release = adsr->r * SAMPLE_RATE + 1;

  float mod = 0.0f;
  
  // if note is pressed
  if (noteOn != 0 && noteOff == 0) {
    if (elapsed < attack) {
      // we are in the attack phase
      mod = ((float)elapsed / attack) * adsr->peak;
    }
    else if (/* elapsed >= attack && */ elapsed < attack + decay) {
      // we are in the decay phase
      mod = ((float)(elapsed - attack) / decay) * (adsr->s - adsr->peak) + adsr->peak;
    }
    else {// sustain
      mod = adsr->s;
    }
  }
  else { // note is not pressed
    if (elapsed < noteOffSample + release) {
      // we are on the release

      // reuse previous block to find starting value
      if (noteOffSample < attack) {
        mod = ((float)noteOffSample / attack) * adsr->peak;
      }
      else if (/* elapsed >= attack && */ noteOffSample < attack + decay) {
        mod = ((float)(noteOffSample - attack) / decay) * (adsr->s - adsr->peak) + adsr->peak;
      }
      else {
        mod = adsr->s;
      }

      mod = (0 - mod) / (float)release * (elapsed - noteOffSample) + mod;
    }
    //printf("el: %ld, att: %ld MOD: %f, on %f, off %f\n", elapsed, attack, mod, noteOn, noteOff);
  }


  return mod;
}