Lots of work!

Got a bunch of it working.
TODO Make the nextroute function actually function :P