#include "MaxHeap.h"

static inline void setItem(MaxHeap * heap, const pyWord pos, const pyWord x, const pyWord d) {
    heap->indices[pos] = x;
    heap->distances[pos] = d;
}

static inline void copyItem(MaxHeap * heap, const pyWord dst, const pyWord src) {
    heap->indices[dst] = heap->indices[src];
    heap->distances[dst] = heap->distances[src];
}


void maxHeap_insert(MaxHeap * heap, const pyWord id, const pyReal distance) {
    if(!heap || heap->maxItems <= 0) {
        return;
    }

    if(heap->size < heap->maxItems) {
        pyWord pos = heap->size;
        heap->size ++;
        setItem(heap, pos, id, distance);
        while(pos > 0) {
            const pyWord p = pos / 2;
            if (heap->distances[p] >= heap->distances[pos]) {
                break;
            }
            copyItem(heap, pos, p);
            pos = p;
            setItem(heap, pos, id, distance);
        }
        return;
    }

    if(distance > heap->distances[0]) {
        return;
    }

    setItem(heap, 0, id, distance);

    pyWord pos = 0;
    while(pos < heap->size) {
        const pyWord right = (pos + 1) * 2;
        const pyWord left = right - 1;

        if(left >= heap->size) {
            break;
        }

        const pyWord v = (right >= heap->size || heap->distances[right] < heap->distances[left]) ? left : right;

        if(distance >= heap->distances[v]) {
            break;
        }

        copyItem(heap, pos, v);
        setItem(heap, v, id, distance);
        pos = v;
    }
}