#include "pqueue.h"
#include <stdio.h>
#include <string.h>

priorityqueue_t * pqueue_create() {
    priorityqueue_t* prioq = (priorityqueue_t*)malloc(sizeof(priorityqueue_t));
    prioq->length = 0;
    prioq->maxLength = 0;

    return prioq;
}

void pqueue_insert(priorityqueue_t* pq, char * value, float p) {
    int index = -1;

    if (isFull(pq))increase(pq);

    if (pq->length > 0) {
        if (pq->elements[0]->priority >= p) {
            index = 0;
            memmove(pq->elements + 1, pq->elements, (pq->length) * sizeof(pquentry_t*));
        } else if (pq->elements[pq->length - 1]->priority <= p) {
            index = pq->length;
        } else {
            index = getIndexFromPriority(pq, &p);
            memmove(pq->elements + index + 1, pq->elements + index, (pq->length - index) * sizeof(pquentry_t*));
        }

        if (index < 0) index = pq->length;
    } else index = 0;

    pq->elements[index] = (pquentry_t*)malloc(sizeof(pquentry_t));
    pq->elements[index]->value = value;
    pq->elements[index]->priority = p;
    pq->length++;
}

char* pqueue_extractMin(priorityqueue_t* pq) {
    pquentry_t min;
    pquentry_t *cache;
    if (pq->length < 1) return "List empty!";

    min = *pq->elements[0];
    cache = pq->elements[0];
    pq->elements++;
    pq->length--;
    free(cache);
    return min.value;
}

void pqueue_decreaseKey(priorityqueue_t* pq, char * value, float p) {
    for (int i = 0; i < pq->length; i++) {
        if (pq->elements[i]->value == value) {
            if (i + 1 >= pq->length) {
                free(pq->elements[i]);
            } else {
                memmove(pq->elements + i, pq->elements + i + 1, (pq->length - i - 1) * sizeof(pquentry_t*));
            }
            pq->length--;
            pqueue_insert(pq, value, p);
            return;
        }
    }
}

void pqueue_remove(priorityqueue_t* pq, char* value) {
    int i;
    for (i = 0; i < pq->length; i++) {
        if (pq->elements[i]->value == value) {
            memmove(pq->elements + i, pq->elements + i+1, (pq->length - i - 1) * sizeof(pquentry_t*));

            pq->length--;
        }
    }
}

void pqueue_destroy(priorityqueue_t* pq) {
    int i;
    for (i = 0; i < pq->length; i++) {
        free(pq->elements[i]);
    }

    free(pq);
}

char isFull(priorityqueue_t* pq) {
    return pq->length >= pq->maxLength;
}

void increase(priorityqueue_t * pq) {
    if (pq->maxLength == 0) {
        pq->elements = (pquentry_t**)malloc(1 *sizeof(pquentry_t*));
        pq->maxLength = 1;
    } else {
        pq->elements = (pquentry_t**)realloc(pq->elements, (pq->length * 2) * sizeof(pquentry_t*));
        pq->maxLength = (pq->length * 2);
    }
}

void printQueue(priorityqueue_t* pq) {
    int i;
    printf("(%i/%i) -> {\n", pq->length, pq->maxLength);
    for (i = 0; i < pq->length; i++) {
        printf("    [%i] -> (Wert: %s - Prio: %f)\n",i, pq->elements[i]->value, pq->elements[i]->priority);
    }
    printf("}\n");
}

int getIndexFromPriority(priorityqueue_t* pq, float* p) {
    float s = (float)pq->length / (float)(pq->elements[pq->length - 1]->priority);
    int index = s * (*p);

    if (index < 0) index = 0;
    if (index > (pq->length -1)) index = pq->length - 1;

    int prio = pq->elements[index]->priority;

    if (prio < (*p)) {
        for (int i = index; i < pq->length; i++) {
            if (pq->elements[i]->priority >= (*p)) {
                return i;
            }
        }
    } else if (prio >(*p)) {
        for (int i = index; i >= 0; i--) {
            float pr = pq->elements[i]->priority;
            if (pr <= (*p)) {
                return i + 1;
            }
        }
    } else {
        return index;
    }
    return pq->length;
}