#include <cstdint>

#include "gpio.h"
#include "itoa.h"
#include "sleep.h"
#include "trace.h"
#include "uart.h"

extern "C" uint32_t _initial_stack_pointer, _text_begin, _text_end;

namespace crash {
namespace {
[[maybe_unused]] void CrashHexDump(uint32_t* begin, uint32_t* end,
                                   int direction = 1) {
    char number[] = "00000000 ";
    char addr[] = "00000000: ";
    int i = 0;
    itoa(reinterpret_cast<uint32_t>(begin), addr);
    UartWriteCrash(addr);
    for (uint32_t* ptr = begin; direction > 0 ? ptr < end : ptr > end;
         ptr += direction, i++) {
        itoa(*ptr, number);
        UartWriteCrash(number);
        if (i % 4 == 3) {
            UartWriteCrash("\r\n");
            itoa(reinterpret_cast<uint32_t>(ptr + 1), addr);
            UartWriteCrash(addr);
        }
    }
    if (i % 4 != 3) {
        UartWriteCrash("\r\n");
    }
}

void StackTrace(uint32_t* sp) {
    char number[] = "00000000\r\n";
    for (uint32_t* ptr = sp; ptr < &_initial_stack_pointer; ptr++) {
        auto addr = reinterpret_cast<uint32_t*>(*ptr);
        if (addr < &_text_begin || addr >= &_text_end || (*ptr & 0x1) == 0) {
            continue;
        }
        itoa(*ptr, number);
        UartWriteCrash(number);
    }
}

struct Armv6mRegs {
    uintptr_t r4;
    uintptr_t r5;
    uintptr_t r6;
    uintptr_t r7;
    uintptr_t r8;
    uintptr_t r9;
    uintptr_t r10;
    uintptr_t r11;
    uintptr_t sp;

    // saved by the CPU
    uintptr_t r0;
    uintptr_t r1;
    uintptr_t r2;
    uintptr_t r3;
    uintptr_t r12;
    uintptr_t lr;
    uintptr_t pc;
    uintptr_t xpsr;
};

void CrashHandler(Armv6mRegs* regs) {
    char number[] = "00000000\r\n";

    UartWriteCrash("\r\n\r\nCra$h!!\r\n- xpsr: 0x");
    itoa(regs->xpsr, number);
    UartWriteCrash(number);
    UartWriteCrash("- pc: 0x");
    itoa(regs->pc, number);
    UartWriteCrash(number);
    UartWriteCrash("- lr: 0x");
    itoa(regs->lr, number);
    UartWriteCrash(number);
    UartWriteCrash("- Stack trace:\r\n");
    StackTrace(reinterpret_cast<uint32_t*>(regs->sp));

    tracing::dump();

    while (1) {
        gpio0->data = 0x55;
        sleep(100);
        gpio0->data = 0xaa;
        sleep(100);
    }
}
}  // namespace

__attribute__((naked)) void HardFaultHandler() {
    asm volatile(
        // set those leds first
        "mov r0, %0            \n"
        "mov r1, #0xa5         \n"
        "str r1, [r0]          \n"

        "mov r0, r8            \n"
        "mov r1, r9            \n"
        "mov r2, r10           \n"
        "mov r3, r11           \n"
        "mrs lr, msp           \n"
        "push {r0-r3, lr}      \n"
        "push {r4-r7}          \n"
        :
        : "r"(gpio0));
    asm volatile(
        "mrs r0, msp           \n"
        "mov r1, %0            \n"
        "blx r1                \n"
        :
        : "r"(CrashHandler));
}
}  // namespace crash