#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/clocks.h"

#include "constants.hpp"
#include "leds.pio.h"

static inline void leds_program_init(PIO pio, uint sm, uint offset) {
    uint mask = 0;
    for (uint pin=LED_BASE; pin<LED_NUM+LED_BASE; pin++) {
        mask |= (1 << pin);
        pio_gpio_init(pio, pin);
    }
    //pio_sm_set_pins_with_mask(pio, sm, 0, mask);
    pio_sm_set_consecutive_pindirs(pio, sm, LED_BASE, LED_NUM, true);

    pio_sm_config c = leds_program_get_default_config(offset);
    //sm_config_set_sideset(&c, 2, true, false);
    sm_config_set_sideset_pins(&c, LED_INTERLACE);
    sm_config_set_out_pins(&c, LED_BASE, LED_NUM);
    sm_config_set_set_pins(&c, 10, 2);
    // 1 bit per LED, plus 1 to signal which interlaced side
    sm_config_set_out_shift(&c, true, false, LED_NUM+1);
    // Combine TX and RX FIFOs into one large TX FIFO
    sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_TX);
    // MOV STATUS will be set to all-ones if the TX FIFO is empty, else all-zeros
    sm_config_set_mov_status(&c, STATUS_TX_LESSTHAN, 1);
    // Run at full speed since there's no critical timing (we just need
    // to toggle the LEDs and process incoming data as fast as possible)
    sm_config_set_clkdiv(&c, clock_get_hz(clk_sys));
    //sm_config_set_clkdiv(&c, 1.0f);
    pio_sm_init(pio, sm, offset + leds_offset_start, &c);
    pio_sm_set_enabled(pio, sm, true);
}

const PIO leds_pio = LEDS_PIO;
const uint leds_sm = LEDS_SM;

int main() {
  stdio_init_all();

  printf("LED Begin!\n");
  //sleep_ms(1000);
  uint offset_leds = pio_add_program(leds_pio, &leds_program);
  leds_program_init(leds_pio, leds_sm, offset_leds);
  pio_sm_set_enabled(leds_pio, leds_sm, true);

  sleep_ms(1000);

  //pio_payload payload = {};
  while (1) {
      printf("Sending All On\n");
      pio_sm_put_blocking(leds_pio, leds_sm, 0b101111110);
      pio_sm_put_blocking(leds_pio, leds_sm, 0b010000001);
      sleep_ms(1000);

      printf("Sending All Off\n");
      pio_sm_put_blocking(leds_pio, leds_sm, 0b010000000);
      pio_sm_put_blocking(leds_pio, leds_sm, 0b101111111);
      sleep_ms(1000);
  }
}