Started creating a seperate pathfinding code based on debugging code