#include "cos_lpc51u68_bsp.h"

static void report_led_step(cos_lpc51u68_led_step_t step, uint32_t now)
{
    cos_lpc51u68_uart0_puts("led ");
    cos_lpc51u68_uart0_puts(cos_lpc51u68_gpio_leds_step_name(step));
    cos_lpc51u68_uart0_puts(" systick ms ");
    cos_lpc51u68_uart0_put_hex32(now);
    cos_lpc51u68_uart0_puts(" step ");
    cos_lpc51u68_uart0_put_hex32((uint32_t)step);
    cos_lpc51u68_uart0_puts("\r\n");
}

int main(void)
{
    uint32_t next_step_ms = 1000u;
    cos_lpc51u68_led_step_t step = COS_LPC51U68_LED_STEP_RED;

    cos_lpc51u68_uart0_init_9600_8n1();
    cos_lpc51u68_gpio_leds_init();
    cos_lpc51u68_systick_init_1ms();

    cos_lpc51u68_uart0_puts("\r\nCOS LPC51U68 GPIO blink smoke test\r\n");
    cos_lpc51u68_gpio_leds_set_step(step);
    report_led_step(step, cos_lpc51u68_systick_ms());

    for (;;) {
        uint32_t now = cos_lpc51u68_systick_ms();
        if ((int32_t)(now - next_step_ms) >= 0) {
            step = (cos_lpc51u68_led_step_t)(((uint32_t)step + 1u) & 0x3u);
            cos_lpc51u68_gpio_leds_set_step(step);
            report_led_step(step, now);
            next_step_ms += 1000u;
        }
        __asm volatile("wfi");
    }
}