Add binary heap data structure, also finish two graph algorithm