Small tweaks

- Spell checks
- Added prepend function to PathGeometric that adds states to front of path
- Made getOptimizationObjective() const
- Better debug output

In preparation for merging Lightning.