Path finder implemented but not tested yet