Added waypoints
Fixed some other bugs