use cortex_m::peripheral::MPU;
use atsam3xa_hal::prelude::*;
use crate::console::Console;
use crate::console::serial::SerialTerminal;
use crate::mpu::{self, MemoryRegion, MemoryPermission};
use crate::timer;
use crate::video;

pub struct Peripherals {
    pub led: atsam3xa_hal::pio::Pin,
    pub watchdog: atsam3xa_hal::watchdog::WDT,
}

impl Peripherals {
    pub fn new() -> Peripherals {
        let mut mp = cortex_m::peripheral::Peripherals::take().unwrap();
        let pp = atsam3xa::Peripherals::take().unwrap();

        // Initialize flash wait states
        pp.EFC0.constrain().set_wait_states(4);
        pp.EFC1.constrain().set_wait_states(4);

        // Initialize clocks
        let pmc = pp.PMC.constrain();
        pmc.init_clocks();

        pmc.enable_peripheral_clock(PeripheralID::PIOB);
        pmc.enable_peripheral_clock(PeripheralID::UART);

        let pio_a = pp.PIOA.constrain();
        let pio_b = pp.PIOB.constrain();

        let led = pio_b.get_pin(27);
        led.output_mode();

        let uart = pp.UART.constrain(&pio_a, 115200, ParityType::NO);
        mp.NVIC.enable(atsam3xa::Interrupt::UART);
        unsafe { mp.NVIC.set_priority(atsam3xa::Interrupt::UART, 7 << 4) };
        uart.enable_interrupts();
        Console::new(SerialTerminal::new(uart));
        
        let watchdog = pp.WDT.constrain();

        Self::mpu_setup(mp.MPU);

        timer::start(atsam3xa_hal::timer::Timer::syst(mp.SYST));

        // Only enable video gen on release - debug mode code is too slow
        #[cfg(release)]
        video::gen::start(pp.TC0, &mut mp.NVIC, &pmc, &pio_b);

        Peripherals {
            led,
            watchdog,
        }
    }

    pub fn mpu_setup(mpu: MPU) {
        mpu::init(mpu);

        mpu::add_regions(&[
            // ROM
            MemoryRegion {
                start: 0x00000000,
                length: 0x20000000,
                permissions: MemoryPermission::RO,
                xn: false,
            },
            // Peripherals
            MemoryRegion {
                start: 0x40000000,
                length: 0x20000000,
                permissions: MemoryPermission::RW,
                xn: true,
            },
            // KGRAM
            MemoryRegion {
                start: 0x20080000,
                length: 0x4000,
                permissions: MemoryPermission::RW,
                xn: true,
            },
        ]);
            
        // Enable MPU
        mpu::enable();
    }
}