// main.c — RP2040 PIO PWM generator with external control (2 pots)
// - Embedded PIO program built with pio_encode_* macros (no .pio file, no literals)
// - Frequency controlled by ADC0 (GPIO26)
// - Duty controlled by ADC1 (GPIO27)
// - Output on GPIO0 by default
//
// Key idea (adaptive resolution):
//   The PWM period is implemented as a number of PIO clock cycles.
//   At low frequency -> large cycle count -> finer duty resolution.
//   At high frequency -> small cycle count -> coarser duty resolution,
//   but we can still reach 12 MHz (with small cycle counts).
//
// Tested conceptually for sys_clk ~ 125 MHz (Pico default).
// NOTE: At very high frequencies, signal integrity depends on wiring/measurement.

#include <stdio.h>
#include <math.h>

#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/adc.h"
#include "hardware/clocks.h"

// -----------------------------
// User configuration
// -----------------------------
#define PWM_GPIO        0       // Output pin (GPIO0)
#define ADC_FREQ_GPIO   26      // ADC0 on GPIO26
#define ADC_DUTY_GPIO   27      // ADC1 on GPIO27
#define ADC_FREQ_CH     0
#define ADC_DUTY_CH     1

// Frequency range controlled by the frequency pot
// You can change these to whatever you like:
#define FREQ_MIN_HZ     1000.0f     // 1 kHz
#define FREQ_MAX_HZ     12000000.0f // 12 MHz (requested)

// Update rate (how often we read the pots and reconfigure)
#define CONTROL_UPDATE_MS  5

// Small hysteresis to avoid constantly restarting the SM due to ADC noise
#define MIN_PERIOD_CHANGE_COUNTS  1
#define MIN_DUTY_CHANGE_COUNTS    1

// -----------------------------
// PIO PWM program (embedded)
// -----------------------------
//
// We generate PWM by counting cycles in PIO.
// PIO clock is typically clk_sys / clkdiv; we keep clkdiv = 1.0 for max range.
//
// Instruction-level behavior (steady-state period):
//
//   period_loop:
//     mov x, isr      ; x = high_count
//     mov y, osr      ; y = low_count
//     set pins, 1     ; output HIGH
// high_loop:
//     jmp x-- high_loop  ; loop 'high_count' times, 1 cycle per loop
//     set pins, 0     ; output LOW
// low_loop:
//     jmp y-- low_loop   ; loop 'low_count' times
//     jmp period_loop
//
// Cycle accounting per PWM period:
//
//   mov x,isr     : 1
//   mov y,osr     : 1
//   set high      : 1
//   high loop     : high_count cycles (each jmp is 1 cycle when taken)
//   set low       : 1
//   low loop      : low_count cycles
//   jmp loop      : 1
//
// Total cycles per period = high_count + low_count + 5
//
// Therefore output frequency:
//   f_out = f_pio / (high + low + 5)
// and with clkdiv=1:
//   f_pio = f_sys
//
// We load (high_count, low_count) once at startup into ISR/OSR,
// then each period we copy them into X/Y because X/Y get decremented.
//
// Program prolog (runs once after reset/restart):
//
//   pull block           ; wait for 32-bit word from TX FIFO
//   out isr, 16          ; ISR = low16   (shift direction chosen accordingly)
//   out osr, 16          ; OSR = high16  (remaining bits)
//
// Then steady-state loop uses MOVs as above.
//
// Packing format we send from CPU:
//   bits[15:0]   = low_count
//   bits[31:16]  = high_count

static uint16_t pwm_pio_instructions[] = {
    // pull block
    pio_encode_pull(true, false),

    // We want: ISR = low_count, OSR = high_count
    // With OUT destination = ISR/OSR.
    // NOTE: OUT shifts bits from OSR -> destination.
    // We'll configure SHIFT RIGHT, so the first OUT gets the least-significant bits.
    pio_encode_out(pio_isr, 16), // ISR <- low_count  (LSB 16)
    pio_encode_out(pio_osr, 16), // OSR <- high_count (next 16)

    // period_loop:
    pio_encode_mov(pio_x, pio_isr),  // X <- low_count? WAIT: we want X=high, Y=low in loops
    pio_encode_mov(pio_y, pio_osr),  // Y <- high_count? swapped...
    // We'll fix by swapping interpretation consistently below.

    // set pins, 1
    pio_encode_set(pio_pins, 1),

    // high_loop: jmp x-- high_loop
    // Placeholder target, we’ll set wrap + offsets so it jumps to itself.
    pio_encode_jmp_x_dec(0), // target filled via relative offset using the final program layout

    // set pins, 0
    pio_encode_set(pio_pins, 0),

    // low_loop: jmp y-- low_loop
    pio_encode_jmp_y_dec(0), // target filled similarly

    // jmp period_loop
    pio_encode_jmp(0),
};

// We must define correct jump targets after we know instruction indices.
static void pwm_fixup_jumps(uint16_t *prog) {
    // Indices for readability (matching the array above):
    const uint I_PULL      = 0;
    const uint I_OUT_ISR   = 1;
    const uint I_OUT_OSR   = 2;
    const uint I_MOV_X     = 3;
    const uint I_MOV_Y     = 4;
    const uint I_SET_HI    = 5;
    const uint I_JMP_XDEC  = 6;
    const uint I_SET_LO    = 7;
    const uint I_JMP_YDEC  = 8;
    const uint I_JMP_LOOP  = 9;

    // We want:
    // - jmp x-- goes back to I_JMP_XDEC
    // - jmp y-- goes back to I_JMP_YDEC
    // - final jmp goes back to I_MOV_X (start of steady-state period_loop)
    prog[I_JMP_XDEC] = pio_encode_jmp_x_dec(I_JMP_XDEC);
    prog[I_JMP_YDEC] = pio_encode_jmp_y_dec(I_JMP_YDEC);
    prog[I_JMP_LOOP] = pio_encode_jmp(I_MOV_X);

    (void)I_PULL; (void)I_OUT_ISR; (void)I_OUT_OSR; (void)I_SET_HI; (void)I_SET_LO;
}

static struct pio_program pwm_pio_program = {
    .instructions = pwm_pio_instructions,
    .length = 10,
    .origin = -1,
};

// -----------------------------
// Helpers: map pots -> freq/duty -> counts
// -----------------------------

static inline uint16_t clamp_u16(int v) {
    if (v < 0) return 0;
    if (v > 65535) return 65535;
    return (uint16_t)v;
}

// Log mapping feels natural for wide range (1kHz..12MHz).
// adc_val: 0..4095
static float adc_to_frequency_hz(uint16_t adc_val) {
    float t = (float)adc_val / 4095.0f;  // 0..1
    float ratio = FREQ_MAX_HZ / FREQ_MIN_HZ;
    // f = fmin * ratio^t
    return FREQ_MIN_HZ * powf(ratio, t);
}

// Duty mapping linear: 0..1
static float adc_to_duty(uint16_t adc_val) {
    float t = (float)adc_val / 4095.0f;
    // Keep away from exact 0 or 1 if you want guaranteed toggling:
    // return 0.001f + t * 0.998f;
    return t;
}

// Compute (high_count, low_count) for the PIO program.
//
// The steady-state period uses total_cycles = high + low + 5
// So for a desired f_out:
//
static void compute_counts(uint32_t sys_hz, float f_out_hz, float duty,
                           uint16_t *high_count, uint16_t *low_count,
                           float *actual_freq_hz) {
    if (f_out_hz < 1.0f) f_out_hz = 1.0f;
    if (duty < 0.0f) duty = 0.0f;
    if (duty > 1.0f) duty = 1.0f;

    // Desired total PIO cycles per period:
    float total_cycles_f = (float)sys_hz / f_out_hz;

    // Remove fixed overhead (5 cycles):
    // variable_cycles = high + low
    float variable_cycles_f = total_cycles_f - 5.0f;

    // At very high freq, variable_cycles can go small or even negative due to overhead.
    // Clamp to at least 0 so the program still runs (then total ~ 5 cycles).
    int variable_cycles = (int)lroundf(variable_cycles_f);
    if (variable_cycles < 0) variable_cycles = 0;
    if (variable_cycles > 65535) variable_cycles = 65535;

    // Split variable cycles by duty:
    // high = round(variable * duty)
    // low  = variable - high
    int high = (int)lroundf((float)variable_cycles * duty);
    if (high < 0) high = 0;
    if (high > variable_cycles) high = variable_cycles;
    int low = variable_cycles - high;

    // Return as u16
    *high_count = clamp_u16(high);
    *low_count  = clamp_u16(low);

    // Actual frequency achieved (based on integer cycle counts):
    int total_cycles = high + low + 5;
    if (total_cycles < 1) total_cycles = 1;
    *actual_freq_hz = (float)sys_hz / (float)total_cycles;
}

// Pack counts to 32-bit word for PIO program
// bits[15:0]=low, bits[31:16]=high
static inline uint32_t pack_counts(uint16_t high, uint16_t low) {
    return ((uint32_t)high << 16) | (uint32_t)low;
}

// -----------------------------
// PIO init
// -----------------------------
static void pwm_pio_init(PIO pio, uint sm, uint offset, uint gpio) {
    // Assign pin to PIO
    pio_gpio_init(pio, gpio);
    pio_sm_set_consecutive_pindirs(pio, sm, gpio, 1, true);

    pio_sm_config c = pio_get_default_sm_config();
    sm_config_set_set_pins(&c, gpio, 1);

    // SHIFT RIGHT so OUT pulls LSB-first (low 16 first)
    sm_config_set_out_shift(&c, true, false, 32);

    // Fastest: clkdiv = 1.0 (max possible output frequency)
    sm_config_set_clkdiv(&c, 1.0f);

    // Wrap the steady-state loop from MOV_X (index 3) to JMP_LOOP (index 9)
    sm_config_set_wrap(&c, offset + 3, offset + 9);

    pio_sm_init(pio, sm, offset, &c);
}

// Load new (high,low) by restarting SM at program start, then feeding one word.
// This avoids adding any "pull" overhead inside the fast PWM loop.
static void pwm_pio_load_counts(PIO pio, uint sm, uint offset, uint32_t packed_counts) {
    // Stop SM cleanly
    pio_sm_set_enabled(pio, sm, false);

    // Restart state machine (resets PC, shift registers, etc.)
    pio_sm_restart(pio, sm);

    // Clear FIFOs to avoid stale data
    pio_sm_clear_fifos(pio, sm);

    // Feed the word the program "pull block" is waiting for
    pio_sm_put_blocking(pio, sm, packed_counts);

    // Start SM
    pio_sm_set_enabled(pio, sm, true);
}

// -----------------------------
// ADC read
// -----------------------------
static uint16_t read_adc_channel(uint ch) {
    adc_select_input(ch);
    // Small settling time for mux
    sleep_us(5);
    return adc_read(); // 12-bit (0..4095)
}

int main() {
    stdio_init_all();
    sleep_ms(200);

    // Fix up jump targets (because we built the program in C)
    pwm_fixup_jumps(pwm_pio_instructions);

    // Init ADC for pots
    adc_init();
    adc_gpio_init(ADC_FREQ_GPIO);
    adc_gpio_init(ADC_DUTY_GPIO);

    // PIO setup
    PIO pio = pio0;
    uint sm = 0;

    uint offset = pio_add_program(pio, &pwm_pio_program);
    pwm_pio_init(pio, sm, offset, PWM_GPIO);

    uint32_t sys_hz = clock_get_hz(clk_sys);

    // Initial values
    uint16_t last_high = 0, last_low = 0;
    float last_f = 0.0f, last_duty = 0.0f;

    // Start with something reasonable
    {
        float f = 100000.0f;   // 100 kHz
        float d = 0.5f;        // 50%
        float actual;
        uint16_t high, low;
        compute_counts(sys_hz, f, d, &high, &low, &actual);
        pwm_pio_load_counts(pio, sm, offset, pack_counts(high, low));
        last_high = high; last_low = low;
        last_f = f; last_duty = d;

        printf("SYS=%lu Hz, start f=%.1f Hz (actual %.1f Hz), duty=%.1f%%, high=%u low=%u\n",
               (unsigned long)sys_hz, f, actual, d*100.0f, high, low);
    }

    absolute_time_t next = make_timeout_time_ms(CONTROL_UPDATE_MS);

    while (true) {
        sleep_until(next);
        next = delayed_by_ms(next, CONTROL_UPDATE_MS);

        uint16_t adc_f = read_adc_channel(ADC_FREQ_CH);
        uint16_t adc_d = read_adc_channel(ADC_DUTY_CH);

        float f = adc_to_frequency_hz(adc_f);
        float duty = adc_to_duty(adc_d);

        // Compute counts for this target
        uint16_t high, low;
        float actual;
        compute_counts(sys_hz, f, duty, &high, &low, &actual);

        // Hysteresis / noise guard
        int dh = (int)high - (int)last_high;
        if (dh < 0) dh = -dh;
        int dl = (int)low - (int)last_low;
        if (dl < 0) dl = -dl;

        // Restart SM only if counts changed meaningfully
        if (dh >= MIN_DUTY_CHANGE_COUNTS || dl >= MIN_PERIOD_CHANGE_COUNTS) {
            pwm_pio_load_counts(pio, sm, offset, pack_counts(high, low));
            last_high = high;
            last_low = low;
            last_f = f;
            last_duty = duty;

            // Print occasionally (optional; can be noisy)
            printf("target=%.3f MHz, actual=%.3f MHz, duty=%.1f%%, counts: high=%u low=%u, total=%u\n",
                   f / 1e6f, actual / 1e6f, duty * 100.0f,
                   high, low, (unsigned)(high + low + 5));
        }
    }
}