#include <stdint.h>
#include <stdio.h>

#include "ch573/gpio.h"
#include "ch573/pwr.h"
#include "ch573/uart.h"
#include "clock.h"
#include "system.h"

#define GPIO_PORT_A ch573_gpio__gpio_port_a
#define GPIO_PORT CH573_GPIO__GPIO_PORT_T_INTF

#define UART1 ch573_uart__uart1
#define UART CH573_UART__UART_T_INTF

#define PWR1 ch573_pwr__pwr_mgmt
#define PWR CH573_PWR__PWR_MGMT_T_INTF

uint32_t collatz(uint32_t n)
{
  uint32_t c = 0;

  while (n > 1) {
    if (n % 2 == 0) {
      n /= 2;
    } else {
      n = n * 3 + 1;
    }
    c++;
  }

  return c;
}

static void print_reg(uint32_t reg)
{
  // 0x40001008
  printf("register @0x%08x = 0x%08x\n", reg, *(uint32_t*)reg);
}

static void set_system_clock_6Mhz(void)
{
  print_reg(0x40001008);
  printf("Setting system clock to 6Mhz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_HSE;
  clk_cfg.pll_clock_divisor = 5;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
  print_reg(0x40001008);
  printf("Done\n");
}

static void set_system_clock_3Mhz(void)
{
  print_reg(0x40001008);
  printf("Setting system clock to 3Mhz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_HSE;
  clk_cfg.pll_clock_divisor = 10;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
  print_reg(0x40001008);
  printf("Done\n");
}

static void set_system_clock_60Mhz(void)
{
  print_reg(0x40001008);
  printf("Setting system clock to 60Mhz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_PLL;
  clk_cfg.pll_clock_divisor = 8;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
  print_reg(0x40001008);
  printf("Done\n");
}

static void set_system_clock_30Mhz(void)
{
  print_reg(0x40001008);
  printf("Setting system clock to 30Mhz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_PLL;
  clk_cfg.pll_clock_divisor = 16;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
  print_reg(0x40001008);
  printf("Done\n");
}

static void set_system_clock_32kHz()
{
  print_reg(0x40001008);
  printf("Setting system clock to 32kHz...\n");
  clock_cfg_t clk_cfg = {0};
  clk_cfg.sel = CLOCK_SELECTION_LSE;
  if (set_system_clock(&clk_cfg)) {
    printf("Failed to set system clock.\n");
  }
}

static void basic_delay()
{
  for (int i = 0; i < 100000; ++i) {
    asm volatile("");
  }
}

static void fast_delay()
{
  for (int i = 0; i < 1000; ++i) {
    asm volatile("");
  }
}

/*
 * Main routine. This is called on_reset once everything else has been set up.
 */
int main(void)
{
  GPIO_PORT.dir.set(GPIO_PORT_A, DIR_OUT, 8);
  GPIO_PORT.pd_drv.set(GPIO_PORT_A, 0, 8);
  GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);

  GPIO_PORT.dir.set(GPIO_PORT_A, DIR_OUT, 11);
  GPIO_PORT.pd_drv.set(GPIO_PORT_A, 0, 11);

  GPIO_PORT.dir.set(GPIO_PORT_A, DIR_IN, 10);
  GPIO_PORT.pd_drv.set(GPIO_PORT_A, PD_DRV_OPEN_DRAIN, 11);

  set_system_clock_6Mhz();
  uint32_t reg = (*(uint32_t*)0x40001008);
  reg &= ~0x1f;
  reg |= 10;
  enter_safe_mode();
  (*(uint32_t*)0x40001008) = reg;


  for (int i = 0;; ++i) {
    GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
    GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
  }

  // clock_cfg_t cfg;
  // for (int i = 0;; ++i) {
  //   if (i % 16 == 0) {
  //     if (cur_clock) {
  //       set_system_clock_60Mhz();
  //     } else {
  //       set_system_clock_6Mhz();
  //     }
  //     get_system_clock(&cfg);
  //     printf("Cur Clock Mhz: %d\n", get_clock_freq(&cfg));
  //     cur_clock = !cur_clock;
  //   }

  //   basic_delay();
  //   GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
  //   basic_delay();
  //   GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
  // }
  return 0;
}