use core::arch::global_asm;

use cortex_m::peripheral::SCB;

use crate::console::flush;
use crate::kprintf;
use crate::task::{with_task_manager, TaskRegisters};

fn print_regs(regs: &TaskRegisters) {
    kprintf!(
        "R0:  {:08X}  R1:  {:08X}  R2:  {:08X}  R3:  {:08X}\r\n",
        regs.r0,
        regs.r1,
        regs.r2,
        regs.r3
    );
    kprintf!(
        "R4:  {:08X}  R5:  {:08X}  R6:  {:08X}  R7:  {:08X}\r\n",
        regs.r4,
        regs.r5,
        regs.r6,
        regs.r7
    );
    kprintf!(
        "R8:  {:08X}  R9:  {:08X}  R10: {:08X}  R11: {:08X}\r\n",
        regs.r8,
        regs.r9,
        regs.r10,
        regs.r11
    );
    kprintf!(
        "R12: {:08X}  SP:  {:08X}  LR:  {:08X}  PC:  {:08X}\r\n",
        regs.r12,
        regs.sp,
        regs.lr,
        regs.pc
    );
    kprintf!("XPSR: {:08X}\r\n", regs.xpsr);
}

unsafe fn print_stack(regs: &TaskRegisters) {
    let sp = regs.sp as *const u32;

    kprintf!("SP: {:08X}\r\n", sp as u32);

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

unsafe fn print_mpu() {
    let p = doa_hallonbrod::pac::Peripherals::steal();
    let ctrl = p.PPB.mpu_ctrl.read();
    kprintf!(
        "MPU_CTRL: ENABLE: {}, HFIMENA: {}, PRIVDEFENA: {}\r\n",
        ctrl.enable().bit(),
        ctrl.hfnmiena().bit(),
        ctrl.privdefena().bit(),
    );
    for i in 0..8 {
        p.PPB.mpu_rnr.write(|w| w.region().bits(i));
        let rbar = p.PPB.mpu_rbar.read();
        let rasr = p.PPB.mpu_rasr.read();
        kprintf!("MPU Region {} ", i);
        if rasr.enable().bit() {
            let attrs = rasr.attrs().bits();
            kprintf!(
                "ENABLED addr {:08X} size {:08X} srd {:08b} TEXSCB {:06b} {}",
                rbar.addr().bits() << 8,
                2_u32.pow(rasr.size().bits() as u32 + 1),
                rasr.srd().bits(),
                attrs & 0b111111,
                match (attrs >> 8) & 0b111 {
                    0b000 => "none/none",
                    0b001 => "RW/none",
                    0b010 => "RW/RO",
                    0b011 => "RW/RW",
                    0b100 => "RESERVED",
                    0b101 => "RO/none",
                    0b110 => "RO/RO (110)",
                    0b111 => "RO/RO (111)",
                    _ => unreachable!(),
                }
            );
            if attrs & (1 << 12) != 0 {
                kprintf!(" XN");
            }
        } else {
            kprintf!("DISABLED");
        }
        kprintf!("\r\n");
    }
}

// This one is slightly different than the usual ExceptionEntry/Exit
// because we want to know the registers even if we came from handler
// mode.
global_asm!(
    r#"
    .section .HardFault, "ax"
    .global HardFault
    .type HardFault,function
HardFault:

    // stack the registers
    push {{r4, r5, r6, r7}} // push r4-r7
    mov r0, r8
    mov r1, r9
    mov r2, r10
    mov r3, r11
    push {{r0, r1, r2, r3}} // push r8-r11

    mov r3, lr
    movs r1, #0x04       // Check which mode we just came from
    ands r3, r1
    beq 2f              // if we came from thread mode,
    mrs r0, PSP         // use the process stack
    b 3f
2:  mrs r0, MSP         // otherwise, use main stack
3:
    push {{r0}}         // push SP
    sub sp, #32         // reserve space for the next eight registers
    mov r1, sp          // copy the stack pointer
    ldm r0!, {{r4, r5, r6, r7}} // load r0-r3
    stm r1!, {{r4, r5, r6, r7}} // store r0-r3
    ldm r0!, {{r4, r5, r6, r7}} // load r12, lr, pc, xpsr
    stm r1!, {{r4, r5, r6, r7}} // save r12, lr, pc, xpsr
    mov r0, sp          // get address of TaskRegisters struct
    mov r1, r3
    movs r2, 2          // shift the thread mode flag right so it's a
    rors r1, r2         // bool and it's the second argument
    push {{lr}}         // save LR

    bl hardfault_handler   // call the handler

    pop {{r0}}
    add sp, #68         // pop everything off the stack. Restoring the
                        // registers is irrelevant; the process is crashing.
    bx r0               // exit the handler
"#
);

#[no_mangle]
unsafe fn hardfault_handler(regs: &mut TaskRegisters, thread_mode: bool) {
    kprintf!(
        "\r\nHardFault! ({})\r\n",
        if thread_mode { "thread" } else { "handler" }
    );
    print_regs(regs);
    kprintf!("\r\n");

    print_stack(regs);
    kprintf!("\r\n");

    print_mpu();
    kprintf!("\r\n");

    let scb = &(*cortex_m::peripheral::SCB::PTR);
    kprintf!("ICSR: {:08X}\r\n", scb.icsr.read());
    flush();

    if thread_mode {
        // kill the process and invoke the scheduler
        with_task_manager(|tm| {
            tm.exit_current_task();
        });
        SCB::set_pendsv();
    } else {
        // Can't go nowhere from here
        cortex_m::interrupt::disable();

        loop {}
    }
}