#include "debug.h"

#include "FT18_STW_INIT.h"

#include <DueFlashStorage.h>

DueFlashStorage Flash;

void flash_dump_fault(const uint32_t *stack, const FaultStatusRegisters *fsr) {
  FlashDumpInfo info;
  memcpy(&info, flash_dump_get_info(), sizeof(info));

  FlashDump dump;
  memcpy(dump.stacked_registers, stack, sizeof(dump.stacked_registers));
  memcpy(&dump.fsr, fsr, sizeof(dump.fsr));

  uint32_t new_addr = FLASH_DUMP_ADDR_DUMPS + info.n_dumps * sizeof(dump);
  Flash.write(new_addr, (uint8_t *)&dump, sizeof(dump));

  info.n_dumps++;
  Flash.write(FLASH_DUMP_ADDR_INFO, (uint8_t *)&info, sizeof(info));
}

const FlashDumpInfo *flash_dump_get_info() {
  if (Flash.read(FLASH_DUMP_ADDR_INFO) != 0) {
    FlashDumpInfo info;
    info._zero = 0;
    info.n_dumps = 0;
    Flash.write(FLASH_DUMP_ADDR_INFO, (uint8_t *)&info, sizeof(info));
  }
  return (const FlashDumpInfo *)Flash.readAddress(FLASH_DUMP_ADDR_INFO);
}

const FlashDump *flash_dump_get_fault(uint32_t n) {
  uint32_t addr = FLASH_DUMP_ADDR_DUMPS + n * sizeof(FlashDump);
  return (const FlashDump *)Flash.readAddress(addr);
}

bool check_serial_available() {
  int available = Serial.availableForWrite();
  if (available == 0) {
    // No bytes available for write, maybe we were just writing? Wait 10 ms and
    // check again.
    delay(10);
    available = Serial.availableForWrite();
    if (available == 0) {
      // Still no bytes available for write, looks like we can't write at all.
      return false;
    }
  }
  // Write a byte, and wait 10 ms. If the number of available bytes decreased,
  // we can't write.
  Serial.write('\0');
  delay(10);
  return Serial.availableForWrite() >= available;
}

void print_dumped_faults() {
  uint32_t n = flash_dump_get_info()->n_dumps;

  Serial.println("====================");
  Serial.print(n);
  Serial.println(" DUMPED FAULTS");

  for (uint32_t i = 0; i < n; i++) {
    Serial.println("--------------------");
    Serial.print("Fault ");
    Serial.println(i);
    const FlashDump *dump = flash_dump_get_fault(i);
    print_stacked_registers(dump->stacked_registers);
    print_fault_registers(&dump->fsr);
  }
  Serial.println("====================");
}

void print_stacked_registers(const uint32_t *stack) {
  Serial.print("R0    = ");
  Serial.println(stack[0], HEX);
  Serial.print("R1    = ");
  Serial.println(stack[1], HEX);
  Serial.print("R2    = ");
  Serial.println(stack[2], HEX);
  Serial.print("R3    = ");
  Serial.println(stack[3], HEX);
  Serial.print("R12   = ");
  Serial.println(stack[4], HEX);
  Serial.print("LR    = ");
  Serial.println(stack[5], HEX);
  Serial.print("PC    = ");
  Serial.println(stack[6], HEX);
  Serial.print("PSR   = ");
  Serial.println(stack[7], HEX);
  Serial.println();
}

void print_fault_registers(const FaultStatusRegisters *fsr) {
  Serial.print("HFSR  = ");
  Serial.println(fsr->HFSR, HEX);
  Serial.print("CFSR  = ");
  Serial.println(fsr->CFSR, HEX);
  Serial.print("BFAR  = ");
  Serial.println(fsr->BFAR, HEX);
  Serial.print("DFSR  = ");
  Serial.println(fsr->DFSR, HEX);
  Serial.print("AFSR  = ");
  Serial.println(fsr->AFSR, HEX);
  Serial.print("SHCSR = ");
  Serial.println(fsr->SHCSR, HEX);
  Serial.println();
}

FaultStatusRegisters get_current_fsr() {
  FaultStatusRegisters fsr;
  fsr.HFSR = SCB->HFSR;
  fsr.CFSR = SCB->CFSR;
  fsr.BFAR = SCB->BFAR;
  fsr.DFSR = SCB->DFSR;
  fsr.AFSR = SCB->AFSR;
  fsr.SHCSR = SCB->SHCSR;
  return fsr;
}

void fault_handler(uint32_t *stack_addr, const char *fault_type,
                   const int *leds, unsigned n_leds) {
  Serial.begin(9600);
  for (unsigned i = 0; i < n_leds; i++) {
    pinMode(leds[i], OUTPUT);
  }

  FaultStatusRegisters fsr = get_current_fsr();

  flash_dump_fault(stack_addr, &fsr);

  while (1) {
    if (check_serial_available()) {
      Serial.println("====================");
      Serial.println(fault_type);
      Serial.println("====================");

      print_stacked_registers(stack_addr);
      print_fault_registers(&fsr);
    }
    for (unsigned i = 0; i < n_leds; i++) {
      digitalWrite(leds[i], HIGH);
    }
    delay(500);
    for (unsigned i = 0; i < n_leds; i++) {
      digitalWrite(leds[i], LOW);
    }
    delay(500);
  }
}

// If we don't wrap the handler functions in an extern C block, their names get
// mangled and they won't override the weakly-linked handler functions defined
// in the Arduino(-SAM) framework.
extern "C" {

// Get the stack pointer and pass it to the respective C handler. Depending on
// the EXC_RETURN value, we need to either use the Main Stack Pointer (msp) or
// the Process Stack Pointer (psp).
void HardFault_Handler(void) {
  __ASM("tst lr, #4\n"
        "ite eq\n"
        "mrseq r0, msp\n"
        "mrsne r0, psp\n"
        "b HardFault_Handler_C");
}

void MemManage_Handler(void) {
  __ASM("tst lr, #4\n"
        "ite eq\n"
        "mrseq r0, msp\n"
        "mrsne r0, psp\n"
        "b MemManage_Handler_C");
}

void BusFault_Handler(void) {
  __ASM("tst lr, #4\n"
        "ite eq\n"
        "mrseq r0, msp\n"
        "mrsne r0, psp\n"
        "b BusFault_Handler_C");
}

void UsageFault_Handler(void) {
  __ASM("tst lr, #4\n"
        "ite eq\n"
        "mrseq r0, msp\n"
        "mrsne r0, psp\n"
        "b UsageFault_Handler_C");
}

void HardFault_Handler_C(uint32_t *stack_addr) {
  // All LEDs
  static const int leds[] = {led1,  led2,  led3,  led4,  led5,  led6,
                             led7,  led8,  led9,  led10, led11, led12,
                             led13, led14, led15, led16};
  fault_handler(stack_addr, "HARD FAULT", leds, sizeof(leds) / sizeof(int));
}

void MemManage_Handler_C(uint32_t *stack_addr) {
  // Left of display
  static const int leds[] = {led11, led12, led13};
  fault_handler(stack_addr, "MEMMANAGE FAULT", leds,
                sizeof(leds) / sizeof(int));
}

void BusFault_Handler_C(uint32_t *stack_addr) {
  // Right of display
  static const int leds[] = {led14, led15, led16};
  fault_handler(stack_addr, "BUS FAULT", leds, sizeof(leds) / sizeof(int));
}

void UsageFault_Handler_C(uint32_t *stack_addr) {
  // Left and right of display
  static const int leds[] = {led11, led12, led13, led14, led15, led16};
  fault_handler(stack_addr, "USAGE FAULT", leds, sizeof(leds) / sizeof(int));
}
}