Add support for waypoints and routes

I made my best to keep code concise, but don't keep your critics :)