Fixed heap. The problem was on pop(). Have to implement MST now