#include "sust.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.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;
    }
}