Loads of new path following logic this has not been tested yet and will not work right now