Path finding

merging CJ's pathfinding