Add support for waypoints and routes

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