Implemented parts 1 and 2 of milestone 3, FSM with 3 states using navmeshes