Pathfinding update

Implemented Entity Iterator
Fixed Bugs in Entity Iterator
Fixed Bugs in pathfinding
Need to find and fix more bugs