Finished the A* pathfinding and waypoints