#include <stdio.h>           //provides printf function
#include "pico/stdlib.h"     //provides standard library for the pico
#include "hardware/gpio.h"   //provides GPIO functions
#include "hardware/clocks.h" //provides Clock functions
#include "hardware/pio.h"    //provides PIO functions

#include "blink.pio.h"       //created by the SDK using pioasm

#define LED_GPIO PICO_DEFAULT_LED_PIN

void blink_pin_forever(PIO pio, uint sm, uint offset, uint pin, uint freq) {
    blink_program_init(pio, sm, offset, pin);
    pio_sm_set_enabled(pio, sm, true);

    printf("Blinking pin %d at %d Hz\n", pin, freq);

    // PIO counter program takes 3 more cycles in total than we pass as
    // input (wait for n + 1; mov; jmp)
    pio->txf[sm] = (clock_get_hz(clk_sys) / (2 * freq)) - 3;
}

int main()
{
    stdio_init_all();

    uint offset = pio_add_program(pio0, &blink_program);
    printf("Loaded program at %d\n", offset);

    blink_pin_forever(pio0, 0, offset, PICO_DEFAULT_LED_PIN, 1);
    
    //Try to add a LED (using the blue + button) and assign it to a second SM
    //blink_pin_forever(pio0, 1, offset, [OTHER PIN], [FREQUENCY]);

    while (true) {

      /*Do work*/
      printf("work work\n");

      sleep_ms(1000);
    }
}