Adding in StateMachine and improvements to Pathfinding