Added waypoints, but no pathfinding yet.

Also fixed small bugs. All in a day's job.