Heap implementation, A* and BCU optmized