Added stuff for pathfinding

Just needs un commented and changed to fit the node class