#include <cstdarg>
#include <cstdint>
#include <cstdio>
#include "libopencm3/stm32/usart.h"
#include "libopencmsis/core_cm3.h"

static void PanicPrintf(const char* fmt, ...)
{
    va_list l;
    va_start(l, fmt);
    char buf[200] = {0};
    vsprintf(buf, fmt, l);

    const char* c = buf;
    while (*c != '\0')
    {
        usart_send_blocking(USART2, *c);
        c++;
    }

    va_end(l);
}

typedef struct
{
    __I uint32_t CPUID;   /*!< Offset: 0x000 (R/ )  CPUID Base Register                                   */
    __IO uint32_t ICSR;   /*!< Offset: 0x004 (R/W)  Interrupt Control and State Register                  */
    __IO uint32_t VTOR;   /*!< Offset: 0x008 (R/W)  Vector Table Offset Register                          */
    __IO uint32_t AIRCR;  /*!< Offset: 0x00C (R/W)  Application Interrupt and Reset Control Register      */
    __IO uint32_t SCR;    /*!< Offset: 0x010 (R/W)  System Control Register                               */
    __IO uint32_t CCR;    /*!< Offset: 0x014 (R/W)  Configuration Control Register                        */
    __IO uint8_t SHP[12]; /*!< Offset: 0x018 (R/W)  System Handlers Priority Registers (4-7, 8-11, 12-15) */
    __IO uint32_t SHCSR;  /*!< Offset: 0x024 (R/W)  System Handler Control and State Register             */
    __IO uint32_t CFSR;   /*!< Offset: 0x028 (R/W)  Configurable Fault Status Register                    */
    __IO uint32_t HFSR;   /*!< Offset: 0x02C (R/W)  HardFault Status Register                             */
    __IO uint32_t DFSR;   /*!< Offset: 0x030 (R/W)  Debug Fault Status Register                           */
    __IO uint32_t MMFAR;  /*!< Offset: 0x034 (R/W)  MemManage Fault Address Register                      */
    __IO uint32_t BFAR;   /*!< Offset: 0x038 (R/W)  BusFault Address Register                             */
    __IO uint32_t AFSR;   /*!< Offset: 0x03C (R/W)  Auxiliary Fault Status Register                       */
    __I uint32_t PFR[2];  /*!< Offset: 0x040 (R/ )  Processor Feature Register                            */
    __I uint32_t DFR;     /*!< Offset: 0x048 (R/ )  Debug Feature Register                                */
    __I uint32_t ADR;     /*!< Offset: 0x04C (R/ )  Auxiliary Feature Register                            */
    __I uint32_t MMFR[4]; /*!< Offset: 0x050 (R/ )  Memory Model Feature Register                         */
    __I uint32_t ISAR[5]; /*!< Offset: 0x060 (R/ )  Instruction Set Attributes Register                   */
    uint32_t RESERVED0[5];
    __IO uint32_t CPACR; /*!< Offset: 0x088 (R/W)  Coprocessor Access Control Register                   */
} SCB_Type;

#define SCB_FULL ((SCB_Type*)SCB_BASE)

extern "C" __attribute__((used)) void prvGetRegistersFromStack(uint32_t* pulFaultStackAddress)
{
    /* These are volatile to try and prevent the compiler/linker optimising them
    away as the variables never actually get used.  If the debugger won't show the
    values of the variables, make them global my moving their declaration outside
    of this function. */

    volatile uint32_t r0 = pulFaultStackAddress[0];
    volatile uint32_t r1 = pulFaultStackAddress[1];
    volatile uint32_t r2 = pulFaultStackAddress[2];
    volatile uint32_t r3 = pulFaultStackAddress[3];

    volatile uint32_t r12 = pulFaultStackAddress[4];
    volatile uint32_t lr = pulFaultStackAddress[5];  /* Link register. */
    volatile uint32_t pc = pulFaultStackAddress[6];  /* Program counter. */
    volatile uint32_t psr = pulFaultStackAddress[7]; /* Program status register. */

    //    volatile uint16_t* nvic = *((uint16_t*)0xe000ed04);
    uint32_t cfsr = SCB_FULL->CFSR;
    uint32_t hfsr = SCB_FULL->HFSR;
    uint32_t mmfar = SCB_FULL->MMFAR;
    uint32_t bfar = SCB_FULL->BFAR;

    /* When the following line is hit, the variables contain the register values. */

    PanicPrintf("CFSR: 0x%X\nHFSR: 0x%X\nMMFAR: 0x%X\nBFAR: 0x%X\nLR: 0x%X\nPC: 0x%X\nPSR: 0x%X", cfsr, hfsr, mmfar, bfar, lr, pc, psr);

    while (1)
        ;
}

__attribute__((naked)) void hard_fault_handler()
{
    __asm volatile(" tst lr, #4                                                \n"
                   " ite eq                                                    \n"
                   " mrseq r0, msp                                             \n"
                   " mrsne r0, psp                                             \n"
                   " ldr r1, [r0, #24]                                         \n"
                   " ldr r2, handler2_address_constHF                          \n"
                   " bx r2                                                     \n"
                   " handler2_address_constHF: .word prvGetRegistersFromStack  \n");
}