#include <stdint.h>
#include <strings.h>

#include "rtl8710bx.h"

extern uint32_t DiagPrintf(const char *fmt, ...)
    __attribute__((format(printf, 1, 2)));
extern void DelayMs(uint32_t ms);

#define printf DiagPrintf

typedef struct {
  char ssid[33];
  uint8_t bssid[6];
  uint8_t channel;
  int8_t rssi;
} Network_Info_t;

__attribute__((interrupt)) void HardFault_Handler(void) {
  __asm volatile(
      "tst lr, #4\n"
      "ite eq\n"
      "mrseq r0, msp\n"
      "mrsne r0, psp\n"
      "mov r1, lr\n"
      "ldr r2, [r0, #24]\n"
      "ldr r3, =0xE000ED30\n"
      "ldr r3, [r3]\n"
      "bl fault_handler_c\n"
      "bkpt #0\n");
}

void fault_handler_c(uint32_t *stack_frame, uint32_t lr, uint32_t pc,
                     uint32_t hfsr) {
  volatile uint32_t r0 = stack_frame[0];
  volatile uint32_t r1 = stack_frame[1];
  volatile uint32_t r2 = stack_frame[2];
  volatile uint32_t r3 = stack_frame[3];
  volatile uint32_t r12 = stack_frame[4];
  volatile uint32_t lr_ret = stack_frame[5];
  volatile uint32_t pc_ret = stack_frame[6];
  volatile uint32_t psr = stack_frame[7];

  printf("\n=== HARD FAULT ===\n");
  printf("HFSR: 0x%08lx\n", hfsr);
  printf("Fault PC: 0x%08lx\n", pc);
  printf("LR: 0x%08lx\n", lr);
  printf("R0: 0x%08lx\n", r0);
  printf("R1: 0x%08lx\n", r1);
  printf("R2: 0x%08lx\n", r2);
  printf("R3: 0x%08lx\n", r3);
  printf("R12: 0x%08lx\n", r12);
  printf("LR (ret): 0x%08lx\n", lr_ret);
  printf("PC (ret): 0x%08lx\n", pc_ret);
  printf("PSR: 0x%08lx\n", psr);

  volatile uint32_t cfsr = *((volatile uint32_t *)0xE000ED28);
  printf("CFSR: 0x%08lx\n", cfsr);

  while (1) {
  }
}

static void wifi_init(void) {
  PERI_ON->PESOC_CLK_CTRL |=
      BIT_SOC_ACTCK_BTCMD_EN | BIT_SOC_ACTCK_VENDOR_REG_EN;
  PERI_ON->SOC_HCI_COM_FUNC_EN |= BIT_SOC_HCI_WL_MACON_EN;

  WIFI->SYS.FUNC_EN |= FEN_MREGEN;
  WIFI->SYS.APS_FSMCO |= APFM_ONMAC;
  WIFI->SYS.APS_FSMCO &= ~SUS_HOST;
  WIFI->SYS.ISO_CTRL &= ~PWC_EV12V;

  // printf("waiting for APS_FSMCO\n");
  // while (!(wifi->SYS.APS_FSMCO & RDY_MACON));
  // printf("RDY_MACON\n");

  WIFI->WMAC.RCR |= (BM_DATA_EN | UC_DATA_EN);  // broadcast, unicast
  WIFI->MAC.CR |= MACRXEN;                      // en RX
}

void print_wifi_version(void) {
  printf("SYSTEM_CFG0 address: 0x%08X\n", (uint32_t)&SYSTEM_CTRL->SYSTEM_CFG0);
  printf("SYSTEM_CFG0: 0x%08X\n", SYSTEM_CTRL->SYSTEM_CFG0);
  // printf("SYSTEM_CFG1 address: 0x%08X\n",
  // (uint32_t)&SYSTEM_CTRL->SYSTEM_CFG1); printf("SYSTEM_CFG1: 0x%08X\n",
  // SYSTEM_CTRL->SYSTEM_CFG1); printf("SYSTEM_CFG2 address: 0x%08X\n",
  // (uint32_t)&SYSTEM_CTRL->SYSTEM_CFG2); printf("SYSTEM_CFG2: 0x%08X\n",
  // SYSTEM_CTRL->SYSTEM_CFG2);

  // uint32_t xtal_type = ((cfg ^ 0x10000) >> 16) & 1;
  // uint32_t cut_id = (cfg >> 8) & 3;
  // uint32_t chip_id = (cfg >> 4) & 0xF;
  // uint32_t revision;

  // if (cut_id == 1)
  //   revision = 2;
  // else if (cut_id == 2)
  //   revision = 1;
  // else
  //   revision = 0;

  // printf("SYSCFG0: 0x%08X\n", cfg);
  // printf("Version: 11\n");
  // printf("XTAL Type: 0x%x\n", xtal_type);
  // printf("Chip ID: 0x%x\n", chip_id);
  // printf("Revision: 0x%x\n", revision);
}

#define CLK_TABLE_ROM 0x00046E68
#define XTAL_TABLE_ROM 0x000046E10

void dump_hex(const void *data, size_t size, const char *prefix) {
  const uint8_t *bytes = (const uint8_t *)data;
  for (size_t i = 0; i < size; i++) {
    if (i % 16 == 0) printf("\n%s%04zX: ", prefix, i);
    printf("%02X ", bytes[i]);
  }
  printf("\n");
}

void print_clock_config(void) {
  printf("\n=== CLOCK CONFIGURATION ===\n");

  // all releveant regs
  printf("\nRegister Values (Raw):\n");
  printf("CLK_CTRL1:     0x%08X\n", SYSTEM_CTRL->CLK_CTRL1);
  printf("SYSPLL_CTRL0:  0x%08X\n", SYSTEM_CTRL->SYSPLL_CTRL0);
  printf("SYSPLL_CTRL1:  0x%08X\n", SYSTEM_CTRL->SYSPLL_CTRL1);
  printf("SYSPLL_CTRL2:  0x%08X\n", SYSTEM_CTRL->SYSPLL_CTRL2);
  printf("SYSPLL_CTRL3:  0x%08X\n", SYSTEM_CTRL->SYSPLL_CTRL3);
  printf("XTAL_CTRL0:    0x%08X\n", SYSTEM_CTRL->XTAL_CTRL0);
  printf("XTAL_CTRL1:    0x%08X\n", SYSTEM_CTRL->XTAL_CTRL1);
  printf("EFUSE_SYSCFG2: 0x%08X\n", SYSTEM_CTRL->EFUSE_SYSCFG[2]);

  // CLK_CTRL1
  uint32_t clk_ctrl1 = SYSTEM_CTRL->CLK_CTRL1;
  printf("\nCLK_CTRL1:\n");
  printf("  EXT32K_CK_SEL:    %d\n", (clk_ctrl1 & SYS_CLK_EXT32K_SEL) ? 1 : 0);
  printf("  OCP_CPU_CK_SEL:   %d\n", (clk_ctrl1 & SYS_CLK_CPU_CLK_SEL) >> 4);
  printf("  EELDR_CK_SEL:     %d\n", (clk_ctrl1 & SYS_CLK_EELDR_SEL) ? 1 : 0);

  uint32_t syspll_ctrl0 = SYSTEM_CTRL->SYSPLL_CTRL0;
  printf("\nSYSPLL_CTRL0:\n");
  printf("  CKTST_EN:         %d\n", (syspll_ctrl0 & SYSPLL_CKTST_EN) ? 1 : 0);
  printf("  MONCK_SEL:        %d\n",
         (syspll_ctrl0 & SYSPLL_MONCK_SEL_MASK) >> 19);
  printf("  CP_IOFFSET:       %d\n",
         (syspll_ctrl0 & SYSPLL_CP_IOFFSET_MASK) >> 14);
  printf("  FREF_EDGE:        %d\n", (syspll_ctrl0 & SYSPLL_FREF_EDGE) ? 1 : 0);
  printf("  PLL_EN:           %d\n", (syspll_ctrl0 & SYSPLL_EN) ? 1 : 0);
  printf("  LVPC_EN:          %d\n", (syspll_ctrl0 & SYSPLL_LVPC_EN) ? 1 : 0);

  // SYSPLL_CTRL1
  uint32_t syspll_ctrl1 = SYSTEM_CTRL->SYSPLL_CTRL1;
  printf("\nSYSPLL_CTRL1:\n");
  printf("  CL200M_SEL:       %d\n",
         (syspll_ctrl1 & SYSPLL_CL200M_SEL) ? 1 : 0);
  printf("  CK500K_SEL:       %d\n",
         (syspll_ctrl1 & SYSPLL_CK500K_SEL) ? 1 : 0);
  printf("  CK200M_EN:        %d\n", (syspll_ctrl1 & SYSPLL_CK200M_EN) ? 1 : 0);
  printf("  CKSDR_EN:         %d\n", (syspll_ctrl1 & SYSPLL_CKSDR_EN) ? 1 : 0);
  printf("  CKSDR_DIV:        %d\n",
         (syspll_ctrl1 & SYSPLL_CKSDR_DIV_MASK) >> 11);
  printf("  CK24P576_EN:      %d\n",
         (syspll_ctrl1 & SYSPLL_CK24P576_EN) ? 1 : 0);
  printf("  CK22P5792_EN:     %d\n",
         (syspll_ctrl1 & SYSPLL_CK22P5792_EN) ? 1 : 0);
  printf("  CK83P33M_EN:      %d\n",
         (syspll_ctrl1 & SYSPLL_CK83P33M_EN) ? 1 : 0);
  printf("  CK_PS_EN:         %d\n", (syspll_ctrl1 & SYSPLL_PS_EN) ? 1 : 0);
  printf("  CK_PS_SEL:        %d\n", (syspll_ctrl1 & SYSPLL_PS_SEL_MASK) >> 4);

  uint32_t syspll_ctrl3 = SYSTEM_CTRL->SYSPLL_CTRL3;
  printf("\nSYSPLL_CTRL3:\n");
  printf("  DIV_500M:         %d\n", (syspll_ctrl3 & SYSPLL_DIV_MASK) >> 6);
  printf("  PHASE_SEL:        %d\n", (syspll_ctrl3 & SYSPLL_PHASE_MASK) >> 3);
  printf("  500M_PS_EN:       %d\n",
         (syspll_ctrl3 & SYSPLL_500M_PS_EN) ? 1 : 0);
  printf("  500M_EN:          %d\n", (syspll_ctrl3 & SYSPLL_500M_EN) ? 1 : 0);

  // EFUSE_SYSCFG2
  uint32_t efuse_syscfg2 = SYSTEM_CTRL->EFUSE_SYSCFG[2];
  uint32_t spll_24_15 = (efuse_syscfg2 & SYSCFG2_ANAPAR_SPLL_24_15_MASK) >> 21;
  uint32_t spll_05_02 = (efuse_syscfg2 & SYSCFG2_ANAPAR_SPLL_05_02_MASK) >> 16;
  uint32_t xtal_stel_sel = (efuse_syscfg2 & SYSCFG2_XTAL_STEL_SEL_MASK) >> 12;
  uint32_t xtal_freq_sel = (efuse_syscfg2 & SYSCFG2_XTAL_FREQ_SEL_MASK) >> 8;

  printf("\nEFUSE_SYSCFG2:\n");
  printf("  SPLL 24:15 Config: 0x%03X\n", spll_24_15);
  printf("  SPLL 05:02 Config: 0x%X\n", spll_05_02);
  printf("  XTAL_STEL_SEL:    0x%X\n", xtal_stel_sel);
  printf("  XTAL_FREQ_SEL:    0x%X\n", xtal_freq_sel);

  printf("\nEFUSE_SYSCFG Array Dump:\n");
  for (int i = 0; i < 32; i++) {
    uint32_t val = SYSTEM_CTRL->EFUSE_SYSCFG[i];
    if (val != 0) {
      printf("EFUSE_SYSCFG[%2d]: 0x%08X\n", i, val);
    }
  }

  printf("\nSurrounding Registers:\n");
  volatile uint32_t *base = (volatile uint32_t *)&SYSTEM_CTRL->EFUSE_SYSCFG[0];
  for (int i = -16; i < 48; i++) {
    uint32_t val = base[i];
    if (val != 0) {
      printf("Offset %3d: 0x%08X\n", i * 4, val);
    }
  }

  // rom XTAL table
  printf("\nXTAL Table Contents (first 16 entries):\n");
  const uint32_t *xtal_table = (const uint32_t *)XTAL_TABLE_ROM;
  for (int i = 0; i < 16; i++) {
    printf("  [%d]: %d (0x%x Hz)\n", i, xtal_table[i], xtal_table[i]);
  }

  // xtal freq
  uint32_t xtal_freq = xtal_table[xtal_freq_sel];
  printf("\nSelected Crystal:\n");
  printf("  Index: %d\n", xtal_freq_sel);
  printf("  Frequency: %d Hz (0x%08X)\n", xtal_freq, xtal_freq);

  uint32_t xtal_ctrl0 = SYSTEM_CTRL->XTAL_CTRL0;
  printf("\nXTAL_CTRL0:\n");
  printf("  XTAL_EN:          %d\n", (xtal_ctrl0 & XTAL_EN) ? 1 : 0);
  printf("  BGMB_EN:          %d\n", (xtal_ctrl0 & XTAL_BGMB_EN) ? 1 : 0);
  printf("  GSPL_EN:          %d\n", (xtal_ctrl0 & XTAL_GSPL_EN) ? 1 : 0);
  printf("  GMP:              0x%02X\n", (xtal_ctrl0 & XTAL_GMP_MASK) >> 8);
  printf("  GMN:              0x%02X\n", (xtal_ctrl0 & XTAL_GMN_MASK) >> 13);
  printf("  SC_XI:            0x%02X\n", (xtal_ctrl0 & XTAL_SC_XI_MASK) >> 18);
  printf("  SC_XO:            0x%02X\n", (xtal_ctrl0 & XTAL_SC_XO_MASK) >> 24);
  printf("  GATED_OK:         %d\n", (xtal_ctrl0 & XTAL_GATED_OK) ? 1 : 0);
  printf("  XQSEL_RF:         %d\n", (xtal_ctrl0 & XTAL_XQSEL_RF) ? 1 : 0);

  uint32_t xtal_ctrl1 = SYSTEM_CTRL->XTAL_CTRL1;
  printf("\nXTAL_CTRL1:\n");
  printf("  DELAY_SYSPLL:     %d\n", (xtal_ctrl1 & XTAL_DELAY_SYSPLL) ? 1 : 0);
  printf("  DELAY_USB:        %d\n", (xtal_ctrl1 & XTAL_DELAY_USB) ? 1 : 0);
  printf("  DELAY_WLAFE:      %d\n", (xtal_ctrl1 & XTAL_DELAY_WLAFE) ? 1 : 0);
  printf("  AAC_GM_EN:        %d\n", (xtal_ctrl1 & XTAL_AAC_GM_EN) ? 1 : 0);
  printf("  AAC_PEAKDET_EN:   %d\n",
         (xtal_ctrl1 & XTAL_AAC_PEAKDET_EN) ? 1 : 0);
  printf("  DRV_SYSPLL:       %d\n", (xtal_ctrl1 & XTAL_DRV_SYSPLL_MASK) >> 15);
  printf("  GATE_SYSPLL:      %d\n", (xtal_ctrl1 & XTAL_GATE_SYSPLL) ? 1 : 0);
  printf("  LDO:              %d\n", (xtal_ctrl1 & XTAL_LDO_MASK));

  // dump clock table
  const uint32_t *clk_table = (uint32_t *)CLK_TABLE_ROM;
  printf("\nFull table contents:\n");
  for (int i = 0; i < 6; i++) {
    printf("  [%d]: %d Hz (%d MHz)\n", i, clk_table[i], clk_table[i] / 1000000);
  }

  // PLL freq
  uint32_t pll_base_freq =
      (syspll_ctrl1 & SYSPLL_CL200M_SEL) ? 200000000 : 166666666;
  printf("\nPLL Frequencies:\n");
  printf("  Base:        %d Hz (0x%x)\n", pll_base_freq, pll_base_freq);
  printf("  SDR Clock:   %d Hz (if enabled)\n",
         pll_base_freq >> (2 - ((syspll_ctrl1 & SYSPLL_CKSDR_DIV_MASK) >> 11)));
  printf("  83.33M:      %d Hz (if enabled)\n", 83330000);
  printf("  24.576M:     %d Hz (if enabled)\n", 24576000);
  printf("  22.5792M:    %d Hz (if enabled)\n", 22579200);

  // some rom table
  uint32_t cpu_clock_sel = (clk_ctrl1 & SYS_CLK_CPU_CLK_SEL) >> 4;
  uint32_t cpu_freq = clk_table[cpu_clock_sel];
  printf("\nCPU Configuration:\n");
  printf("  Clock Selection Index: %d\n", cpu_clock_sel);
  printf("  Frequency: %d Hz (%d MHz) (0x%x)\n", cpu_freq, cpu_freq / 1000000,
         cpu_freq);

  uint32_t xtal_to_pll = (pll_base_freq / (xtal_freq / 1000));
  uint32_t pll_to_cpu =
      ((uint32_t)(cpu_freq * 1000000UL) / (pll_base_freq / 1000));
  uint32_t xtal_to_cpu =
      ((uint32_t)(cpu_freq * 1000000UL) / (xtal_freq / 1000));

  printf("\nFrequency Relationships (x1000):\n");
  printf("  XTAL to PLL Base: %x.%03x\n", xtal_to_pll / 1000,
         xtal_to_pll % 1000);
  printf("  PLL Base to CPU:  %x.%03x\n", pll_to_cpu / 1000, pll_to_cpu % 1000);
  printf("  XTAL to CPU:      %x.%03x\n", xtal_to_cpu / 1000,
         xtal_to_cpu % 1000);
}

extern uint32_t _vector_table;

__attribute__((interrupt)) __attribute__((used)) void SysTick_Handler(void) {
  GPIOA->DR ^= (1 << 23);
}

// /* REG_SYS_CLK_CTRL1 */
// #define BIT_PESOC_EXT32K_CK_SEL (0x00000001 << 8)
// #define BIT_PESOC_OCP_CPU_CK_SEL (0x00000007 << 4)
// #define BIT_PESOC_EELDR_CK_SEL (0x00000001 << 0)

// unsigned int __fastcall CPU_ClkSet(int CpuType)
// {
//   unsigned int result; // r0

//   result = dword_40000014 & 0xFFFFFF8F | (16 * CpuType);
//   dword_40000014 = result;
//   return result;
// }

uint32_t CPU_ClkGet(uint8_t Is_FPGA);
void CPU_ClkSet(uint8_t CpuType);

int main(void) {
  printf("[main]\n");
  printf("Vector table @ %p\n", &_vector_table);
  printf("VTOR: 0x%08x\n", SCB->VTOR);
  // printf("FUNC_EN ptr:     0x%08X\n", &SYSTEM_CTRL->FUNC_EN);
  // printf("CLK_CTRL0 ptr:     0x%08X\n", &SYSTEM_CTRL->CLK_CTRL0);
  // printf("CLK_CTRL1 ptr:     0x%08X\n", &SYSTEM_CTRL->CLK_CTRL1);
  volatile uint32_t *raw_reg = (volatile uint32_t *)0x40000014;
  printf("Raw register before: 0x%08x\n", *raw_reg);
  printf("CLK_CTRL1 before: 0x%08x\n", SYSTEM_CTRL->CLK_CTRL1);

  __disable_irq();

  SYSTEM_CTRL->CLK_CTRL1 =
      (SYSTEM_CTRL->CLK_CTRL1 & ~SYS_CLK_CPU_CLK_SEL) | CPU_CLK_125M;

  // ppll 200m
  SYSTEM_CTRL->SYSPLL_CTRL1 |= SYSPLL_CK200M_EN;

  // Remove gating and increase drive strength
  uint32_t new_ctrl1 = SYSTEM_CTRL->XTAL_CTRL1;
  new_ctrl1 &= ~XTAL_GATE_SYSPLL;
  new_ctrl1 |= (3 << 15);
  SYSTEM_CTRL->XTAL_CTRL1 = new_ctrl1;

  // cur value is 0x000000C2 (DIV_500M = 3)
  // set DIV_500M to 0, (/ 2)
  uint32_t new_ctrl3 = SYSTEM_CTRL->SYSPLL_CTRL3;
  new_ctrl3 &= ~SYSPLL_DIV_MASK;
  // keep the 500M_EN bit (0x02) that's already set
  SYSTEM_CTRL->SYSPLL_CTRL3 = new_ctrl3;

  uint32_t new_ctrl0 = SYSTEM_CTRL->SYSPLL_CTRL0;
  new_ctrl0 &= ~SYSPLL_CP_IOFFSET_MASK;
  new_ctrl0 |= (0x10 << 14);
  SYSTEM_CTRL->SYSPLL_CTRL0 = new_ctrl0;

  // CPU_ClkSet(CLK_62_5M);

  printf("Raw register after: 0x%08x\n", *raw_reg);
  printf("CLK_CTRL1 after: 0x%08x\n", SYSTEM_CTRL->CLK_CTRL1);

  printf("CPU Clock: %d", CPU_ClkGet(0));

  SysTick_Config(100);  // tick every 100 cycles
  __enable_irq();

  print_clock_config();

  PERI_ON->SOC_PERI_FUNC1_EN |= BIT_PERI_GPIO_EN;
  PERI_ON->PESOC_CLK_CTRL |= APBPeriph_GPIO_CLOCK;
  // PINMUX_Config(_PA_0, PINMUX_FN_GPIO);
  PINMUX_Config(_PA_23, PINMUX_FN_GPIO);
  // PINMUX_ConfigPadPull(_PA_0, GPIO_PuPd_DOWN);
  PINMUX_ConfigPadPull(_PA_23, GPIO_PuPd_NOPULL);
  // GPIOA->DDR |= (1 << 0) | (1 << 23);
  GPIOA->DDR |= (1 << 23);

  while (1) {
  }
}
