Added tests for waypoint calculations