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