#include <c_IndexPQ.h>
#include <c_Memory.h>

#define INVALID_INDEX ((c_size_t)-1)

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




// Internal Helper: Swaps two locations inside the heap and updates their inverse map positions
C_STATIC_FORCE_INLINE
void swap_nodes(c_IndexPQ_t* self, c_size_t i, c_size_t j) {
    c_size_t temp = self->heap[i];
    self->heap[i] = self->heap[j];
    self->heap[j] = temp;

    // Synchronize inverse lookups mapping: ID -> heap position index
    self->inverse_heap[self->heap[i]] = i;
    self->inverse_heap[self->heap[j]] = j;
}

// Internal Helper: Balanced restoration path upwards (O(log N))
C_STATIC_FORCE_INLINE
void heapify_up(c_IndexPQ_t* self, c_size_t index) {
    while (index > 0) {
        c_size_t parent = (index - 1) / 2;
        void* curr_val = (char*)self->keys_data + (self->heap[index] * self->obj_size);
        void* parent_val = (char*)self->keys_data + (self->heap[parent] * self->obj_size);

        if (self->compare(curr_val, parent_val) < 0) {
            swap_nodes(self, index, parent);
            index = parent;
        } else {
            break;
        }
    }
}

// Internal Helper: Balanced restoration path downwards (O(log N))
C_STATIC_FORCE_INLINE
void heapify_down(c_IndexPQ_t* self, c_size_t index) {
    c_size_t left, right, smallest;
    while (1) {
        left = 2 * index + 1;
        right = 2 * index + 2;
        smallest = index;

        void* smallest_val = (char*)self->keys_data + (self->heap[smallest] * self->obj_size);

        if (left < self->size) {
            void* left_val = (char*)self->keys_data + (self->heap[left] * self->obj_size);
            if (self->compare(left_val, smallest_val) < 0) {
                smallest = left;
                smallest_val = left_val;
            }
        }

        if (right < self->size) {
            void* right_val = (char*)self->keys_data + (self->heap[right] * self->obj_size);
            if (self->compare(right_val, smallest_val) < 0) {
                smallest = right;
            }
        }

        if (smallest != index) {
            swap_nodes(self, index, smallest);
            index = smallest;
        } else {
            break;
        }
    }
}

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

c_err_t c_IndexPQ_Init(c_IndexPQ_t* self, int obj_size, c_size_t max_size, c_IndexPQ_Compare_f compare) {
    if (!self || obj_size <= 0 || max_size == 0 || !compare) return C_ERR_PARAM;

    self->obj_size = obj_size;
    self->max_size = max_size;
    self->size = 0;
    self->compare = compare;

    self->keys_data = C_ALLOC(self->max_size * self->obj_size);
    self->heap = (c_size_t*)C_ALLOC(self->max_size * sizeof(c_size_t));
    self->inverse_heap = (c_size_t*)C_ALLOC(self->max_size * sizeof(c_size_t));

    if (!self->keys_data || !self->heap || !self->inverse_heap) {
        c_IndexPQ_Destroy(self);
        return C_ERR_NOMEM;
    }

    // Initialize inverse heap mappings to an invalid sentinel value representing emptiness
    for (c_size_t i = 0; i < self->max_size; i++) {
        self->inverse_heap[i] = INVALID_INDEX;
    }

    return C_ERR_SUCCESS;
}

void c_IndexPQ_Destroy(c_IndexPQ_t* self) {
    if (!self) return;
    if (self->keys_data) C_FREE(self->keys_data);
    if (self->heap) C_FREE(self->heap);
    if (self->inverse_heap) C_FREE(self->inverse_heap);

    self->keys_data = NULL;
    self->heap = NULL;
    self->inverse_heap = NULL;
    self->max_size = 0;
    self->size = 0;
    self->obj_size = 0;
}

c_bool_t c_IndexPQ_Contains(const c_IndexPQ_t* self, c_size_t id) {
    if (!self || id >= self->max_size) return C_FALSE;
    return self->inverse_heap[id] != INVALID_INDEX;
}

// Inserts an entry associated with a fixed identifier ID into the heap (O(log N))
c_err_t c_IndexPQ_Push(c_IndexPQ_t* self, c_size_t id, const void* obj) {
    if (!self || !obj) return C_ERR_PARAM;
    if (id >= self->max_size) return C_ERR_FULL;
    if (c_IndexPQ_Contains(self, id)) return C_ERR_ALREADY_EXISTS;

    // Map user payload value into the slot indexed directly by ID
    char* slot = (char*)self->keys_data + (id * self->obj_size);
    memcpy(slot, obj, self->obj_size);

    // Place the ID at the end of the active binary heap
    self->heap[self->size] = id;
    self->inverse_heap[id] = self->size;
    self->size++;

    heapify_up(self, self->size - 1);
    return C_ERR_SUCCESS;
}

// Pops the minimum elements out while tracking its associated identifier value (O(log N))
c_err_t c_IndexPQ_Pop(c_IndexPQ_t* self, c_size_t* out_id, void* out_obj) {
    if (!self || !out_id || !out_obj) return C_ERR_PARAM;
    if (self->size == 0) return C_ERR_EMPTY;

    c_size_t min_id = self->heap[0];
    *out_id = min_id;

    // Extract values into safe user-allocated buffers
    void* slot = (char*)self->keys_data + (min_id * self->obj_size);
    memcpy(out_obj, slot, self->obj_size);

    // Swap top node with the last active entry node element
    swap_nodes(self, 0, self->size - 1);
    self->inverse_heap[min_id] = INVALID_INDEX; // Clear historical identifier tracking values
    self->size--;

    if (self->size > 0) {
        heapify_down(self, 0);
    }

    return C_ERR_SUCCESS;
}

// Modifies the priority value for an arbitrary key ID anywhere inside the queue in O(log N) time
c_err_t c_IndexPQ_Change(c_IndexPQ_t* self, c_size_t id, const void* new_obj) {
    if (!self || !new_obj) return C_ERR_PARAM;
    if (!c_IndexPQ_Contains(self, id)) return C_ERR_NOT_FOUND;

    // Overwrite the existing data value
    char* slot = (char*)self->keys_data + (id * self->obj_size);
    memcpy(slot, new_obj, self->obj_size);

    // Retrieve its current heap position to fix the balance path triggers
    c_size_t heap_pos = self->inverse_heap[id];
    heapify_up(self, heap_pos);
    heapify_down(self, heap_pos);

    return C_ERR_SUCCESS;
}

void* c_IndexPQ_PeekValue(c_IndexPQ_t* self) {
    if (!self || self->size == 0) return NULL;
    return (char*)self->keys_data + (self->heap[0] * self->obj_size);
}

long long c_IndexPQ_PeekID(c_IndexPQ_t* self) {
    if (!self || self->size == 0) return -1;
    return (long long)self->heap[0];
}

c_size_t c_IndexPQ_GetSize(const c_IndexPQ_t* self) {
    if (!self) return 0;
    return self->size;
}

c_bool_t c_IndexPQ_IsEmpty(const c_IndexPQ_t* self) {
    if (!self) return C_TRUE;
    return self->size == 0;
}