Pathfinding script

There are a few more things to improve but for now it can solve simple
path.