#include <c_IndexMaxPQ.h>

#include <c_IndexMaxPQ.h>
#include <c_Memory.h>

/**
 * Internal helper to swap two positions inside the heap structures.
 * Keeps the inverted index (qp) tightly synchronized.
 */
C_STATIC_FORCE_INLINE
void c_IndexMaxPQ_Swap(c_IndexMaxPQ_t* pq_inst, c_size_t i, c_size_t j) {
    long long temp_pq = pq_inst->pq[i];
    pq_inst->pq[i] = pq_inst->pq[j];
    pq_inst->pq[j] = temp_pq;

    pq_inst->qp[pq_inst->pq[i]] = (long long)i;
    pq_inst->qp[pq_inst->pq[j]] = (long long)j;
}

/**
 * Sift-Up Operational Core for Max-Heap
 */
C_STATIC_FORCE_INLINE
void c_IndexMaxPQ_SiftUp(c_IndexMaxPQ_t* pq_inst, c_size_t current) {
    char* keys_arr = (char*)pq_inst->keys;
    c_size_t es = pq_inst->element_size;

    while (current > 0) {
        c_size_t parent = (current - 1) / 2;

        const void* current_key = keys_arr + (pq_inst->pq[current] * es);
        const void* parent_key = keys_arr + (pq_inst->pq[parent] * es);

        // Max-Heap condition: Break if current element is less than or equal to its parent
        if (pq_inst->compar(current_key, parent_key) <= 0) {
            break;
        }
        c_IndexMaxPQ_Swap(pq_inst, current, parent);
        current = parent;
    }
}

/**
 * Sift-Down Operational Core for Max-Heap
 */
C_STATIC_FORCE_INLINE
void c_IndexMaxPQ_SiftDown(c_IndexMaxPQ_t* pq_inst, c_size_t current) {
    char* keys_arr = (char*)pq_inst->keys;
    c_size_t es = pq_inst->element_size;

    while (1) {
        c_size_t left_child = (2 * current) + 1;
        c_size_t right_child = (2 * current) + 2;
        c_size_t largest = current;

        // Max-Heap condition: Target the larger of the two children to sift down
        if (left_child < pq_inst->size) {
            if (pq_inst->compar(keys_arr + (pq_inst->pq[left_child] * es), keys_arr + (pq_inst->pq[largest] * es)) > 0) {
                largest = left_child;
            }
        }
        if (right_child < pq_inst->size) {
            if (pq_inst->compar(keys_arr + (pq_inst->pq[right_child] * es), keys_arr + (pq_inst->pq[largest] * es)) > 0) {
                largest = right_child;
            }
        }

        if (largest == current) {
            break;
        }
        c_IndexMaxPQ_Swap(pq_inst, current, largest);
        current = largest;
    }
}

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

c_err_t c_IndexMaxPQ_Init(c_IndexMaxPQ_t* pq_inst, c_size_t max_items, c_size_t element_size,
                          int (*compar)(const void*, const void*)) {
    if (pq_inst == NULL || max_items == 0 || element_size == 0 || compar == NULL) return C_ERR_PARAM;

    pq_inst->max_items = max_items;
    pq_inst->element_size = element_size;
    pq_inst->size = 0;
    pq_inst->compar = compar;

    pq_inst->keys = C_ALLOC(max_items * element_size);
    pq_inst->pq = (long long*)C_ALLOC(max_items * sizeof(long long));
    pq_inst->qp = (long long*)C_ALLOC(max_items * sizeof(long long));

    if (pq_inst->keys == NULL || pq_inst->pq == NULL || pq_inst->qp == NULL) {
        C_FREE(pq_inst->keys); C_FREE(pq_inst->pq); C_FREE(pq_inst->qp);
        return C_ERR_NOMEM;
    }

    for (c_size_t i = 0; i < max_items; i++) {
        pq_inst->qp[i] = -1;
    }

    return C_ERR_OK;
}

void c_IndexMaxPQ_Destroy(c_IndexMaxPQ_t* pq_inst) {
    if (pq_inst) {
        C_FREE(pq_inst->keys); pq_inst->keys = NULL;
        C_FREE(pq_inst->pq);   pq_inst->pq = NULL;
        C_FREE(pq_inst->qp);   pq_inst->qp = NULL;
        pq_inst->size = 0;
        pq_inst->max_items = 0;
    }
}

c_bool_t c_IndexMaxPQ_Contains(c_IndexMaxPQ_t* pq_inst, c_size_t ext_id) {
    if (pq_inst == NULL || ext_id >= pq_inst->max_items) return C_FALSE;
    return pq_inst->qp[ext_id] != -1;
}

c_err_t c_IndexMaxPQ_Push(c_IndexMaxPQ_t* pq_inst, c_size_t ext_id, const void* element) {
    if (pq_inst == NULL || ext_id >= pq_inst->max_items || element == NULL) return C_ERR_PARAM;
    if (c_IndexMaxPQ_Contains(pq_inst, ext_id)) return C_ERR_ALREADY_EXISTS;

    char* keys_arr = (char*)pq_inst->keys;
    memcpy(keys_arr + (ext_id * pq_inst->element_size), element, pq_inst->element_size);

    c_size_t current = pq_inst->size;
    pq_inst->pq[current] = (long long)ext_id;
    pq_inst->qp[ext_id] = (long long)current;

    pq_inst->size++;

    c_IndexMaxPQ_SiftUp(pq_inst, current);

    return C_ERR_OK;
}

c_err_t c_IndexMaxPQ_Pop(c_IndexMaxPQ_t* pq_inst, c_size_t* out_ext_id, void* out_element_buffer) {
    if (pq_inst == NULL) return C_ERR_PARAM;
    if (pq_inst->size == 0) return C_ERR_EMPTY;

    long long max_ext_id = pq_inst->pq[0];
    if (out_ext_id) *out_ext_id = (c_size_t)max_ext_id;

    if (out_element_buffer) {
        char* keys_arr = (char*)pq_inst->keys;
        memcpy(out_element_buffer, keys_arr + (max_ext_id * pq_inst->element_size), pq_inst->element_size);
    }

    c_IndexMaxPQ_Swap(pq_inst, 0, pq_inst->size - 1);

    pq_inst->qp[max_ext_id] = -1;
    pq_inst->size--;

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

    return C_ERR_OK;
}

c_err_t c_IndexMaxPQ_ChangeKey(c_IndexMaxPQ_t* pq_inst, c_size_t ext_id, const void* new_element) {
    if (pq_inst == NULL || ext_id >= pq_inst->max_items || new_element == NULL) return C_ERR_PARAM;
    if (!c_IndexMaxPQ_Contains(pq_inst, ext_id)) return C_ERR_NOT_FOUND;

    char* keys_arr = (char*)pq_inst->keys;
    c_size_t es = pq_inst->element_size;

    memcpy(keys_arr + (ext_id * es), new_element, es);

    c_size_t heap_pos = (c_size_t)pq_inst->qp[ext_id];

    c_IndexMaxPQ_SiftUp(pq_inst, heap_pos);
    c_IndexMaxPQ_SiftDown(pq_inst, heap_pos);

    return C_ERR_OK;
}

void* c_IndexMaxPQ_Peek(c_IndexMaxPQ_t* pq_inst, c_size_t* out_ext_id) {
    if (pq_inst == NULL || pq_inst->size == 0) return NULL;
    if (out_ext_id) *out_ext_id = (c_size_t)pq_inst->pq[0];

    char* keys_arr = (char*)pq_inst->keys;
    return keys_arr + (pq_inst->pq[0] * pq_inst->element_size);
}