Fixed bugs related to heuristic in planner. Passed sanity checks.