almost done implementation of pathfinding, debugging needed