#include "waveform.h"
#include "debug.h"

#include <math.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"

#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif

static channel_state_t channels[2];
static SemaphoreHandle_t mutex;

void waveform_init(void)
{
    mutex = xSemaphoreCreateMutex();
    memset(channels, 0, sizeof(channels));
    channels[0].duty = 0.5f;
    channels[1].duty = 0.5f;
    DBG("waveform_init complete");
}

void waveform_set(int ch, wave_type_t type, float freq, float duty)
{
    if (ch < 0 || ch > 1) return;
    xSemaphoreTake(mutex, portMAX_DELAY);
    channels[ch].type  = type;
    channels[ch].freq  = freq;
    channels[ch].duty  = duty;
    channels[ch].phase = 0.0f;
    xSemaphoreGive(mutex);
    DBG("waveform_set ch=%d type=%d freq=%.1f duty=%.2f", ch, type, freq, duty);
}

static inline int16_t generate(channel_state_t *s)
{
    float p = s->phase;
    float v;

    switch (s->type) {
    case WAVE_SINE:
        v = sinf(2.0f * M_PI * p);
        break;
    case WAVE_SQUARE:
        v = (p < s->duty) ? 1.0f : -1.0f;
        break;
    case WAVE_SAW:
        if (p < 0.5f)
            v = p * 4.0f - 1.0f;
        else
            v = 3.0f - p * 4.0f;
        break;
    default:
        return 0;
    }

    s->phase += s->freq / (float)SAMPLE_RATE;
    if (s->phase >= 1.0f)
        s->phase -= 1.0f;

    return (int16_t)(AMPLITUDE * v);
}

int16_t waveform_next_sample(int ch)
{
    if (ch < 0 || ch > 1) return 0;
    xSemaphoreTake(mutex, portMAX_DELAY);
    int16_t s = generate(&channels[ch]);
    xSemaphoreGive(mutex);
    return s;
}