Dry code - Improved parser and planner

Removed repeated code, added heuristic to planner, and cleaned up some random stuff.