#include "pch.h"
#include <stdio.h>



PRIORITYQUEUE_API BOOL InitializePriorityQueue(_Out_ PPRIORITY_QUEUE pPQ)
{
    printf("\n");
	TRACE_FUNCTION_BEGIN();

	if (!pPQ) 
	{
		LOG_ERROR("InitializePriorityQueue called with NULL Object");
		TRACE_FUNCTION_END();
		return FALSE;
	}


	InitializeCriticalSection(&pPQ->Lock);
	// Each level is an independent doubly linked list (with its own lock inside).
	for (int level = 0; level < PRIORITY_LEVELS; level++) 
	{
		InitializeListHead(&pPQ->Levels[level]);
	}

	pPQ->Count = 0;

	LOG_INFO("Priority queue initialized (Levels=%d)", PRIORITY_LEVELS);

	TRACE_FUNCTION_END();
	return TRUE;

}


PRIORITYQUEUE_API BOOL EnqueueWithPriority(_Inout_ PPRIORITY_QUEUE pPQ, _In_ PNODE pNode, _In_ PRIORITY Priority)
{
	TRACE_FUNCTION_BEGIN();

	if (!pPQ || !pNode) 
	{
		LOG_ERROR("EnqueueWithPriority called with NULL parameter (pPQ=%p, pNode=%p)", pPQ, pNode);
		TRACE_FUNCTION_END();
		return FALSE;
	}

	// Check out-of-range priority so we never index invalid values
	if (Priority < 0 || Priority >= PRIORITY_COUNT) 
	{
		LOG_ERROR("EnqueueWithPriority called with invalid priority %d (valid range 0..%d)", Priority, PRIORITY_COUNT - 1);
		TRACE_FUNCTION_END();
		return FALSE;
	}

	// Reuse the intrusive list: append to the matching level.
	EnterCriticalSection(&pPQ->Lock);
	AddTail(&pPQ->Levels[Priority], pNode);
	LONG newCount = InterlockedIncrement(&pPQ->Count);
	LeaveCriticalSection(&pPQ->Lock);

	LOG_TRACE("Enqueued node %p at priority %d (TotalCount=%ld)", pNode, Priority, newCount);

	TRACE_FUNCTION_END();
	return TRUE;
}



PRIORITYQUEUE_API _Ret_maybenull_ PNODE DequeueHighest(_Inout_ PPRIORITY_QUEUE pPQ, _Out_opt_ PRIORITY* pDequeuedPriority)
{
	TRACE_FUNCTION_BEGIN();

	if (!pPQ) 
	{
		LOG_ERROR("DequeueHighest called with NULL pPQ");
		TRACE_FUNCTION_END();
		return NULL;
	}

	// RemoveHead returns NULL for an empty list, so we simply try the next level.
	EnterCriticalSection(&pPQ->Lock);
	for (int level = 0; level < PRIORITY_LEVELS; level++) {
		PNODE pNode = RemoveHead(&pPQ->Levels[level]);

		if (pNode) 
		{
			LONG newCount = InterlockedDecrement(&pPQ->Count);
			LeaveCriticalSection(&pPQ->Lock);

			if (pDequeuedPriority) 
			{
				*pDequeuedPriority = (PRIORITY)level;
			}

			LOG_TRACE("Dequeued node %p from priority %d (TotalCount=%ld)", pNode, level, newCount);

			TRACE_FUNCTION_END();
			return pNode;
		}
	}

	// Every level was empty.
	LeaveCriticalSection(&pPQ->Lock);
	LOG_TRACE("DequeueHighest found the queue empty");
	TRACE_FUNCTION_END();
	return NULL;
}



PRIORITYQUEUE_API BOOL IsPriorityQueueEmpty(_In_ PPRIORITY_QUEUE pPQ)
{
	if (!pPQ) 
	{
		LOG_ERROR("IsPriorityQueueEmpty called with NULL pPQ");
		return TRUE;
	}

	EnterCriticalSection(&pPQ->Lock);
	BOOL empty = (pPQ->Count == 0);
	LeaveCriticalSection(&pPQ->Lock);
	return empty;
}



PRIORITYQUEUE_API ULONG GetPriorityQueueCount(_In_ PPRIORITY_QUEUE pPQ)
{
	if (!pPQ) 
	{
		LOG_ERROR("GetPriorityQueueCount called with NULL pPQ");
		return 0;
	}

	EnterCriticalSection(&pPQ->Lock);
	ULONG count = (ULONG)pPQ->Count;
	LeaveCriticalSection(&pPQ->Lock);
	return count;
}



PRIORITYQUEUE_API VOID CleanupPriorityQueue(_Inout_ PPRIORITY_QUEUE pPQ)
{
    printf("\n");
	TRACE_FUNCTION_BEGIN();

	if (!pPQ) 
	{
		LOG_ERROR("CleanupPriorityQueue called with NULL pPQ");
		TRACE_FUNCTION_END();
		return;
	}

	for (int level = 0; level < PRIORITY_LEVELS; level++) 
	{
		while (RemoveHead(&pPQ->Levels[level])) 
		{
			// Nothing to do with the node - the caller owns its memory.
		}

		DestroyListHead(&pPQ->Levels[level]);
	}

	pPQ->Count = 0;

	DeleteCriticalSection(&pPQ->Lock);
	LOG_INFO("Priority queue cleaned up successfully");
	TRACE_FUNCTION_END();
}