Refactoring and cleanup on some new pathing. Adding larger map tests (need performance improvements on path finding of closest agent when lots of agents are involved.