#include "xysdk_common.h"



// 创建新节点
xy_heap_node* xysdk_queue_create_heap_node(void* data, uint8_t priority) 
{
    xy_heap_node* newNode = (xy_heap_node*)xy_common_malloc(sizeof(xy_heap_node));
    if (!newNode) 
    {
        XY_LOG_W("内存分配失败\n");
        return NULL;
    }
    newNode->data = data;
    newNode->priority = priority;
    return newNode;
}

// 初始化优先级队列
void xysdk_queue_init_priority_queue(xy_priority_queue* queue, uint8_t capacity) 
{
    queue->capacity = capacity;
    queue->size = 0;
    queue->nodes = (xy_heap_node**)xy_common_malloc(capacity * sizeof(xy_heap_node*));
    if (!queue->nodes) 
    {
        XY_LOG_W("内存分配失败\n");
        return ;
    }
}

// 交换两个节点
static void xysdk_queue_swap_nodes(xy_heap_node** a, xy_heap_node** b) 
{
    xy_heap_node* temp = *a;
    *a = *b;
    *b = temp;
}

// 堆调整向上
static void xysdk_queue_heapify_up(xy_priority_queue* queue, uint8_t index) 
{
    while (index > 0) 
    {
        uint8_t parentIndex = (index - 1) / 2;
        if (queue->nodes[index]->priority >= queue->nodes[parentIndex]->priority)
        {
            break;
        } 
        xysdk_queue_swap_nodes(&queue->nodes[index], &queue->nodes[parentIndex]);
        index = parentIndex;
    }
}

// 堆调整向下
void xysdk_queue_heapify_down(xy_priority_queue* queue, uint8_t index) 
{
    uint8_t leftChild, rightChild, minIndex;
    while ((leftChild = 2 * index + 1) < queue->size) 
    {
        rightChild = leftChild + 1;
        minIndex = index;

        if (queue->nodes[leftChild]->priority < queue->nodes[minIndex]->priority) 
        {
            minIndex = leftChild;
        }

        if (rightChild < queue->size && queue->nodes[rightChild]->priority < queue->nodes[minIndex]->priority)
        {
            minIndex = rightChild;
        }

        if (minIndex == index) 
        {
            break;
        }

        xysdk_queue_swap_nodes(&queue->nodes[index], &queue->nodes[minIndex]);
        index = minIndex;
    }
}

// 插入元素
int xysdk_queue_append(xy_priority_queue* queue, void* data, uint8_t priority) 
{
    if (queue->size >= queue->capacity) 
    {
        XY_LOG_W("队列已满\n");
        return -1;
    }

    xy_heap_node* newNode = xysdk_queue_create_heap_node(data, priority);
    queue->nodes[queue->size] = newNode;
    xysdk_queue_heapify_up(queue, queue->size);
    queue->size++;
    return 0;
}

// 删除元素
void* xysdk_queue_delete(xy_priority_queue* queue) 
{
    if (queue->size == 0) 
    {
        XY_LOG_W("队列为空\n");
        return NULL;
    }

    void* retData = queue->nodes[0]->data;
    xy_common_free((void**)&queue->nodes[0]);
    queue->nodes[0] = queue->nodes[--queue->size];
    xysdk_queue_heapify_down(queue, 0);

    return retData;
}

// 打印优先级队列内容（假设存储的是整数）
void xysdk_queue_print(xy_priority_queue* queue, void (*printFunc)(void*)) 
{
    for (uint8_t i = 0; i < queue->size; i++) 
    {
        printFunc(queue->nodes[i]->data);
        XY_LOG_I("(优先级: %d) ", queue->nodes[i]->priority);
    }
}