#![feature(used)]
#![feature(const_fn)]
#![feature(asm)]
#![no_main]
#![no_std]

extern crate mk66f18;

extern crate cortex_m;
extern crate panic_semihosting;

extern crate cortex_m_rt as rt;
use rt::{entry, pre_init};

#[entry]
fn main() -> ! {
    let sim = mk66f18::SIM::ptr();

    let porta = mk66f18::PORTA::ptr();
    let pta = mk66f18::PTA::ptr();

    let portc = mk66f18::PORTC::ptr();
    let ptc = mk66f18::PTC::ptr();

    let porte = mk66f18::PORTE::ptr();
    let pte = mk66f18::PTE::ptr();

    unsafe {
        // Enable porta, portc and porte
        (*sim).scgc5.modify(|_, w| w.porta().set_bit());
        (*sim).scgc5.modify(|_, w| w.portc().set_bit());
        (*sim).scgc5.modify(|_, w| w.porte().set_bit());

        // set the blue LED pin to output, PORTA[11]
        (*porta).pcr[11].modify(|_, w| w.mux()._001());
        (*pta).pddr.modify(|r, w| w.bits(r.bits() | 0b1 << 11));
        (*pta).pcor.write(|w| w.bits(0b1 << 11));

        // set the red LED pin to output, PORTC[9]
        (*portc).pcr[9].modify(|_, w| w.mux()._001());
        (*ptc).pddr.modify(|r, w| w.bits(r.bits() | 0b1 << 9));
        (*ptc).pcor.write(|w| w.bits(0b1 << 9));

        // set the green LED pin to output, PORTE[6]
        (*porte).pcr[6].modify(|_, w| w.mux()._001());
        (*pte).pddr.modify(|r, w| w.bits(r.bits() | 0b1 << 6));
        (*pte).pcor.write(|w| w.bits(0b1 << 6));
    }

    /*


    // Split GPIO pins
    let mut gpioa = p.GPIOA.split();

    // Setup GPIO pins for built-in blue color of RGB LED
    let mut led = gpioa.pta18.into_push_pull_output();

    */
    loop {
        // Green LED
        unsafe {
            (*pta).pcor.write(|w| w.bits(0b1 << 11));
            (*ptc).psor.write(|w| w.bits(0b1 << 9));
            (*pte).psor.write(|w| w.bits(0b1 << 6));
        }

        for _ in 0..100_000 {
            cortex_m::asm::nop();
        }

        // Red LED
        unsafe {
            (*pta).psor.write(|w| w.bits(0b1 << 11));
            (*ptc).pcor.write(|w| w.bits(0b1 << 9));
            (*pte).psor.write(|w| w.bits(0b1 << 6));
        }

        for _ in 0..100_000 {
            cortex_m::asm::nop();
        }

        // Blue LED
        unsafe {
            (*pta).psor.write(|w| w.bits(0b1 << 11));
            (*ptc).psor.write(|w| w.bits(0b1 << 9));
            (*pte).pcor.write(|w| w.bits(0b1 << 6));
        }

        for _ in 0..100_000 {
            cortex_m::asm::nop();
        }
    }
}

/// Disable MK66F18 watchdog by first unlocking the WDOG_STCTRLH register
#[pre_init]
unsafe fn pre_init() {
    // Disable the watchdog
    // Unlock watchdog by writing 0xc520 and 0xd928 to 0x4007_200E (WDOG_UNLOCK)
    // Dusable watchdog by writing 0x01d2 to 0x4007_2000 (WDOG_STCTRLH)
    asm!("
           movw r0, #0x2000
           movt r0, #0x4005
           movw r1, #0xc520
           strh r1, [r0, #14]
           movw r1, #0xd928
           strh r1, [r0, #14]
           movw r1, #0x01d2
           strh r1, [r0, #0]
           "
           :
           :
           :
           : "volatile");
}