#include "clock.h"
#include <stdio.h>

#include "system.h"
#include "ch573/clock.h"

#define SAFE_MODE_CLOCK enter_safe_mode(); \
  CH573_CLOCK__CLOCK_CFG_T_INTF
#define CLOCK CH573_CLOCK__CLOCK_CFG_T_INTF
#define CLOCK_CFG ch573_clock__clock_cfg

/* Interface to a clock. */
struct clock_intf {
  int (*enable_clock)(const clock_cfg_t*);
  int (*switch_clock)(const clock_cfg_t*);
  int (*disable_other_clocks)(const clock_cfg_t*);
};

static inline int enable_pll_clock(const clock_cfg_t*);
static inline int switch_to_pll(const clock_cfg_t*);
static inline int disable_for_pll(const clock_cfg_t*);

static inline int enable_lse_clock(const clock_cfg_t*);
static inline int switch_to_lse(const clock_cfg_t*);
static inline int disable_for_lse(const clock_cfg_t*);

static inline int enable_lsi_clock(const clock_cfg_t*);
static inline int switch_to_lsi(const clock_cfg_t*);
static inline int disable_for_lsi(const clock_cfg_t*);

static inline int enable_hse_clock(const clock_cfg_t*);
static inline int switch_to_hse(const clock_cfg_t*);
static inline int disable_for_hse(const clock_cfg_t*);

#define pll_clock                              \
  ((struct clock_intf){                        \
      .enable_clock = enable_pll_clock,        \
      .switch_clock = switch_to_pll,           \
      .disable_other_clocks = disable_for_pll, \
  })

#define lse_clock                              \
  ((struct clock_intf){                        \
      .enable_clock = enable_lse_clock,        \
      .switch_clock = switch_to_lse,           \
      .disable_other_clocks = disable_for_lse, \
  })

#define lsi_clock                              \
  ((struct clock_intf){                        \
      .enable_clock = enable_lsi_clock,        \
      .switch_clock = switch_to_lsi,           \
      .disable_other_clocks = disable_for_lsi, \
  })

#define hse_clock                              \
  ((struct clock_intf){                        \
      .enable_clock = enable_hse_clock,        \
      .switch_clock = switch_to_hse,           \
      .disable_other_clocks = disable_for_hse, \
  })

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

static inline int disable_pll(void)
{
  SAFE_MODE_CLOCK.hfck_pwr_ctrl.clk_pll_pon.set(CLOCK_CFG, OFF);
  return 0;
}

static inline int disable_hse(void)
{
  SAFE_MODE_CLOCK.hfck_pwr_ctrl.clk_xt32m_pon.set(CLOCK_CFG, OFF);
  return 0;
}

static inline int disable_lse(void)
{
  SAFE_MODE_CLOCK.ck32k_config.clk_xt32k_pon.set(CLOCK_CFG, DISABLED);
  return 0;
}

static inline int disable_lsi(void)
{
  SAFE_MODE_CLOCK.ck32k_config.clk_int32k_pon.set(CLOCK_CFG, DISABLED);
  return 0;
}

static inline int enable_pll(void)
{
  SAFE_MODE_CLOCK.hfck_pwr_ctrl.clk_pll_pon.set(CLOCK_CFG, ON);
  return 0;
}

static inline int enable_hse(void)
{
  SAFE_MODE_CLOCK.hfck_pwr_ctrl.clk_xt32m_pon.set(CLOCK_CFG, ON);
  return 0;
}

static inline int enable_lse(void)
{
  SAFE_MODE_CLOCK.ck32k_config.clk_xt32k_pon.set(CLOCK_CFG, ENABLED);
  fast_delay();
  SAFE_MODE_CLOCK.xt32k_tune.xt32k_c_load.set(CLOCK_CFG, 0xf);
  fast_delay();
  return 0;
}

static inline int enable_lsi(void)
{
  SAFE_MODE_CLOCK.ck32k_config.clk_int32k_pon.set(CLOCK_CFG, ENABLED);
  return 0;
}

static inline int enable_pll_clock(const clock_cfg_t* cfg)
{
  printf("Turning on PLL\n");
  int ec = hse_clock.enable_clock(cfg);  // The PLL uses the HSE.
  if (ec) {
    return ec;
  };
  printf("Do enable.\n");
  enable_pll();
  printf("Fast delay.\n");
  fast_delay();  // Delay for a few microseconds to wait for the PLL to stablize
  printf("Done\n");
  return 0;
}

static inline int enable_lse_clock(const clock_cfg_t* cfg)
{
  enable_lse();
  fast_delay();
  return 0;
}

static inline int enable_lsi_clock(const clock_cfg_t* cfg)
{
  enable_lsi();
  fast_delay();
  return 0;
}

static inline int enable_hse_clock(const clock_cfg_t* cfg)
{
  uint8_t divisor = cfg->hse_clock_divisor;
  // Divisor should only be 5 bits in size with a minimum value of 2.
  if (divisor > 0x1f || divisor == 1) {
    return 1;
  }

  // divisor = 0 => use the default value.
  if (divisor == 0) {
    divisor = 5;  // Default value
  }

  enable_hse();
  printf("Setting Divisor: %d\n", divisor);
  SAFE_MODE_CLOCK.clk_sys_cfg.clk_pll_div.set(CLOCK_CFG, divisor);
  fast_delay();
  return 0;
}

static inline int switch_to_pll(const clock_cfg_t* cfg)
{
  printf("Switch to PLL.\n");
  SAFE_MODE_CLOCK.clk_sys_cfg.clk_sys_mod.set(CLOCK_CFG, HCLK_SRC_PLL);
  return 0;
}

static inline int switch_to_lse(const clock_cfg_t* cfg)
{
  fast_delay();
  SAFE_MODE_CLOCK.ck32k_config.clk_osc32k_xt.set(CLOCK_CFG, EXTERNAL_32K_OSC);
  fast_delay();
  SAFE_MODE_CLOCK.clk_sys_cfg.clk_sys_mod.set(CLOCK_CFG, HCLK_SRC_CK32K);
  return 0;
}

static inline int switch_to_lsi(const clock_cfg_t* cfg)
{
  SAFE_MODE_CLOCK.ck32k_config.clk_osc32k_xt.set(CLOCK_CFG, INTERNAL_32K_OSC);
  fast_delay();
  SAFE_MODE_CLOCK.clk_sys_cfg.clk_sys_mod.set(CLOCK_CFG, HCLK_SRC_CK32K);
  fast_delay();
  return 0;
}

static inline int switch_to_hse(const clock_cfg_t* cfg)
{
  SAFE_MODE_CLOCK.clk_sys_cfg.clk_sys_mod.set(CLOCK_CFG, HCLK_SRC_CK32M);
  return 0;
}

static inline int disable_for_pll(const clock_cfg_t* cfg)
{
  printf("Disable other clocks.\n");
  // disable_lsi();
  // disable_lse();
  // Need to keep HSE on
  return 0;
}

static inline int disable_for_lse(const clock_cfg_t* cfg)
{
  // disable_lsi();
  disable_pll();
  disable_hse();
  return 0;
}

static inline int disable_for_lsi(const clock_cfg_t* cfg)
{
  // disable_lse();
  disable_pll();
  disable_hse();
  return 0;
}

static inline int disable_for_hse(const clock_cfg_t* cfg)
{
  // disable_lse();
  // disable_lsi();
  disable_pll();
  return 0;
}

int get_system_clock(clock_cfg_t* out)
{
  int ck32k = CLOCK.ck32k_config.clk_osc32k_xt.get(CLOCK_CFG);
  int ckSel = CLOCK.clk_sys_cfg.clk_sys_mod.get(CLOCK_CFG);

  if (ckSel == HCLK_SRC_PLL) {
    out->sel = CLOCK_SELECTION_PLL;
  } else if (ckSel == HCLK_SRC_CK32M) {
    out->sel = CLOCK_SELECTION_HSE;
  } else if (ckSel == HCLK_SRC_CK32K) {
    if (ck32k == INTERNAL_32K_OSC) {
      out->sel = CLOCK_SELECTION_LSI;
    } else if (ck32k == EXTERNAL_32K_OSC) {
      out->sel = CLOCK_SELECTION_LSE;
    } else {
      return 1;
    }
  } else {
    return 1;
  }

  out->hse_clock_divisor = 0;
  if (out->sel == CLOCK_SELECTION_HSE || out->sel == CLOCK_SELECTION_PLL) {
    out->hse_clock_divisor = CLOCK.clk_sys_cfg.clk_pll_div.get(CLOCK_CFG);
  }

  return 0;
}

uint32_t get_clock_freq(const clock_cfg_t* cfg)
{
  uint32_t freq = 0;

  switch (cfg->sel) {
    case CLOCK_SELECTION_PLL:
      freq = 480000000 / cfg->pll_clock_divisor;
      break;
    case CLOCK_SELECTION_HSE:
      freq = 32000000 / cfg->hse_clock_divisor;
      break;
    case CLOCK_SELECTION_LSE:
    case CLOCK_SELECTION_LSI:
      freq = 32000;
      break;
  }

  return freq;
}

extern clock_chg_evt_listener_t CLOCK_CHANGE_LISTENERS_START;
extern clock_chg_evt_listener_t CLOCK_CHANGE_LISTENERS_END;

static void fire_clock_change_listeners(const clock_cfg_t* clk_cfg)
{
  clock_chg_evt_listener_t* cur = &CLOCK_CHANGE_LISTENERS_START;
  while (cur < &CLOCK_CHANGE_LISTENERS_END) {
    cur->on_clk_change(clk_cfg);
    ++ cur;
  }
}

int set_system_clock(const clock_cfg_t* clk_cfg)
{
#define clocks \
  ((struct clock_intf[]){pll_clock, lse_clock, lsi_clock, hse_clock})
  int ec = clocks[clk_cfg->sel].enable_clock(clk_cfg);

  if (ec) {
    return ec;
  }

  ec = clocks[clk_cfg->sel].switch_clock(clk_cfg);

  if (ec) {
    return ec;
  }

  ec = clocks[clk_cfg->sel].disable_other_clocks(clk_cfg);

  if (ec) {
    return ec;
  }

  fire_clock_change_listeners(clk_cfg);
#undef clocks
  return 0;
}