#include "wakequeue.h"

/*
TODO: This is unused at the moment. It will eventually be used for keeping
track of which device last woke a Bedrock instance, to optimally choose
which device to use when waking from sleep.
*/

// Reset a wake queue.
void wakequeue_reset(WakeQueue *queue) {
    // Populate the queue with fifteen slot numbers. Slot zero is excluded,
    // as it always comes last in the wake order.
    for (int i=0; i<15; i++) queue->mem[i] = i+1;
}

// TODO: Not quite done yet.
bool wakequeue_wake(WakeQueue *queue, u16 device_list) {
    // Test each entry in the queue against the device list.
    for (int i=0; i<15; i++) {
        u8 slot_number = queue->mem[i];
        if (TEST(device_list, 0x8000 >> slot_number)) {
            // Move the matched entry to the back of the queue.
            for (; i<14; i++) queue->mem[i] = queue->mem[i+1];
            queue->mem[14] = slot_number;
            return true;
        }
    }
    // Test system device last.
    return TEST(device_list, 0x8000);
}