Heap implementation and dynamic programming improvement, also solution 
to optimization problem