Got Planner and States working. Heuristic needs work