#include "pio_pwm.h"
#include "pwm.h"
#include "pins.h"
#include "hardware/pio.h"
#include "hardware/clocks.h"
#include "pico/error.h"
#include "pio_pwm.pio.h"

// 5 State Machines koexistieren mit dem CYW43-Chip, der einen SM auf pio0 haelt.
// Wir claimen dynamisch: erst pio1 leerraeumen (4 SMs), dann eine SM aus pio0.
// Das Programm wird pro benutztem PIO-Block einmal geladen.
static struct {
    PIO pio;
    uint sm;
    uint pin;
} g_channels[PWM_PIO_COUNT];

static int try_claim(PIO pio) {
    // pio_claim_unused_sm(pio, false) liefert -1 statt panic bei Misserfolg.
    return pio_claim_unused_sm(pio, false);
}

void pio_pwm_init(void) {
    int offset[2] = {-1, -1};  // Programm-Offset in pio0 / pio1, -1 = noch nicht geladen.

    for (unsigned i = 0; i < PWM_PIO_COUNT; i++) {
        uint pin = PWM_PIO_FIRST_PIN + i;

        // Erst pio1 abgrasen (kein CYW43 dort), dann pio0.
        PIO pio = pio1;
        int sm = try_claim(pio1);
        if (sm < 0) {
            pio = pio0;
            sm = try_claim(pio0);
            if (sm < 0) {
                panic("Keine freie PIO-SM fuer PWM-Kanal %u", i);
            }
        }

        unsigned idx = (pio == pio0) ? 0 : 1;
        if (offset[idx] < 0) {
            if (!pio_can_add_program(pio, &pio_pwm_program)) {
                panic("PIO-Programm passt nicht mehr auf PIO%u", idx);
            }
            offset[idx] = (int)pio_add_program(pio, &pio_pwm_program);
        }

        g_channels[i].pio = pio;
        g_channels[i].sm = (uint)sm;
        g_channels[i].pin = pin;

        // Cycle-Bilanz pro PWM-Periode:
        //   3 Zyklen fuer pull/mov/mov (einmalig)
        // + 3 Zyklen * (PWM_MAX_LEVEL + 1) fuer die Zaehlschleife
        // Bei PWM_MAX_LEVEL=999 sind das 3003 PIO-Zyklen pro 1-kHz-Periode.
        float cycles_per_period = 3.0f * (PWM_MAX_LEVEL + 1) + 3.0f;
        float div = (float)clock_get_hz(clk_sys) / (cycles_per_period * PWM_FREQ_HZ);

        pio_pwm_program_init(pio, (uint)sm, (uint)offset[idx], pin, div);

        // Periode ins ISR laden: FIFO->OSR->ISR via SM-EXEC, ohne dass die
        // Hauptschleife laeuft. Danach wird der eigentliche Level (Startwert 0)
        // ins FIFO gelegt, den die Schleife via `pull noblock` in X uebernimmt.
        pio_sm_set_enabled(pio, (uint)sm, false);
        pio_sm_clear_fifos(pio, (uint)sm);
        pio_sm_put_blocking(pio, (uint)sm, PWM_MAX_LEVEL);
        pio_sm_exec(pio, (uint)sm, pio_encode_pull(false, false));
        pio_sm_exec(pio, (uint)sm, pio_encode_out(pio_isr, 32));

        pio_sm_put_blocking(pio, (uint)sm, 0);

        pio_sm_set_enabled(pio, (uint)sm, true);
    }
}

void pio_pwm_set(unsigned index, uint16_t level) {
    if (index >= PWM_PIO_COUNT) return;
    if (level > PWM_MAX_LEVEL) level = PWM_MAX_LEVEL;
    pio_sm_put_blocking(g_channels[index].pio, g_channels[index].sm, level);
}