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

#include "ch573/gpio.h"

#define GPIO_PORT_A ch573_gpio__gpio_port_a
#define GPIO_PORT CH573_GPIO__GPIO_PORT_T_INTF

#define FLASH_1 12
#define DOWN_TIME 6
#define FLASH_0 1

static void delay(void)
{
  for (volatile uint32_t i = 0; i < 10000; ++i) {
    asm volatile("");
  }
}

static void flash_1()
{
  GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
  for (int i = 0; i < FLASH_1; ++i) {
    delay();
  }
  GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
  for (int i = 0; i < DOWN_TIME; ++i) {
    delay();
  }
}

static void reset_flash()
{
  for (int i = 0; i < 20; ++i) {
    GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
    delay();
    GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
    delay();
  }

  delay();
  delay();
  delay();
  delay();
  delay();
  delay();
  delay();
  delay();
  delay();
}

static void flash_0()
{
  GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
  for (int i = 0; i < FLASH_0; ++i) {
    delay();
  }
  GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
  for (int i = 0; i < (DOWN_TIME + (FLASH_1 - FLASH_0)); ++i) {
    delay();
  }
}

void stack_dump(uint32_t* sp, size_t n)
{
  puts("\nStack Dump:\n");
  int i = 0;
  while ((uint32_t)sp < 0x20008000 && i < n) {
    printf("   %p -> %08x\n", sp, *sp);
    sp++;
    i++;
  }
}

void flash_code(uint32_t code)
{
  while (1) {
    reset_flash();
    uint32_t c = code;
    for (int i = 0; i < 32; ++i, c <<= 1) {
      int msb = (c & 0x80000000) >> 31;
      if (msb) {
        flash_1();
      } else {
        flash_0();
      }
    }
  }
}

void panic(uint32_t code)
{
  GPIO_PORT.dir.set(GPIO_PORT_A, DIR_OUT, 8);
  GPIO_PORT.pd_drv.set(GPIO_PORT_A, 0, 8);

  printf("Panic called. Code 0x%08x\n", code);
  uint32_t* sp;
  asm volatile("add %0, sp, x0" : "=r"(sp));
  stack_dump(sp, 128);

  flash_code(code);
}