#include <c_IndexMaxPQ.h>

/**
 * @brief 内部静态原子操作：双向路由置换
 */
C_STATIC_FORCE_INLINE
void c_IMPQ_Swap(c_IndexMaxPQ_t* pq, c_size_t i, c_size_t j) {
    c_size_t swap_index_i = pq->pq[i];
    c_size_t swap_index_j = pq->pq[j];

    pq->pq[i] = swap_index_j;
    pq->pq[j] = swap_index_i;

    pq->qp[swap_index_i] = j;
    pq->qp[swap_index_j] = i;
}

/**
 * @brief 内部静态辅助：根据堆物理位置提取真实 keys 空间对应数据的物理指针
 */
C_STATIC_FORCE_INLINE
char* c_IMPQ_GetKeyByHeapPos(const c_IndexMaxPQ_t* pq, c_size_t heap_pos) {
    c_size_t user_idx = pq->pq[heap_pos];
    return pq->keys + (user_idx * pq->elem_size);
}


static void c_IndexMaxPQ_SiftUp(c_IndexMaxPQ_t* pq, c_size_t child) {
    // 堆物理下标从 1 开始，子父级自减收缩终止位置是 1。在无符号数（child > 1）下绝对安全
    while (child > 1) {
        c_size_t parent = child >> 1;
        // 🌟【索引最大堆定义】：若子节点的值“大于”父节点的值，向上置换顶推
        if (pq->cmp(c_IMPQ_GetKeyByHeapPos(pq, child), c_IMPQ_GetKeyByHeapPos(pq, parent), pq->args) > 0) {
            c_IMPQ_Swap(pq, child, parent);
            child = parent;
        } else {
            break;
        }
    }
}

static void c_IndexMaxPQ_SiftDown(c_IndexMaxPQ_t* pq, c_size_t parent) {
    c_size_t num = pq->size;
    while (1) {
        c_size_t left_child = parent << 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) {
            // 🌟【索引最大堆定义】：若右子节点的值比左子节点还“大”，切换最大目标到右半区
            if (pq->cmp(c_IMPQ_GetKeyByHeapPos(pq, right_child), c_IMPQ_GetKeyByHeapPos(pq, left_child), pq->args) > 0) {
                larger_child = right_child;
            }
        }

        // 🌟【索引最大堆定义】：若最大子节点依然“大于”当前的父节点，下沉对调
        if (pq->cmp(c_IMPQ_GetKeyByHeapPos(pq, larger_child), c_IMPQ_GetKeyByHeapPos(pq, parent), pq->args) > 0) {
            c_IMPQ_Swap(pq, parent, larger_child);
            parent = larger_child;
        } else {
            break;
        }
    }
}

/* ------------------------------------------------------------------------------------------------------------------ */
/*  */

/**
 * @brief 就地初始化索引最大堆优先队列
 *
 * @param max_elements 允许传入的最大唯一索引值。这会一次性静态开辟好所需的双向路由映射槽
 */
c_err_t c_IndexMaxPQ_Init(c_IndexMaxPQ_t* self, c_size_t max_elements, c_size_t elem_size, c_SortCompare_t cmp, void* args, c_Allocator_t* allocator) {
    if (!self || max_elements == 0 || elem_size == 0 || !cmp) {
        return C_ERR_PARAM;
    }

    if (allocator) {
        self->allocator = *allocator;
    } else {
        self->allocator = c_DefaultAllocator;
    }

    self->max_elements = max_elements;
    self->size = 0;
    self->elem_size = elem_size;
    self->cmp = cmp;
    self->args = args;

    // 前置逆向除法溢出安全审计：确保三组大空间开辟时不会发生乘法溢出
    if (((c_size_t)-1) / sizeof(c_size_t) < (max_elements + 1)) return C_ERR_NOMEM;
    if (((c_size_t)-1) / elem_size < max_elements) return C_ERR_NOMEM;

    // 分配堆数组 pq（堆下标从 1 开始使用到 max_elements，方便进行二叉父子节点位移运算）
    self->pq = (c_size_t*)c_Allocator_Alloc(&self->allocator, (max_elements + 1) * sizeof(c_size_t));
    // 分配逆向映射表 qp
    self->qp = (c_size_t*)c_Allocator_Alloc(&self->allocator, max_elements * sizeof(c_size_t));
    // 分配密集优先级数据存储块
    self->keys = (char*)c_Allocator_Alloc(&self->allocator, max_elements * elem_size);

    if (!self->pq || !self->qp || !self->keys) {
        if (self->pq)   c_Allocator_Free(&self->allocator, self->pq);
        if (self->qp)   c_Allocator_Free(&self->allocator, self->qp);
        if (self->keys) c_Allocator_Free(&self->allocator, self->keys);
        self->pq = NULL; self->qp = NULL; self->keys = NULL;
        return C_ERR_NOMEM;
    }

    // 将所有 qp 映射槽初始化填充为 (c_size_t)-1（代表未入队列），完美消除下溢舒适区
    memset(self->qp, 0xFF, max_elements * sizeof(c_size_t));
    return C_ERR_OK;
}

/**
 * @brief 检查某个唯一用户索引当前是否在队列中有效存在
 */
bool c_IndexMaxPQ_Contains(const c_IndexMaxPQ_t* pq, c_size_t index) {
    if (!pq || index >= pq->max_elements) return false;
    return pq->qp[index] != (c_size_t)-1;
}


/**
 * @brief 绑定一个唯一的唯一 index 压入优先级关联数据
 */
c_err_t c_IndexMaxPQ_Push(c_IndexMaxPQ_t* pq, c_size_t index, const void* item) {
    if (!pq || !item) return C_ERR_PARAM;
    if (index >= pq->max_elements) return C_ERR_PARAM;
    if (c_IndexMaxPQ_Contains(pq, index)) return C_ERR_EXIST;

    pq->size++;
    pq->pq[pq->size] = index;
    pq->qp[index]    = pq->size;

    memcpy(pq->keys + (index * pq->elem_size), item, pq->elem_size);

    c_IndexMaxPQ_SiftUp(pq, pq->size);
    return C_ERR_OK;
}

/**
 * @brief 弹出当前的绝对最大值元素（堆顶），并将其关联的唯一用户索引（index）通过 out_index 吐出
 */
c_err_t c_IndexMaxPQ_Pop(c_IndexMaxPQ_t* pq, c_size_t* out_index) {
    if (!pq) return C_ERR_PARAM;
    if (pq->size == 0) return C_ERR_EMPTY;

    c_size_t max_idx_result = pq->pq[1];
    if (out_index) {
        *out_index = max_idx_result;
    }

    c_IMPQ_Swap(pq, 1, pq->size);
    pq->size--;

    if (pq->size > 0) {
        c_IndexMaxPQ_SiftDown(pq, 1);
    }

    pq->qp[max_idx_result] = (c_size_t)-1; // 恢复注销状态
    return C_ERR_OK;
}

/**
 * @brief 在常数级时间内寻找并修改某个特定 index 的优先级数据
 */
c_err_t c_IndexMaxPQ_Change(c_IndexMaxPQ_t* pq, c_size_t index, const void* new_item) {
    if (!pq || !new_item || index >= pq->max_elements) return C_ERR_PARAM;
    if (!c_IndexMaxPQ_Contains(pq, index)) return C_ERR_EMPTY;

    memcpy(pq->keys + (index * pq->elem_size), new_item, pq->elem_size);

    c_size_t heap_pos = pq->qp[index];

    c_IndexMaxPQ_SiftUp(pq, heap_pos);
    c_IndexMaxPQ_SiftDown(pq, heap_pos);

    return C_ERR_OK;
}

/**
 * @brief 观察读取但不弹出最大索引堆的堆顶最大关联索引（Index）
 */
c_err_t c_IndexMaxPQ_Peek(const c_IndexMaxPQ_t* pq, c_size_t* out_index) {
    if (!pq || !out_index) return C_ERR_PARAM;
    if (pq->size == 0) return C_ERR_EMPTY;
    *out_index = pq->pq[1];
    return C_ERR_OK;
}

/**
 * @brief 反初始化并释放内存矩阵
 */
void c_IndexMaxPQ_Destroy(c_IndexMaxPQ_t* pq) {
    if (pq) {
        if (pq->pq)   c_Allocator_Free(&pq->allocator, pq->pq);
        if (pq->qp)   c_Allocator_Free(&pq->allocator, pq->qp);
        if (pq->keys) c_Allocator_Free(&pq->allocator, pq->keys);
        pq->pq = NULL; pq->qp = NULL; pq->keys = NULL;
        pq->size = 0; pq->max_elements = 0;
    }
}