Lots of changes, refactored the get_waypoint method.
Mostly works