#include <c_MaxPQ.h>
#include <c_Memory.h>


/**
 * Internal macro helper to swap two arbitrary blocks of memory.
 */
C_STATIC_FORCE_INLINE
void c_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_MaxPQ_Init(c_MaxPQ_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);

    if (pq->data == NULL) return C_ERR_NOMEM;
    return C_ERR_SUCCESS;
}

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

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

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

    // Capacity check: Scale memory boundary out if full
    if (pq->size >= pq->capacity) {
        c_size_t new_capacity = pq->capacity * 2;
        // Reallocate manually utilizing framework macros
        void* new_data = C_ALLOC(new_capacity * pq->element_size);
        if (new_data == NULL) return C_ERR_NOMEM; // Allocation failure block

        memcpy(new_data, pq->data, pq->size * pq->element_size);
        C_FREE(pq->data);
        pq->data = new_data;
        pq->capacity = new_capacity;
        arr = (char*)pq->data;
    }

    // Allocate stack cache buffer for object swapping routines
#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 new element at the bottom-most leaf slot of the max-heap tree
    c_size_t current = pq->size;
    memcpy(arr + (current * pq->element_size), element, pq->element_size);
    pq->size++;

    // Sift-Up loop processing
    while (current > 0) {
        c_size_t parent = (current - 1) / 2;

        // Max-heap rule tracking: If child <= parent, tree balancing properties are correct
        if (pq->compar(arr + (current * pq->element_size), arr + (parent * pq->element_size)) <= 0) {
            break;
        }

        c_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_MaxPQ_Pop(c_MaxPQ_t* pq, void* output_buffer) {
    if (!pq) return C_ERR_PARAM;

    if (pq->size == 0) return C_ERR_EMPTY;

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

    // If a tracking output buffer pointer is supplied, export the maximum item
    if (output_buffer != NULL) {
        memcpy(output_buffer, arr, pq->element_size);
    }

    // Shrink element count tracking early
    pq->size--;

    if (pq->size > 0) {
        // Swap the last leaf node up 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 balancing loop processing
        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 largest = current;

            // Check if left child is larger than current node
            if (left_child < pq->size &&
                pq->compar(arr + (left_child * pq->element_size), arr + (largest * pq->element_size)) > 0) {
                largest = left_child;
                }

            // Check if right child is larger than the currently tracked largest node
            if (right_child < pq->size &&
                pq->compar(arr + (right_child * pq->element_size), arr + (largest * pq->element_size)) > 0) {
                largest = right_child;
                }

            // Balanced condition achieved
            if (largest == current) {
                break;
            }

            c_HeapSwap(arr, current, largest, pq->element_size, temp);
            current = largest;
        }

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

    return C_ERR_SUCCESS;
}

void* c_MaxPQ_Peek(c_MaxPQ_t* pq) {
    if (pq == NULL || pq->size == 0) return NULL;
    return pq->data; // Root node is consistently maximum element
}

c_err_t c_MaxPQ_Clear(c_MaxPQ_t* pq) {
    if (pq == NULL) return C_ERR_PARAM;

    // Simply reset the size to zero. The underlying buffer remains allocated.
    pq->size = 0;

    return C_ERR_OK;
}

/**
 * Manually resize the memory allocation capacity of the Priority Queue.
 * @param pq            Pointer to the Max Priority Queue instance.
 * @param new_capacity  The desired number of element slots to allocate.
 * @return              C_ERR_OK if successful, C_ERR_INVALID for bad arguments,
 *                      or C_ERR_NOMEM if memory allocation fails.
 */
c_err_t c_MaxPQ_Resize(c_MaxPQ_t* pq, c_size_t new_capacity) {
    if (pq == NULL) return C_ERR_PARAM;

    // Prevent shrinking below the current number of active elements inside the heap
    if (new_capacity < pq->size) return C_ERR_PARAM;

    // If the capacity is already identical, skip processing to avoid memory overhead
    if (new_capacity == pq->capacity) return C_ERR_OK;

    // Handle downsizing down to 0 safely if the queue is empty
    if (new_capacity == 0) {
        if (pq->data != NULL) {
            C_FREE(pq->data);
            pq->data = NULL;
        }
        pq->capacity = 0;
        return C_ERR_OK;
    }

    // Allocate a new memory block according to your framework specification
    void* new_data = C_ALLOC(new_capacity * pq->element_size);
    if (new_data == NULL) return C_ERR_NOMEM;

    // If there are existing active elements, move them to the newly allocated block
    if (pq->size > 0 && pq->data != NULL) {
        memcpy(new_data, pq->data, pq->size * pq->element_size);
    }

    // Free the old array and bind the new tracking parameters
    if (pq->data != NULL) {
        C_FREE(pq->data);
    }
    pq->data = new_data;
    pq->capacity = new_capacity;

    return C_ERR_OK;
}