Reimplemented bubbleDown, still some work to do.
Fixed nodePrio and bubbleDown functions, so now update function works as expected.