Add a new version of the Pathfinder implementation, the only remaining
things are: Calculate G and H value