use core::fmt::Write;
use cortex_m;
use cortex_m_rt::exception;
use cortex_m_rt::ExceptionFrame;
use crate::console::get_global_console;
use crate::util::print_hex_padded;

fn print_exception_frame(ef: &ExceptionFrame) {
    let console = get_global_console();

    write!(console, "PC: {:08X}\n", ef.pc).unwrap();
    write!(console, "LR: {:08X}\n", ef.lr).unwrap();
    write!(console, "XPSR: {:08X}\n", ef.xpsr).unwrap();
    write!(console, "R0: {:08X}\n", ef.r0).unwrap();
    write!(console, "R1: {:08X}\n", ef.r1).unwrap();
    write!(console, "R2: {:08X}\n", ef.r2).unwrap();
    write!(console, "R3: {:08X}\n", ef.r3).unwrap();
    write!(console, "IP: {:08X}\n", ef.r12).unwrap();
}

unsafe fn print_stack(ef: &ExceptionFrame) {
    let console = get_global_console();
    let sp = ef as *const ExceptionFrame as *const u32;

    write!(console, "SP: {:08X}\n", sp as u32).unwrap();

    for i in 0..4 {
        write!(console, "{:08X}:", (sp as u32) + i * 32).unwrap();
        for j in 0..8 {
            let d = *sp.offset((i * 8 + j) as isize);
            write!(console, " {:08X}", d).unwrap();
        }
        console.write("\n");
    }
}

#[exception]
unsafe fn HardFault(ef: &ExceptionFrame) -> ! {
    let console = get_global_console();
    console.write("\nHardFault!\n");
    print_exception_frame(ef);
    console.write("\n");

    print_stack(ef);
    console.write("\n");

    let scb = &(*cortex_m::peripheral::SCB::ptr());

    let hfsr = scb.hfsr.read();
    write!(console, "HFSR: {:08X}", hfsr).unwrap();
    if hfsr & 0x00000002 != 0 {
        console.write(" VECTBL");
    }
    if hfsr & 0x40000000 != 0 {
        console.write(" FORCED");
    }
    console.write("\n");

    if hfsr & 0x40000000 != 0 {
        // read all the other fault regs
        let cfsr = scb.cfsr.read();

        let mmfsr = (cfsr & 0xFF) as u8;
        write!(console, "MMFSR: {:08X}", mmfsr).unwrap();
        if mmfsr & 0x01 != 0 {
            console.write(" IACCVIOL");
        }
        if mmfsr & 0x02 != 0 {
            console.write(" DACCVIOL");
        }
        if mmfsr & 0x08 != 0 {
            console.write(" MUNSTKERR");
        }
        if mmfsr & 0x10 != 0 {
            console.write(" MSTKERR");
        }
        console.write("\n");

        if mmfsr & 0x80 != 0 {
            let mmfar = scb.mmfar.read();
            write!(console, "MMFAR: {:08X}", mmfar).unwrap();
        } else {
            console.write("MMFAR invalid");
        }
        console.write("\n");

        let bfsr = ((cfsr & 0xFF00) >> 8) as u8;
        write!(console, "BFSR: {:02X}", bfsr).unwrap();
        if bfsr & 0x01 != 0 {
            console.write(" IBUSERR");
        }
        if bfsr & 0x02 != 0 {
            console.write(" PRECISERR");
        }
        if bfsr & 0x04 != 0 {
            console.write(" IMPRECISERR");
        }
        if bfsr & 0x08 != 0 {
            console.write(" UNSTKERR");
        }
        if bfsr & 0x10 != 0 {
            console.write(" STKERR");
        }
        console.write("\n");

        if bfsr & 0x80 != 0 {
            let bfar = scb.bfar.read();
            write!(console, "BFAR: {:08X}", bfar).unwrap();
        } else {
            console.write("BFAR invalid");
        }
        console.write("\n");

        let ufsr = ((cfsr & 0xFFFF0000) >> 16) as u16;
        write!(console, "UFSR: {:04X}", ufsr).unwrap();
        if ufsr & 0x0001 != 0 {
            console.write(" UNDEFINSTR");
        }
        if ufsr & 0x0002 != 0 {
            console.write(" INVSTATE");
        }
        if ufsr & 0x0004 != 0 {
            console.write(" INVPC");
        }
        if ufsr & 0x0008 != 0 {
            console.write(" NOCP");
        }
        if ufsr & 0x0100 != 0 {
            console.write(" UNALIGNED");
        }
        if ufsr & 0x0200 != 0 {
            console.write(" DIVBYZERO");
        }
        console.write("\n");
    }

    loop {}
}