Working point navigation and Djikstras
Code before comments and ommisions