use core::ptr::addr_of_mut;

use stm32f4xx_hal::pac;

fn jump_to_bootloader() {
    unsafe {
        cortex_m::interrupt::disable();

        let address: u32 = 0x1FFF0000;

        let device = pac::Peripherals::steal();
        device.SYSCFG.memrm.modify(|_, w| w.bits(0x01));

        let mut p = cortex_m::Peripherals::steal();
        p.SCB.invalidate_icache();
        p.SCB.vtor.write(address as u32);

        cortex_m::interrupt::enable();

        cortex_m::asm::bootload(address as *const u32);
    }
}

const BOOTLOADER_REQUESTED: u32 = 0xdecafbad;

fn magic_mut_ptr() -> *mut u32 {
    extern "C" {
        #[link_name = "_dfu_magic"]
        static mut magic: u32;
    }

    unsafe { addr_of_mut!(magic) }
}

fn get_bootloader_flag() -> u32 {
    unsafe { magic_mut_ptr().read_volatile() }
}

fn set_bootloader_flag() {
    unsafe { magic_mut_ptr().write_volatile(BOOTLOADER_REQUESTED) };
}

fn clear_bootloader_flag() {
    unsafe { magic_mut_ptr().write_volatile(BOOTLOADER_REQUESTED) };
}

pub fn init() {
    let requested = get_bootloader_flag() == BOOTLOADER_REQUESTED;
    if requested {
        clear_bootloader_flag();
        jump_to_bootloader();
    }
}

pub fn reboot_to_bootloader() -> ! {
    defmt::info!("Rebooting into the bootloader");
    set_bootloader_flag();
    cortex_m::peripheral::SCB::sys_reset();
}