#include <c_MaxPQ.h>

/**
 * @brief 内部原子操作：泛型就地物理内存块互换
 */
C_STATIC_FORCE_INLINE
void c_MaxPQ_InternalSwap(void* a, void* b, c_size_t size) {
    if (a == b) return;
    char* p1 = (char*)a;
    char* p2 = (char*)b;
    char temp_buf[256];
    c_size_t bytes_left = size;
    while (bytes_left > 0) {
        c_size_t chunk = (bytes_left < sizeof(temp_buf)) ? bytes_left : sizeof(temp_buf);
        memcpy(temp_buf, p1, chunk);
        memcpy(p1, p2, chunk);
        memcpy(p2, temp_buf, chunk);
        p1 += chunk;
        p2 += chunk;
        bytes_left -= chunk;
    }
}


C_STATIC_FORCE_INLINE
void c_MaxPQ_SiftUp(c_MaxPQ_t* pq, c_size_t child) {
    char* array = pq->data;
    c_size_t es = pq->elem_size;
    while (child > 0) {
        c_size_t parent = (child - 1) >> 1;
        char* p_child  = array + (child * es);
        char* p_parent = array + (parent * es);
        if (pq->cmp(p_child, p_parent, pq->args) > 0) {
            c_MaxPQ_InternalSwap(p_child, p_parent, es);
            child = parent;
        } else {
            break;
        }
    }
}


static void c_MaxPQ_SiftDown(c_MaxPQ_t* pq, c_size_t parent) {
    char* array = pq->data;
    c_size_t es = pq->elem_size;
    c_size_t num = pq->size;
    while (1) {
        c_size_t left_child = (parent << 1) + 1;
        if (left_child >= num) break;

        c_size_t larger_child = left_child;
        c_size_t right_child = left_child + 1;
        if (right_child < num) {
            char* p_left  = array + (left_child * es);
            char* p_right = array + (right_child * es);
            if (pq->cmp(p_right, p_left, pq->args) > 0) {
                larger_child = right_child;
            }
        }
        char* p_parent = array + (parent * es);
        char* p_target = array + (larger_child * es);
        if (pq->cmp(p_target, p_parent, pq->args) > 0) {
            c_MaxPQ_InternalSwap(p_parent, p_target, es);
            parent = larger_child;
        } else {
            break;
        }
    }
}

/**
 * @brief 优先队列初始化开辟 (通过用户传入的分配器接管堆空间)
 *
 * @param allocator 用户自制的分配器指针（不能为 NULL）
 */
c_err_t c_MaxPQ_Init(c_MaxPQ_t* self, c_size_t initial_capacity, c_size_t elem_size, c_SortCompare_t cmp, void* args, c_Allocator_t* allocator) {
    if (!self || elem_size == 0 || !cmp ) return C_ERR_PARAM;

    c_size_t cap = (initial_capacity > 0) ? initial_capacity : 4;
    self->allocator = (allocator!=NULL)?*allocator:c_DefaultAllocator;

    // 2. 利用分配器分配底层数据承载连续数组
    self->data = (char*)c_Allocator_Alloc(&self->allocator, cap * elem_size);
    if (!self->data) {
        return C_ERR_NOMEM;
    }

    self->capacity = cap;
    self->size = 0;
    self->elem_size = elem_size;
    self->cmp = cmp;
    self->args = args;

    return C_ERR_OK;
}


/**
 * @brief 向优先队列中压入一个元素（通过 c_Allocator_Realloc 自适应动态翻倍扩容）
 */
c_err_t c_MaxPQ_Push(c_MaxPQ_t* pq, const void* item) {
    if (!pq || !item) return C_ERR_PARAM;

    if (pq->size >= pq->capacity) {
        c_size_t old_cap = pq->capacity;
        c_size_t new_cap = old_cap << 1;

        // 核心加固：改用分配器的托管 Realloc 代替原生 realloc
        c_size_t old_bytes = old_cap * pq->elem_size;
        c_size_t new_bytes = new_cap * pq->elem_size;
        char* new_data = (char*)c_Allocator_Realloc(&pq->allocator, pq->data, old_bytes, new_bytes);
        if (!new_data) return C_ERR_NOMEM; // 扩容失败保护

        pq->data = new_data;
        pq->capacity = new_cap;
    }

    char* target_slot = pq->data + (pq->size * pq->elem_size);
    memcpy(target_slot, item, pq->elem_size);

    c_MaxPQ_SiftUp(pq, pq->size);
    pq->size++;
    return C_ERR_OK;
}

c_err_t c_MaxPQ_Pop(c_MaxPQ_t* pq, void* out_item) {
    if (!pq || pq->size == 0) return C_ERR_PARAM;
    char* array = pq->data;
    c_size_t es = pq->elem_size;
    if (out_item) {
        memcpy(out_item, array, es);
    }
    pq->size--;
    if (pq->size > 0) {
        memcpy(array, array + (pq->size * es), es);
        c_MaxPQ_SiftDown(pq, 0);
    }
    return C_ERR_OK;
}

c_err_t c_MaxPQ_Peek(const c_MaxPQ_t* pq, void* out_item) {
    if (!pq || pq->size == 0 || !out_item) return C_ERR_PARAM;
    memcpy(out_item, pq->data, pq->elem_size);
    return C_ERR_OK;
}

/**
 * @brief 优先队列彻底解构销毁 (通过当时绑定的分配器实例，原路进行内存逆向回收)
 */
void c_MaxPQ_Destroy(c_MaxPQ_t* pq) {
    if (pq && pq->data) {
        c_Allocator_Free(&pq->allocator, pq->data);
        pq->data = NULL;
    }
}