Pathfinding algorithm

New test classe, semi-working ptahfinding