Lots of changes and additions for pathfinding