#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include "priority_queue.h"


void priority_queueFixheap(priority_queue* q, int i);
void priority_queueHeapify(priority_queue* queue, int i);
void priority_queueSwap(void** a, void** b);

priority_queue* newPriority_queue(bool (*compare)()){
    priority_queue* pq = (priority_queue*)malloc(sizeof(priority_queue));
    pq->size = 0;
    pq->capacity = 0;
    pq->growthFactor = PRIORITY_QUEUE_INIT_GROWTH_FACT;

    pq->compare = compare;
    pq->toString = NULL;

    return pq;
}

void priority_queuePush(priority_queue* pq, void* element){
    if(pq->size == pq->capacity){
        if(pq->capacity)
            priority_queueResize(pq, pq->capacity * pq->growthFactor);
        else
            priority_queueResize(pq, 1);
    }

    pq->at[pq->size] = element;
    pq->size++;

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

}

void priority_queueFixheap(priority_queue* pq, int i){
    if(!i)
        return;
    if(pq->compare(pq->at[(i-1)/2], pq->at[i]))
        priority_queueSwap(pq->at[i], pq->at[(i-1)/2]);

    priority_queueFixheap(pq, (i-1)/2);
}

void priority_queuePop(priority_queue* pq){
    if(!pq->size){
        fprintf(stderr, "POP ON EMPTY PRIORITY_QUEUE\n");
        return;
    }

    priority_queueSwap(pq->at[0], pq->at[pq->size-1]);
    pq->size--;
    priority_queueHeapify(pq, 0);
}

void priority_queueHeapify(priority_queue* pq, int i){
    if(i >= (pq->size)/2)
        return;
    
    int minson;
    if(pq->size-1 == 2*i+1 || !pq->compare(pq->at[2*i+1], pq->at[2*i+2]))
        minson = 2*i+1;
    else
        minson = 2*i+2;

    if(pq->compare(pq->at[i], pq->at[minson])){
        priority_queueSwap(pq->at[i], pq->at[minson]);
        priority_queueHeapify(pq, minson);
    }
}

void priority_queueSwap(void** a, void** b){
    void* tmp = *a;
    *a = *b;
    *b = tmp;
}

void priority_queueResize(priority_queue* pq, int capacity){
    for(int i=pq->size; i>capacity; i--)
        free(pq->at[i-1]);
    
    if(capacity){
        void **at = realloc(pq->at, sizeof(void *) * capacity);
        pq->at = at;
        pq->capacity = capacity;
    }else{
        free(pq->at);
    }
}

void priority_queueClear(priority_queue* pq){
    priority_queueResize(pq, 0);
    pq->size = 0;
}

void priority_queueFree(priority_queue* pq){
    priority_queueClear(pq);
    free(pq);
}

void priority_queueShrink_to_fit(priority_queue* pq){
    priority_queueResize(pq, pq->size);
}

bool priority_queueIsEmpty(priority_queue* pq){
    return pq->size > 0 ? false : true;
}

void* priority_queueTop(priority_queue* pq){
    if(!pq->size){
        fprintf(stderr, "PRIORITY_QUEUE IS EMPTY\n");
        return NULL;
    }

    return pq->at[0];
}

void priority_queuePrint(priority_queue* pq){
    if(pq->toString == NULL){
        fprintf(stderr, "TOSTRING NOT DEFINED\n");
        return;
    }

    for(int i=0; i<pq->size; i++)
        pq->toString(pq->at[i]);
    
    printf("\n");
}