Rough working version of pathfinder AI