worked on some debugging
Fixed pathfinding