pub struct WakeQueue {
    queue: Vec<u8>,
}


impl WakeQueue {
    pub fn new() -> Self {
        Self {
            queue: [0x1,0x2,0x3,0x4,0x5,0x6,0x7,0x8,0x9,0xA,0xB,0xC,0xD,0xE,0xF].into(),
        }
    }

    /// Iterate over all masked devices in last-woken order.
    pub fn iter(&self, mask: u16) -> WakeIterator {
        let mut queue = Vec::new();
        for i in &self.queue {
            if mask & (0x8000 >> i) != 0 {
                queue.push(*i);
            }
        }
        // Add system device last.
        if mask & 0x8000 != 0 {
            queue.push(0);
        }
        WakeIterator { queue, pointer: 0 }
    }

    /// Push a device to the back of the queue.
    pub fn wake(&mut self, id: u8) {
        if let Some(index) = self.queue.iter().position(|e| *e == id) {
            self.queue.remove(index);
            self.queue.push(id);
        }
    }
}


pub struct WakeIterator {
    queue: Vec<u8>,
    pointer: usize,
}

impl Iterator for WakeIterator {
    type Item = u8;

    fn next(&mut self) -> Option<u8> {
        let pointer = self.pointer;
        self.pointer += 1;
        self.queue.get(pointer).copied()
    }
}