#include "stdint.h"
#include "stdbool.h"

#include "stm32f4xx_hal.h"

uint32_t vmem[1024];

void prvGetRegistersFromStack( uint32_t *pulFaultStackAddress ) __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;
//    volatile uint32_t r1;
//    volatile uint32_t r2;
//    volatile uint32_t r3;
//    volatile uint32_t r12;
//    volatile uint32_t lr; /* Link register. */
    volatile uint32_t pc; /* Program counter. */
//    volatile uint32_t psr;/* Program status register. */
//
//    r0 = pulFaultStackAddress[ 0 ];
//    r1 = pulFaultStackAddress[ 1 ];
//    r2 = pulFaultStackAddress[ 2 ];
//    r3 = pulFaultStackAddress[ 3 ];
//
//    r12 = pulFaultStackAddress[ 4 ];
//    lr = pulFaultStackAddress[ 5 ];
    pc = pulFaultStackAddress[ 6 ];
//    psr = pulFaultStackAddress[ 7 ];
//
//    REFER(r0);
//    REFER(r1);
//    REFER(r2);
//    REFER(r3);
//    REFER(r12);
//
//    REFER(lr);
    REFER(pc);
//    REFER(psr);

    uint16_t opcodeH = 0[(uint16_t*)pc];
    bool isOp32 = ((opcodeH >> 13) & 7) == 0b111 && ((opcodeH >> 11)&3) > 0;
    if (!isOp32) {
        uint32_t* aut = SCB->BFAR;
        uint32_t idx = ((uint32_t)aut) - 0x30000000;
        uint32_t* arrPtr = &vmem[idx >> 2];

        uint8_t opc7 = opcodeH >> 9;
        uint8_t opc5 = opcodeH >> 11;
        bool isLDR16 = (opc7 == 0b0101100 || opc5 == 0b01101);
        bool isSTR16 = (opc7 == 0b0101000 || opc5 == 0b01100);

        uint8_t rt16 = opcodeH & 7;
//        volatile uint8_t rn = (opcodeH >> 3) & 7;
//        volatile uint8_t rm = (opcodeH >> 6) & 7;

        // r0 = 0
        // r3 = 3
        // r4 = -8
        // r11 = -1
        // note: 16-bit thumb cannot address r8-r11
        int32_t offs;
        if (rt16 < 4) {
            offs = rt16;
        } else {
            offs = rt16 - 12;
        }

        if (isSTR16) {
            *arrPtr = pulFaultStackAddress[offs];
        } else if (isLDR16) {
            pulFaultStackAddress[offs] = *arrPtr;
        }
        pulFaultStackAddress[6] += 2; // increment PC
    } else {
        uint32_t opcodeL = 1[(uint16_t*)pc];
        uint32_t opcode = ( ((uint32_t)opcodeH) << 16) | opcodeL;
        // TODO:
        for(;;);
    }
}

void HardFault_Handler( void ) __attribute__( ( naked ) );

void HardFault_Handler(void) {
    __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_const                            \n"
    " push {r4-r11, lr}                                         \n"
    " blx r2                                                    \n"
    " pop {r4-r11, pc}                                          \n"
    " handler2_address_const: .word prvGetRegistersFromStack    \n"
    );
}