#include "watchos.h"

Queue::Queue()
{
	for (int i = 0; i < QUEUE_RING_SIZE; i++)
	{
		m_item[i] = WATCHOS_HANDLE_NULL;
	}
}

void Queue::push(kernel_handle_t item)
{
	if (m_full)
	{
		watchos::panic("Cannot enqueue item: ring buffer full");
	}

	// Write at m_next
	m_item[m_write] = item;

	// Move the write position up
	m_write = next(m_write);

	// If the next position to write has looped back around to the next position
	// to read, then the buffer is full
	if (m_read == m_write)
	{
		m_full = true;
	}
}

kernel_handle_t Queue::peek()
{
	// If the next position to read and write match but the buffer isn't full,
	// then the buffer is empty
	if (m_read == m_write && !m_full)
	{
		watchos::panic("Cannot dequeue event: ring buffer empty");
	}

	return m_item[m_read];
}

kernel_handle_t Queue::pop()
{
	// If the next position to read and write match but the buffer isn't full,
	// then the buffer is empty
	if (m_read == m_write && !m_full)
	{
		watchos::panic("Cannot dequeue event: ring buffer empty");
	}

	kernel_handle_t nextItem = m_item[m_read];
	// Move to the next read position
	m_read = next(m_read);
	// We can never be full if we just removed an item
	m_full = false;

	return nextItem;
}

bool Queue::isEmpty()
{
	return m_read == m_write && !m_full;
}

int Queue::next(int n)
{
	if (n + 1 == QUEUE_RING_SIZE)
		return 0;
	else
		return n + 1;
}