#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

#define LUT_SIZE    4096
#define LUT_SHIFT   20          /* 32 - 12 = 20, maps phase to 12-bit index */

#define RING_FRAMES 4800        /* 100ms at 48kHz */
#define RING_SAMPLES (RING_FRAMES * 2)

/* left-justified 24-bit amplitude constants */
#define AMP_LJ   ((int32_t)AMPLITUDE_24BIT << 8)
#define AMP_LJ_N (-AMP_LJ)

static int32_t sine_lut[LUT_SIZE];

static channel_state_t channels[2];
static int32_t ring[RING_SAMPLES];
static int ring_pos;
static SemaphoreHandle_t mutex;

static int32_t generate_sample(const channel_state_t *ch, uint32_t phase)
{
    switch (ch->type) {
    case WAVE_SINE:
        return sine_lut[phase >> LUT_SHIFT];

    case WAVE_SQUARE:
        return (phase < (uint32_t)(ch->duty * 4294967296.0f)) ? AMP_LJ : AMP_LJ_N;

    case WAVE_SAW: {
        /* symmetric triangle: -amp at 0, +amp at mid, -amp at max */
        int32_t p;
        if (phase < 0x80000000U) {
            /* first half: -amp to +amp */
            p = (int32_t)((int64_t)phase * 2 * AMP_LJ / (int64_t)0x80000000U) - AMP_LJ;
        } else {
            /* second half: +amp to -amp */
            uint32_t ph2 = phase - 0x80000000U;
            p = AMP_LJ - (int32_t)((int64_t)ph2 * 2 * AMP_LJ / (int64_t)0x80000000U);
        }
        return p;
    }

    default:
        return 0;
    }
}

static void regenerate_ring(void)
{
    uint32_t phase[2] = {0, 0};
    uint32_t inc[2];

    for (int c = 0; c < 2; c++) {
        if (channels[c].type != WAVE_NONE && channels[c].freq > 0.0f)
            inc[c] = (uint32_t)((double)channels[c].freq / SAMPLE_RATE * 4294967296.0);
        else
            inc[c] = 0;
    }

    for (int i = 0; i < RING_FRAMES; i++) {
        ring[i * 2]     = generate_sample(&channels[0], phase[0]);
        ring[i * 2 + 1] = generate_sample(&channels[1], phase[1]);
        phase[0] += inc[0];
        phase[1] += inc[1];
    }

    ring_pos = 0;
}

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

    for (int i = 0; i < LUT_SIZE; i++)
        sine_lut[i] = (int32_t)(AMPLITUDE_24BIT * sinf(2.0f * M_PI * i / LUT_SIZE)) << 8;

    memset(ring, 0, sizeof(ring));
    ring_pos = 0;

    DBG("waveform_init complete, LUT %d entries", LUT_SIZE);
}

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;
    regenerate_ring();
    xSemaphoreGive(mutex);
    DBG("waveform_set ch=%d type=%d freq=%.1f duty=%.2f", ch, type, freq, duty);
}

void waveform_fill_buffer(int32_t *buf, int frames)
{
    int samples = frames * 2;

    xSemaphoreTake(mutex, portMAX_DELAY);
    int pos = ring_pos;
    for (int i = 0; i < samples; i++) {
        buf[i] = ring[pos];
        if (++pos >= RING_SAMPLES)
            pos = 0;
    }
    ring_pos = pos;
    xSemaphoreGive(mutex);
}