A* error corrections. still not working.
FSM Test. Navmesh + basic waypoint navigation