Almost working copy of pathfinding calculation done seperately from the agents.