#include "sust.h"
#include <math.h>
#include <stdlib.h>
#include <stdio.h>

void sust_init( struct sust * sust ) {
    sust->mode = MODE_Sostenuto;

    sust->playing = false;

    sust->start_idx = 0;
    sust->idx = 0;

    sust->buf_len = 51200;
    sust->record_buf = malloc( sizeof(float) * sust->buf_len );
    sust->play_buf = malloc( sizeof(float) * sust->buf_len );
}

float envelope( float x ) {
    if( x < 0.4 )
        return (x / 0.4) * (x / 0.4);
    if( x < 0.6 )
        return 1.0;

   float v  = 1.0 - ((x - 0.6) / 0.4);
   return v*v;
}

float softcos( float x ) {
    return sin( x * (3.141 / 2.0) );
}

void sust_resize( struct sust * sust, size_t new_len ) {
  sust->buf_len = new_len;
  sust->play_buf = realloc( sust->play_buf, sizeof(float) * new_len );
  sust->record_buf = realloc( sust->record_buf, sizeof(float) * new_len );
  sust->idx %= new_len;
}

void sust_swap(
    struct sust * sust
) {
    float * tmp = sust->play_buf;
    sust->play_buf = sust->record_buf;
    sust->record_buf = tmp;

    for( int i = 0; i  < sust->buf_len; ++i ) {
        sust->record_buf[i] = sust->play_buf[i];
    }
}

void sust_process(
    struct sust * sust,
    size_t frame_size,
    float const * in,
    float * out
) {
    for( size_t i = 0; i < frame_size; ++i ) {

        if( sust->mode == MODE_Sustain && sust->idx == 0 ) {
            sust_swap( sust );
        }

        float out_value = in[i];

        if( sust->playing ) {
            int n_voices = 5;

            for( int v = 0; v < n_voices; ++v ) {
                size_t play_idx = (sust->start_idx + sust->idx + ((v*sust->buf_len)/n_voices)) % sust->buf_len;
                float gain = envelope( ((float) play_idx) / ((float)sust->buf_len) );
               //printf("gain = %f\n", gain);
                out_value += 0.5 *gain * sust->play_buf[ play_idx ];
            }
        }

        if( sust->mode == MODE_Sostenuto ) {
            sust->record_buf[ sust->idx ] = in[i];
        }
        if( sust->mode == MODE_Sustain ) {
            sust->record_buf[ sust->idx ] = 0.5*out_value;
        }

        sust->idx = (sust->idx + 1) % sust->buf_len;
        out[i] = out_value;
    }
}