Pathfinding algorithm

Finally working, needs some adjustments.