#include <c_MinPQ.h>
#include <c_Memory.h>

/**
 * Internal helper to swap two memory slots inside the heap array.
 */
C_STATIC_FORCE_INLINE
void c_MinPQ_HeapSwap(char* arr, c_size_t idx1, c_size_t idx2, c_size_t size, void* temp) {
    if (idx1 == idx2) return;
    char* a = arr + (idx1 * size);
    char* b = arr + (idx2 * size);
    memcpy(temp, a, size);
    memcpy(a, b, size);
    memcpy(b, temp, size);
}

c_err_t c_MinPQ_Init(c_MinPQ_t* pq, c_size_t initial_capacity, c_size_t element_size,
                     int (*compar)(const void*, const void*)) {
    if (pq == NULL || element_size == 0 || compar == NULL) return C_ERR_PARAM;

    pq->capacity = (initial_capacity > 0) ? initial_capacity : 4;
    pq->element_size = element_size;
    pq->size = 0;
    pq->compar = compar;
    pq->data = C_ALLOC(pq->capacity * element_size);

    return (pq->data != NULL) ? C_ERR_OK : C_ERR_NOMEM;
}

void c_MinPQ_Destroy(c_MinPQ_t* pq) {
    if (!pq) return;
    C_FREE(pq->data);
    pq->size = 0;
    pq->capacity = 0;
}

c_err_t c_MinPQ_Clear(c_MinPQ_t* pq) {
    if (pq == NULL) return C_ERR_PARAM;
    pq->size = 0;
    return C_ERR_OK;
}

c_err_t c_MinPQ_Resize(c_MinPQ_t* pq, c_size_t new_capacity) {
    if (pq == NULL || new_capacity < pq->size) return C_ERR_PARAM;
    if (new_capacity == pq->capacity) return C_ERR_OK;

    if (new_capacity == 0) {
        if (pq->data != NULL) C_FREE(pq->data);
        pq->data = NULL;
        pq->capacity = 0;
        return C_ERR_OK;
    }

    void* new_data = C_ALLOC(new_capacity * pq->element_size);
    if (new_data == NULL) return C_ERR_NOMEM;

    if (pq->size > 0 && pq->data != NULL) {
        memcpy(new_data, pq->data, pq->size * pq->element_size);
    }

    if (pq->data != NULL) C_FREE(pq->data);
    pq->data = new_data;
    pq->capacity = new_capacity;

    return C_ERR_OK;
}

c_err_t c_MinPQ_Push(c_MinPQ_t* pq, const void* element) {
    if (pq == NULL || element == NULL) return C_ERR_PARAM;

    char* arr = (char*)pq->data;

    // Handle dynamic capacity auto-doubling
    if (pq->size >= pq->capacity) {
        c_err_t err = c_MinPQ_Resize(pq, pq->capacity * 2);
        if (err != C_ERR_OK) return err;
        arr = (char*)pq->data;
    }

    #define PQ_STACK_LIMIT 128
    char stack_buf[PQ_STACK_LIMIT];
    void* temp = (pq->element_size <= PQ_STACK_LIMIT) ? stack_buf : C_ALLOC(pq->element_size);
    if (temp == NULL) return C_ERR_NOMEM;

    // Place element at the next available leaf position
    c_size_t current = pq->size;
    memcpy(arr + (current * pq->element_size), element, pq->element_size);
    pq->size++;

    // Sift-Up for Min-Heap: Move up while element < parent
    while (current > 0) {
        c_size_t parent = (current - 1) / 2;
        if (pq->compar(arr + (current * pq->element_size), arr + (parent * pq->element_size)) >= 0) {
            break;
        }
        c_MinPQ_HeapSwap(arr, current, parent, pq->element_size, temp);
        current = parent;
    }

    if (pq->element_size > PQ_STACK_LIMIT) C_FREE(temp);
    #undef PQ_STACK_LIMIT
    return C_ERR_OK;
}

c_err_t c_MinPQ_Pop(c_MinPQ_t* pq, void* output_buffer) {
    if (pq == NULL) return C_ERR_PARAM;
    if (pq->size == 0) return C_ERR_EMPTY;

    char* arr = (char*)pq->data;

    // Export root minimum element if buffer is provided
    if (output_buffer != NULL) {
        memcpy(output_buffer, arr, pq->element_size);
    }

    pq->size--;

    if (pq->size > 0) {
        // Move last leaf to root position
        memcpy(arr, arr + (pq->size * pq->element_size), pq->element_size);

        #define PQ_STACK_LIMIT 128
        char stack_buf[PQ_STACK_LIMIT];
        void* temp = (pq->element_size <= PQ_STACK_LIMIT) ? stack_buf : C_ALLOC(pq->element_size);
        if (temp == NULL) return C_ERR_NOMEM;

        // Sift-Down for Min-Heap: Swap with the smaller of the two children
        c_size_t current = 0;
        while (1) {
            c_size_t left_child = (2 * current) + 1;
            c_size_t right_child = (2 * current) + 2;
            c_size_t smallest = current;

            if (left_child < pq->size &&
                pq->compar(arr + (left_child * pq->element_size), arr + (smallest * pq->element_size)) < 0) {
                smallest = left_child;
            }
            if (right_child < pq->size &&
                pq->compar(arr + (right_child * pq->element_size), arr + (smallest * pq->element_size)) < 0) {
                smallest = right_child;
            }
            if (smallest == current) {
                break;
            }

            c_MinPQ_HeapSwap(arr, current, smallest, pq->element_size, temp);
            current = smallest;
        }

        if (pq->element_size > PQ_STACK_LIMIT) C_FREE(temp);
        #undef PQ_STACK_LIMIT
    }

    return C_ERR_OK;
}

void* c_MinPQ_Peek(c_MinPQ_t* pq) {
    if (pq == NULL || pq->size == 0) return NULL;
    return pq->data;
}