Fixed bug in pathfinding and some improvements, but still buggy