#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/pll.h"
#include "hardware/clocks.h"
#include "hardware/structs/pll.h"
#include "hardware/structs/clocks.h"

int main() {
  clock_gpio_init(21, CLOCKS_CLK_GPOUT0_CTRL_AUXSRC_VALUE_CLK_SYS, 10);
  clock_gpio_init(23, CLOCKS_CLK_GPOUT1_CTRL_AUXSRC_VALUE_CLK_REF, 10);

  // Temporarily switch to crystal clock
  clock_configure(clk_sys,
                  CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
                  CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_XOSC_CLKSRC,
                  12 * MHZ,
                  12 * MHZ);

  // Configure PLL sys to 1500 / 5 / 3 = 100MHz
  // Parameters obtained by running:
  // python src/rp2_common/hardware_clocks/scripts/vcocalc.py --input 12 100
  pll_init(pll_sys, 1, 1500 * MHZ, 5, 3);

  // Switch back to PLL
  clock_configure(clk_sys,
                  CLOCKS_CLK_SYS_CTRL_SRC_VALUE_CLKSRC_CLK_SYS_AUX,
                  CLOCKS_CLK_SYS_CTRL_AUXSRC_VALUE_CLKSRC_PLL_SYS,
                  100 * MHZ,
                  100 * MHZ);

  // PWM (Clock output) setup
  gpio_set_function(2, GPIO_FUNC_PWM);
  uint slice_num = pwm_gpio_to_slice_num(2);
  pwm_set_wrap(slice_num, 9);
  pwm_set_chan_level(slice_num, PWM_CHAN_A, 5); // 50% duty cycle

  pwm_set_enabled(slice_num, true);

  while (1)
    tight_loop_contents();
}