Basic pathfinding alg working