Findpath functionality added

Heads up, this code needs to be edited to work with the current main