Fix the cost function and add a trivial heuristic
Add the value of the heuristic function to the state repr