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

uint32_t pwm_set_freq_duty(uint slice_num, uint chan, uint32_t f, int d)
{
  uint32_t clock = 125000000;
  uint32_t divider16 = clock / f / 4096 + (clock % (f * 4096) != 0);
  uint32_t t1 = clock / f;
  uint32_t t2 = t1 / 4096;
  uint32_t t3 = clock % (f * 4096);

  if (divider16 / 16 == 0)
    divider16 = 16;
  uint32_t wrap = clock * 16 / divider16 / f - 1;
  uint32_t t4 = wrap * d / 100;
  pwm_set_clkdiv_int_frac(slice_num, divider16 / 16, divider16 & 0xf);
  pwm_set_wrap(slice_num, wrap * d / 100);
  printf("In set freq duty\n");
  printf ("t1: %d, t2: %d, t3: %d, t4: %d\n", t1, t2, t3, t4);
  printf("div: %d, clock: %d, wrap: %d\n", divider16, clock, wrap);
  return wrap;
}



int main() {
  stdio_init_all();
  gpio_set_function(22, GPIO_FUNC_PWM);
  uint slice_num = pwm_gpio_to_slice_num(22);
  uint chan = pwm_gpio_to_channel(22);
  uint32_t wrap = pwm_set_freq_duty(slice_num, chan, 50, 75);
  printf("wrap is %d\n", wrap);
  pwm_set_enabled(slice_num, true);

  wrap = pwm_set_freq_duty(slice_num, chan, 20000, 50);
  printf("wrap is %d\n", wrap);
  pwm_set_enabled(slice_num, true);

  wrap = pwm_set_freq_duty(slice_num, chan, 1, 50);
  printf("wrap is %d\n", wrap);
  pwm_set_enabled(slice_num, true);

  while (true) {
    printf("Hello, Tom4!\n");
    sleep_ms(250);
  }
}