Fixed pathfinding. Still need to cleanup code.