implemented cost to pathfinding