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

static inline void init_systick() {
  systick_hw->csr = 0x5;
  systick_hw->rvr = 0x00FFFFFF;
}

static inline uint32_t get_ticks() {
  return systick_hw->cvr;
}

static inline uint32_t get_elapsed_ticks(uint32_t systick_start) {
  uint32_t systick_elapsed = systick_hw->cvr;
  if(systick_elapsed > systick_start) {
    // underflow/wrap of CVR
    systick_elapsed = 0x00FFFFFF - (systick_elapsed - systick_start);
  } else {
    systick_elapsed = systick_start - systick_elapsed;
  }
  return systick_elapsed;
}

int main() {
  stdio_init_all();
  init_systick();
  uint32_t start_ticks;
  uint32_t elapsed_ticks;
  while (true) {
    start_ticks = get_ticks();
    printf("1");
    elapsed_ticks = get_elapsed_ticks(start_ticks);
    printf("Elapsed ticks: %d\n", elapsed_ticks);
    sleep_ms(250);

    start_ticks = get_ticks();
    printf("1234567890");
    elapsed_ticks = get_elapsed_ticks(start_ticks);
    printf("Elapsed ticks: %d\n", elapsed_ticks);
    sleep_ms(250);

    start_ticks = get_ticks();
    printf("1234567890123456");
    elapsed_ticks = get_elapsed_ticks(start_ticks);
    printf("Elapsed ticks: %d\n", elapsed_ticks);
    sleep_ms(250);

    start_ticks = get_ticks();
    printf("1234567890123456789012345678901");
    elapsed_ticks = get_elapsed_ticks(start_ticks);
    printf("Elapsed ticks: %d\n", elapsed_ticks);
    sleep_ms(250);

    start_ticks = get_ticks();
    printf("1234567890123456789012345678901234567890");
    elapsed_ticks = get_elapsed_ticks(start_ticks);
    printf("Elapsed ticks: %d\n", elapsed_ticks);
    sleep_ms(250);
  }
}