use core::cmp::Ordering;
use cortex_m::interrupt;
use cortex_m::peripheral::SYST;
use cortex_m_rt::exception;
use atsam3xa_hal::timer::Ms;
use embedded_hal::timer::CountDown;

type TimeValue = u32;

#[derive(Clone, Copy, Eq)]
struct TimerCallback {
    tt: TimeValue,
    f: fn(),
}

impl Ord for TimerCallback {
    fn cmp(&self, other: &Self) -> Ordering {
        self.tt.cmp(&other.tt)
    }
}

impl PartialOrd for TimerCallback {
    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
        Some(self.cmp(other))
    }
}

impl PartialEq for TimerCallback {
    fn eq(&self, other: &Self) -> bool {
        self.tt == other.tt
    }
}

struct Timer {
    systick: atsam3xa_hal::timer::Timer<SYST>,
    t: TimeValue,
    callbacks: [Option<TimerCallback>; 10],
}

impl Timer {
    fn new(mut systick: atsam3xa_hal::timer::Timer<SYST>) -> Timer {
        systick.enable_interrupt();
        Timer {
            systick,
            t: 0,
            callbacks: [None; 10],
        }
    }

    fn add_callback(&mut self, dt: TimeValue, f: fn()) {
        let new_index = match self.callbacks.iter().position(|&x| x.is_none()) {
            Some(i) => i,
            None => panic!("Cannot schedule timer callback; too many callbacks in flight"),
        };

        let tt = self.t + dt;
        self.callbacks[new_index] = Some(TimerCallback { tt, f });
        self.recalculate();
    }

    fn recalculate(&mut self) {
        self.callbacks.sort_by(|&a, &b| {
            match (a, b) {
                (Some(i), Some(j)) => i.cmp(&j),
                (Some(_), None) => Ordering::Less,
                (None, Some(_)) => Ordering::Greater,
                (None, None) => Ordering::Equal,
            }
        });
        if let Some(_) = self.callbacks[0] {
            self.systick.start(1 as Ms);
        } else {
            self.systick.stop();
            self.t = 0;
        }
    }

    fn tick(&mut self) {
        self.t += 1;

        if !self.callbacks[0].is_none() {
            let timer_expired = {
                let tc = self.callbacks[0].as_ref().unwrap();
                tc.tt <= self.t
            };
            if timer_expired {
                let tc = self.callbacks[0].take().unwrap();
                (tc.f)();
                self.recalculate();
            }
        }
    }
}

static mut TIMER: Option<Timer> = None;

pub fn start(systick: atsam3xa_hal::timer::Timer<SYST>) {
    interrupt::free(|_| unsafe {
        TIMER = Some(Timer::new(systick));
    });
}

pub fn schedule(d: TimeValue, f: fn()) {
    interrupt::free(|_| unsafe {
        let timer = TIMER.as_mut().expect("Timer not yet initialized");
        timer.add_callback(d, f);
    });
}

#[exception]
fn SysTick() {
    interrupt::free(|_| unsafe {
        let timer = TIMER.as_mut().expect("Timer not yet initialized");
        timer.tick();
    });
}