#include "adsr.h"
 
/* Applies the ADSR envelope to the synthesized signal and scales it to the range -1 to 1 */
 
// Rate contains the number of clocks per period of the LFO
 
void adsr(out_t *y, data_t x, control_t rate, bool lfo)
{
    // The max range of the input is 2^23 - 1
    const temp_t max_val = 8388607;
 
    const frac_t A_length = 0.15;
    const frac_t D_length = 0.15;
    const frac_t S_length = 0.3;
    const frac_t R_length = 0.4;
 
    static udata_t count = 0;
    static bool prev_lfo;
    static control_t prev_rate;
 
    static udata_t A_samples;
    static udata_t D_samples;
    static udata_t S_samples;
    static udata_t R_samples;
 
    static udata_t D_range;
    static udata_t R_range;
 
    static frac_t A_slope, D_slope, R_slope;
 
    frac_t result;
 
    frac_t x_scale = (frac_t) ((temp_t) x / max_val);
 
    // The S section is independent of time so leave it constant
    const frac_t S_val = 0.8;
 
    if(prev_lfo != lfo && lfo)
    {
        // Reset counter on the rising edge
        count = 0;
    }
 
    if(rate != prev_rate)
    {
        // Calculate new bounds and slopes
        A_samples = A_length * rate;
        D_samples = D_length * rate + A_samples;
        S_samples = S_length * rate + D_samples;
        R_samples = R_length * rate + S_samples;
 
        A_slope = (((frac_t) 0.999) / A_samples);       // Can't represent 1 so use something close
        D_slope = ((frac_t) -0.2)   / (D_samples - A_samples);
        R_slope = ((frac_t) -0.75)  / (R_samples - S_samples);
    }
 
    if(count <= A_samples)
    {
        // Apply the "attack" portion of the envelope
        // Envelope varies from 0 -> 1
        result = (frac_t) (x_scale * A_slope) * count;
    }
    else if(count > A_samples && count <= D_samples)
    {
        // Apply the "decay" portion of the envelope
        // Envelope decreases from 1 -> 0.8
        result = x_scale * ((frac_t) (D_slope * (count-A_samples) + (frac_t) 0.999));
    }
    else if(count > D_samples && count <= S_samples)
    {
        // Apply the "sustain" portion of the envelope
        // Envelope remains at 0.8
        result = S_val * x_scale;
    }
    else if(count > S_samples && count <= R_samples)
    {
        // Apply the "release" portion of the envelope
        // Envelope decreases from 0.8 -> 0.05
        result = x_scale * ((frac_t) (R_slope * (count - S_samples)) + (frac_t) 0.8);
    }
    else
    {
        // Outside the envelope, output 0
        result = 0;
    }
 
    // Scale output to -1 to 1
    *y = (out_t) result;
 
    prev_rate = rate;
    prev_lfo = lfo;
 
    // Increment counter, compensating for the average latency of the block
//  count++;
    count += 28;
}