#include "arch.h"
#include "kern/panic.h"
#include "kern/log.h"
#include "kern/init.h"
#include "arch/stm32l4xxx/peripherals/clock.h"
#include "kern/gpio/sysled.h"
#include "kern/gpio/gpio_manager.h"
#include "kern/delay.h"

#include <stdarg.h>

#ifdef ARCH_STM32L4
_Noreturn void panic(const char* fmt, ...)
{
  uint32_t base[0];
  disable_all_interrupts();

  if (get_system_init_level() > INIT_LEVEL_2) {
    va_list l;
    va_start(l, fmt);

    kerr_logf("** Kernel Panic! **\n");
    kerr_vlogf(fmt, l);
    kerr_logf("** Stack:\n");

    int i = 0;
    for (; i < 20 && &base[i] != (void*)STACK_TOP; ++ i) {
      kerr_logf(" (%p) %p\n", &base[i], base[i]);
    }

    set_system_clock_MHz(4); /* reduce power usage while we do nothing. */
    for(;;);
  } else {
    set_system_clock_MHz(4); /* reduce power usage while we do nothing. */
    gpio_reserved_pin_t pin3 = get_sysled();
    for (;;) {
      set_gpio_pin_high(pin3);
      delay(100000);
      set_gpio_pin_low(pin3);
      delay(100000);
    }
  }
}
#endif