#include "pico/stdlib.h"
#include <pico/stdio_uart.h>
#include <stdio.h>

#define LED_DELAY_MS 80
#define LED_WHITE 2
#define LED_BLUE 3
#define LED_GREEN 4
#define LED_RED 5

int pico_led_init(void) {
  gpio_init(PICO_DEFAULT_LED_PIN);
  gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
  gpio_init(LED_WHITE);
  gpio_set_dir(LED_WHITE, GPIO_OUT);
  gpio_init(LED_BLUE);
  gpio_set_dir(LED_BLUE, GPIO_OUT);
  gpio_init(LED_GREEN);
  gpio_set_dir(LED_GREEN, GPIO_OUT);
  gpio_init(LED_RED);
  gpio_set_dir(LED_RED, GPIO_OUT);
  return PICO_OK;
}

void pico_set_led(bool led_on) {}

int main() {
  int rc = pico_led_init();
  hard_assert(rc == PICO_OK);

  // Set up our UART with the required speed.
  stdio_uart_init();

  // Normally, a call to sleep_ms would freeze the device
  // while connected to a debugger. We could use busy_wait_ms
  // as a work-around, but we'll disable this feature instead.
  timer_get_instance(0)->dbgpause = 0;

  gpio_put(PICO_DEFAULT_LED_PIN, true);

  for (int i = 0; i < 1000000; i++) {
    // Send out a string, with CR/LF conversions
    printf("BEEP BOOP POLICE!!! %d \n\r", i);
    gpio_put(LED_RED, true);
    sleep_ms(2);
    gpio_put(LED_RED, false);
    sleep_ms(LED_DELAY_MS);
    gpio_put(LED_RED, true);
    sleep_ms(2);
    gpio_put(LED_RED, false);
    sleep_ms(LED_DELAY_MS);
    gpio_put(LED_BLUE, true);
    sleep_ms(2);
    gpio_put(LED_BLUE, false);
    sleep_ms(LED_DELAY_MS);
    gpio_put(LED_BLUE, true);
    sleep_ms(2);
    gpio_put(LED_BLUE, false);
    sleep_ms(LED_DELAY_MS);
  }
  return 0;
}