//-*-c++-*-
#ifndef INCLUDED_Pilot_h_
#define INCLUDED_Pilot_h_

// Using a single WaypointWalkMC causes problems if we chain a couple
// of WaypointWalkNodes together because some of the LocomotionEvents
// indicating termination of the first one may not arrive until after
// the second node has been activated, causing the second node to
// terminate prematurely.  The only reason to use shared MCs is for
// the Chiara's XWalk, which runs through an elaborate (slow) leg
// initialization sequence on every start().  No other robots have
// this problem, so the Pilot will only use shared motion commands for
// the Chiara.  For everyone else, waypointwalk_id will be invalid_MC_ID.
#ifdef TGT_IS_CHIARA
#  define PILOT_USES_SHARED_MOTION_COMMANDS
#endif

#include <queue>
#include <deque>

// Have to do all our #includes explicitly; can't rely on
// StateMachine.h because of circular dependencies with the Pilot.

#include "Behaviors/Nodes/HeadPointerNode.h"
#include "Behaviors/Nodes/PostMachine.h"
#include "Behaviors/Nodes/SpeechNode.h"
#include "Behaviors/Nodes/WaypointWalkNode.h"
#include "Behaviors/Nodes/WalkNode.h"

#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/PilotTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Behaviors/Transitions/TextMsgTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"

#include "Crew/MapBuilderNode.h"
#include "Crew/PilotRequest.h"

#include "Motion/MMAccessor.h"
#include "Motion/WaypointWalkMC.h"
#include "Motion/WaypointEngine.h"
#include "Motion/WaypointList.h"
#include "Planners/Navigation/ShapeSpacePlannerXY.h"
#include "Planners/Navigation/ShapeSpacePlannerXYTheta.h"

#include "DualCoding/DualCoding.h"

using namespace std;

namespace DualCoding {

using namespace PilotTypes;

$nodeclass Pilot : StateNode : 
    requests(), curReq(NULL), idCounter(0),
    walk_id(MotionManager::invalid_MC_ID), waypointwalk_id(MotionManager::invalid_MC_ID), dispatch_(NULL) {

  typedef unsigned int PilotVerbosity_t;
  static const PilotVerbosity_t PVstart =     1<<0;
  static const PilotVerbosity_t PVevents =    1<<1;
  static const PilotVerbosity_t PVexecute =   1<<2;
  static const PilotVerbosity_t PVsuccess =   1<<3;
  static const PilotVerbosity_t PVfailure =   1<<4;
  static const PilotVerbosity_t PVcomplete =  1<<5;
  static const PilotVerbosity_t PVabort =     1<<6;
  static const PilotVerbosity_t PVpop =       1<<7;
  static const PilotVerbosity_t PVqueued =    1<<8;
  static const PilotVerbosity_t PVcollision = 1<<9;
  static const PilotVerbosity_t PVnavStep =   1<<10;
  static const PilotVerbosity_t PVshowPath =  1<<11;

  static PilotVerbosity_t getVerbosity() { return verbosity; }
  static void setVerbosity(PilotVerbosity_t v) { verbosity = v; }

  struct BearingLessThan {
    AngTwoPi hdg;
    BearingLessThan(const AngTwoPi heading) : hdg(heading) {}

    bool operator()(AngTwoPi a, AngTwoPi b) {
      AngTwoPi aBearing = a - hdg;
      AngTwoPi bBearing = b - hdg;
      return aBearing < bBearing;
    }
  };

  $provide std::vector<ShapeRoot> defaultLandmarks;
  $provide MapBuilderRequest* defaultLandmarkExtractor(NULL);
  $provide Point initPos;
  $provide AngTwoPi initHead;
  $provide PilotRequest planReq;
  $provide NavigationPlan plan;
  $provide AngTwoPi maxTurn(M_PI);
  $provide bool pushingObj;

  virtual void doStart();
  virtual void doStop();
  virtual void doEvent();
  void unwindForNextRequest();
  unsigned int executeRequest(BehaviorBase* requestingBehavior, const PilotRequest& req);
  void executeRequest();
  void requestComplete(PilotTypes::ErrorType_t errorType);
  void requestComplete(const PilotEvent &e);
  void pilotPop();
  void pilotAbort();
  void cancelVelocity(); // forces completion of a setVelocity operation
	void setWorldBounds(float minX, float width, float minY, float height);
  void setWorldBounds(const Shape<PolygonData> &bounds);
	void computeParticleBounds(float widthIncr=0, float heightIncr=0); //!< Compute bounds for random particle distribution based on world shapes

  void setDefaultLandmarks(const std::vector<ShapeRoot> &landmarks);
  const std::vector<ShapeRoot>& getDefaultLandmarks() { return defaultLandmarks; }

  void setDefaultLandmarkExtractor(const MapBuilderRequest &mapreq);
  MapBuilderRequest* getDefaultLandmarkExtractor() const { return defaultLandmarkExtractor; }

  MotionManager::MC_ID getWalk_id() const { return walk_id; }
  MotionManager::MC_ID getWaypointwalk_id() const { return waypointwalk_id; }

  void setupLandmarkExtractor();
  //! Find the landmarks that should be visible from current location
  static std::vector<ShapeRoot> calculateVisibleLandmarks(const DualCoding::Point &currentLocation, 
							  AngTwoPi currentOrientation, AngTwoPi maxTurn,
							  const std::vector<DualCoding::ShapeRoot> &possibleLandmarks);
  static bool isLandmarkViewable(const DualCoding::ShapeRoot &selectedLandmark,
				 const DualCoding::Point &currentLocation, AngTwoPi currentOrientation, AngTwoPi maxTurn);

  //! Returns true if we have enough landmarks to localize.
  /*! This could be much more sophisticated: taking landmark geometry
   into account so we don't settle for two adjacent landmarks if a
   more widely dispersed set is available.
  */
  static bool defaultLandmarkExitTest();

  //! Pilot's setAgent is called by its RunMotionModel behavior.  Or user can call directly.
  /* If @a quiet is false, the new agent position will be announced on
     cout. If @a updateWayPoint is true, the Pilot's WaypointWalk
     engine's position will also be updated.  This doesn't seem to be a good idea,
     as it leads to fights due to the particle filter preventing the Waypoint Walk
     engine from reaching its destination.  Needs further study.
   */
  void setAgent(const Point &loc, AngTwoPi heading, bool quiet=false, bool updateWaypoint=true);

  //================ Background processes ================

  //! Run the particle filter's motion model every few hundred milliseconds to provide odometry
  $nodeclass RunMotionModel :  StateNode {
    virtual void doStart();
    virtual void doEvent();
  }

  //! Collision checker service; its behavior is model-dependent.
  /*! On the Create and Calliope it uses the bump switches and motor torques
   */
  $nodeclass CollisionChecker : StateNode : overcurrentCounter(0) {
    virtual void doStart();
    virtual void doEvent();
    virtual void enableDetection();
    virtual void disableDetection(unsigned int howlong);
    virtual void reportCollision();
    int overcurrentCounter; //!< count Create overcurrent button events to eliminate transients
    static unsigned int const collisionEnableTimer = 1;
    static unsigned int const overcurrentResetTimer = 2;
  }

  //================ Utility Classes Used By Pilot State Machines ================

  $nodeclass ReportCompletion(PilotTypes::ErrorType_t errorType=noError) : StateNode : doStart {
		VRmixin::pilot->requestComplete(errorType);
  }

  //! Used by Pilot walk machines to decide how to respond to a collision
  $nodeclass CollisionDispatch : StateNode {
    virtual void doStart();
    virtual void doEvent();
  }

  //! Terminate a walk operation and indicate that a collision was responsible
  $nodeclass TerminateDueToCollision : StateNode {
    virtual void doStart();
  }

  $nodeclass SetupLandmarkExtractor : VisualRoutinesStateNode : doStart {
    VRmixin::pilot->setupLandmarkExtractor();
  }

  $nodeclass PlanPath : VisualRoutinesStateNode {
    virtual void doStart() {
      $reference Pilot::plan, Pilot::initPos, Pilot::initHead, Pilot::planReq;
      plan.clear();
      doPlan(plan, initPos, initHead, planReq);
    }
    void doPlan(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest &req);
  }

  //! Construct a navigation plan: mixture of moving, turning, and localizing steps
  $nodeclass ConstructNavPlan : VisualRoutinesStateNode {
    virtual void doStart() {
      $reference Pilot::plan, Pilot::initPos, Pilot::initHead, Pilot::planReq;
      doAnalysis(plan, initPos, initHead, planReq);
    }
    void doAnalysis(NavigationPlan &plan, Point initPos, AngTwoPi initHead, PilotRequest &req);
  }

  //! Execute a navigation plan; used for both goToShape and pushObject
  $nodeclass ExecutePlan : VisualRoutinesStateNode {
    $provide DisplacementInstruction nextDisplacementInstruction;
    $provide float boxX;
    $provide float acquireDist;

    //! distance in mm we can be from a destination point and be "there"
    static const float allowableDistanceError;
    static const float allowableAngularError;

    //! Each movement step in a navigation plan is a DisplacementInstruction
    struct DisplacementInstruction {
      DisplacementInstruction() : nextPoint(), angleToTurn(0) {};
      fmat::Column<2> nextPoint;
      AngSignPi angleToTurn;
    };

    enum TurnAdjustType_t {
      collisionAdjust,
      localizationAdjust
    };

    $nodeclass NextTask : VisualRoutinesStateNode : doStart {
      $reference Pilot::plan;
      if ( ++plan.currentStep == plan.steps.end() )
	postStateCompletion();  // indicate that we're done
      else
	postStateSuccess();     // indicate that we have a task to perform
    }

    $nodeclass Walk : WaypointWalkNode : doStart {
      PilotRequest &req = *VRmixin::pilot->curReq;
      $reference ExecutePlan::nextDisplacementInstruction;

      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();

#ifdef TGT_IS_CREATE
      float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 50.f;
      float va = (req.turnSpeed > 0) ? req.turnSpeed : 1.0f;
#else
      float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 15.f;
      float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
#endif
      wp_acc->addEgocentricWaypoint(nextDisplacementInstruction.nextPoint.norm(), 0,  0, true, vx, va);

      motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);				
    }

    $nodeclass Turn : WaypointWalkNode : doStart {
      PilotRequest &req = *VRmixin::pilot->curReq;
      $reference ExecutePlan::nextDisplacementInstruction;

      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();
      // std::cout<<"turning \n";

#ifdef TGT_IS_CREATE
      float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 50.f;
      float va = (req.turnSpeed > 0) ? req.turnSpeed : 1.0f;
      wp_acc->addEgocentricWaypoint(0, 0, nextDisplacementInstruction.angleToTurn, true, vx, va);
#else
      // these values are for Chiara
      float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 15.f;
      float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
      wp_acc->addEgocentricWaypoint(0, 0,nextDisplacementInstruction.angleToTurn, true, vx, va);
#endif
    }

    $nodeclass TurnObj : WaypointWalkNode : doStart {
      $reference ExecutePlan::nextDisplacementInstruction, ExecutePlan::acquireDist;
      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();

      const float backup_step_size = 300;  // back up from the box
      wp_acc->addEgocentricWaypoint(-backup_step_size, 0, M_PI, true, 50.f, 1.f );

      float theta = nextDisplacementInstruction.angleToTurn;
      float abstheta = fabs(theta);
      float hyp = (backup_step_size + 100)/cos(abstheta);
      float x = (backup_step_size + 100) * tan(abstheta);
      float step2 = (backup_step_size/tan(abstheta)) - 100;
      float d = 500 / sin(abstheta);

      cout << "*************** TURN PUSH BOX *****************     " << nextDisplacementInstruction.angleToTurn << "  ******************" << endl;
      if (theta >= 0) {
	//Turn Right
	wp_acc->addEgocentricWaypoint(0, 0, -M_PI/2.f, true, 50.f, 1.0f );
	if (x > 500) {
	  wp_acc->addEgocentricWaypoint(500, 0, 0, true, 50.f, 1.0f );
	  wp_acc->addEgocentricWaypoint(0, 0, M_PI/2.f, true, 50.f, 1.0f );
	  wp_acc->addEgocentricWaypoint(backup_step_size - step2, 0, 0, true, 50.f, 1.0f );
	  wp_acc->addEgocentricWaypoint(0, 0, abstheta, true, 50.f, 1.0f );
	  // wp_acc->addEgocentricWaypoint(d, 0, 0, true, 50.f, 1.0f );
	  acquireDist = d;
	} else {
	  wp_acc->addEgocentricWaypoint(x, 0, 0, true, 50.f, 1.0f );
	  wp_acc->addEgocentricWaypoint(0, 0, M_PI/2.f + abstheta, true, 50.f, 1.0f );
	  //wp_acc->addEgocentricWaypoint(hyp, 0, 0, true, 50.f, 1.0f );
	  acquireDist = hyp;
	}
      } else {
	wp_acc->addEgocentricWaypoint(0, 0, -abstheta + M_PI/2.f, true, 50.f, 1.0f );
	wp_acc->addEgocentricWaypoint(backup_step_size*sin(abstheta), 0, 0, true, 50.f, 1.0f );
	wp_acc->addEgocentricWaypoint(0, 0, -M_PI/2.f, true, 50.f, 1.0f );
	wp_acc->addEgocentricWaypoint(backup_step_size*cos(abstheta), 0, 0, true, 50.f, 1.0f );
      }
    }

    $nodeclass AdjustTurn(const ExecutePlan::TurnAdjustType_t adjustmentType) : WaypointWalkNode : doStart {
      $reference Pilot::plan;
      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();
      DualCoding::Point position = theAgent->getCentroid();
      fmat::Column<3> pos = position.getCoords();
      fmat::Column<3> next = plan.currentStep->waypoint.getCoords();

      if (adjustmentType == collisionAdjust) {
	next = plan.currentStep->waypoint.getCoords();
        cout << "*************** ADJUST TURN COLLISION ***********************************" << endl;
      }
      else {
	plan.currentStep++;
	next = plan.currentStep->waypoint.getCoords();
	plan.currentStep--;
        cout << "*************** ADJUST TURN ***********************************" << endl;
      }

      fmat::Column<2> disp = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
      AngSignPi angle = (atan2(disp[1], disp[0]) - theAgent->getOrientation());

      PilotRequest &req = *VRmixin::pilot->curReq;
      #ifdef TGT_IS_CREATE
      float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 50.f;
      float va = (req.turnSpeed > 0) ? req.turnSpeed : 1.0f;
      wp_acc->addEgocentricWaypoint(0, 0, angle, true, vx, va);
      #else
      // these values are for Chiara
      float vx = (req.forwardSpeed > 0) ? req.forwardSpeed : 15.f;
      float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
      wp_acc->addEgocentricWaypoint(0, 0, angle, true, vx, va);
      #endif
    }

      $nodeclass ExecuteStep : VisualRoutinesStateNode {
      virtual void doStart() {
	$reference ExecutePlan::nextDisplacementInstruction;
	$reference Pilot::plan;
	$reference Pilot::pushingObj;

	doExecute(plan, nextDisplacementInstruction, pushingObj);
      }
      virtual void doExecute(NavigationPlan &plan, DisplacementInstruction &nextDisplacementInstruction, const bool pushType);
    }

    $nodeclass LookForObject : MapBuilderNode : doStart {
      MapBuilderRequest *m = VRmixin::pilot->curReq->objectExtractor;
      if ( m == NULL )
	cancelThisRequest();
      else
	mapreq = *m;
    }

    $nodeclass SelectObject : VisualRoutinesStateNode : doStart {
      $reference  ExecutePlan::boxX;
      PilotRequest &req = *VRmixin::pilot->curReq;
      if ( req.objectMatcher == NULL ) {
	cout << "No objectMatcher specified for pushObject in Pilot" << endl;
	postStateFailure();
      } else {
	ShapeRoot localObj = (*req.objectMatcher)(req.objectShape);
	if ( ! localObj.isValid() )
	  postStateFailure();
	else {
	  boxX = localObj->getCentroid().coordX();
	  postStateCompletion();
	}
      }
    }

    $nodeclass TurnAndMove : WaypointWalkNode : doStart {
      $reference ExecutePlan::acquireDist;
      $reference ExecutePlan::boxX;
      float stepSize = min(acquireDist, 100.f);
      acquireDist -= stepSize;
      float angle;
      angle = asin( (160.f - boxX) * sin(M_PI/6.f)/160.f );  // ***HACK based on 320x240 camera image
      angle = 0;

      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();

#ifdef TGT_IS_CREATE
      float vx = 50.f;
      float va = 1.0f;
#else
      float vx = 15.f;
      float va = 0.1f;
#endif

      wp_acc->addEgocentricWaypoint(0, 0,  angle, true, vx, va);
      wp_acc->addEgocentricWaypoint(stepSize, 0,  0, true, vx, va);
      motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
    }

    $nodeclass DecideNode : VisualRoutinesStateNode : doStart {
        $reference ExecutePlan::acquireDist;
        if (acquireDist < 1.f)
            postStateCompletion();
        else
            postStateFailure();
    }

    $nodeclass AcquireObj : VisualRoutinesStateNode : doStart {
        $reference ExecutePlan::acquireDist;
        $reference ExecutePlan::nextDisplacementInstruction;
        acquireDist = nextDisplacementInstruction.nextPoint.norm();
    }

    $setupmachine{ // set up ExecutePlan
      execute : ExecuteStep
      execute =S<NavigationStepType_t>(localizeStep)=> localizer
      execute =S<NavigationStepType_t>(travelStep)=> walker
      execute =S<NavigationStepType_t>(turnStep)=> turner
	//      execute =S<NavigationStepType_t>(turnObjStep)=> boxturner
      execute =S<NavigationStepType_t>(turnObjStep)=> turner
      execute =S<NavigationStepType_t>(acquireObjStep)=> acquirebox
      execute =S=> next
      execute =C=> PostMachineCompletion

      localizer: LocalizationUtility
      localizer =C=> adjust
      localizer =F=> adjust

      adjust: AdjustTurn(localizationAdjust)[setMC(VRmixin::pilot->waypointwalk_id)] =C=> next

      walker: Walk[setMC(VRmixin::pilot->waypointwalk_id)] =C=> next

      turner: Turn[setMC(VRmixin::pilot->waypointwalk_id)] =C=> next

      boxturner: TurnObj[setMC(VRmixin::pilot->waypointwalk_id)] =C=> locatebox 

      acquirebox: AcquireObj =N=> locatebox

      locatebox: LookForObject
      locatebox =C=> select
      locatebox =F=> select

      select: SelectObject =N=> turnandmove: TurnAndMove =C=> decide

      decide: DecideNode
      decide =C=> next
      decide =F=> locatebox

      next: NextTask
      next =S=> execute
      next =C=> PostMachineCompletion

      collisionDispatch: CollisionDispatch
      {collisionDispatch, localizer, walker, turner, boxturner, turnandmove} =PILOT(collisionDetected)=> term

      term: TerminateDueToCollision
    }
  }

  //================ LocalizationUtility ================

  //! State machine called by various Pilot functions to perform a landmark search and localization.
  $nodeclass LocalizationUtility : VisualRoutinesStateNode {

    $provide AngTwoPi initialHeading;

    $nodeclass TakeInitialPicture : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
      $reference LocalizationUtility::initialHeading;
      initialHeading = VRmixin::theAgent->getOrientation();
      $reference Pilot::maxTurn;
      maxTurn = M_PI;
      if ( VRmixin::pilot->curReq == NULL // needed to prevent crash if behavior is shut down in the ControllerGUI
	   || VRmixin::pilot->curReq->landmarkExtractor == NULL ) {
	cancelThisRequest();
	return;
      }
      mapreq = *VRmixin::pilot->curReq->landmarkExtractor;
      mapreq.clearLocal = true;
    }

    $nodeclass CheckEnoughLandmarks : VisualRoutinesStateNode : doStart {
      if ( VRmixin::pilot->curReq != NULL // needed to prevent crash if behavior is shut down in the ControllerGUI
	   && VRmixin::pilot->curReq->landmarkExitTest() )
	postStateSuccess();
      else
	postStateFailure();
    }

#ifdef TGT_HAS_HEAD
    // Use gazepoints since we have a pan/tilt available
    $provide std::deque<Point> gazePoints;

    $nodeclass ChooseGazePoints : StateNode {
      virtual void doStart() {
	$reference LocalizationUtility::gazePoints;
	setupGazePoints(gazePoints);
      }
      void setupGazePoints(std::deque<Point> &gazePoints);
    }

    $nodeclass PrepareForNextGazePoint : StateNode {
      virtual void doStart() {
	// This node just does a quick check because we'll be using the
	// pan/tilt for the actual work.  The no-head version (below)
	// will turn the body.
	$reference LocalizationUtility::gazePoints;
	if ( gazePoints.empty() )
	  postStateFailure();
	else
	  postStateCompletion();
      }
      void setMC(const MotionManager::MC_ID) {}  // dummy for compatibility with headless case below
    }

    $nodeclass TakeNextPicture : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
      {
	GET_SHAPE(gazePoint, PointData, VRmixin::localShS);
	if ( gazePoint.isValid() )
	  VRmixin::localShS.deleteShape(gazePoint);
      }
      // send the MapBuilder to the next gaze point
      $reference LocalizationUtility::gazePoints;
      NEW_SHAPE(gazePoint, PointData, new PointData(localShS, gazePoints.front()));
      gazePoints.pop_front();
      mapreq =  *VRmixin::pilot->curReq->landmarkExtractor;
      mapreq.searchArea = gazePoint;
      mapreq.clearLocal = false;
    }

    $nodeclass ReturnToInitialHeading : StateNode {
      virtual void doStart() {
        // dummy node since we never moved the body (because we have a pan/tilt)
        postStateCompletion();
      }
      void setMC(const MotionManager::MC_ID) {}  // dummy for compatibility with headless case below
    }

#else
    // No pan/tilt available; must turn the body to acquire more landmarks
    $provide std::deque<AngTwoPi> gazeHeadings;

    $nodeclass ChooseGazePoints : StateNode {
      virtual void doStart() {
	$reference LocalizationUtility::initialHeading;
	$reference LocalizationUtility::gazeHeadings;
	$reference Pilot::maxTurn;

	setupGazeHeadings(initialHeading, gazeHeadings, maxTurn);
      }
      void setupGazeHeadings(const AngTwoPi initialHeading, std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn);
    }

    $nodeclass PrepareForNextGazePoint : WaypointWalkNode {
      virtual void doStart() {
	$reference LocalizationUtility::gazeHeadings;
        $reference Pilot::maxTurn;
	prepareForNext(gazeHeadings, maxTurn);
      }
      void prepareForNext(std::deque<AngTwoPi> &gazeHeadings, const AngTwoPi maxTurn);
    }

    $nodeclass TakeNextPicture : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
      mapreq =  *VRmixin::pilot->curReq->landmarkExtractor;
      mapreq.clearLocal = false;
      $reference LocalizationUtility::initialHeading;
      const AngTwoPi curHeading = VRmixin::theAgent->getOrientation();
      const float headingShift = float(curHeading-initialHeading);
      mapreq.baseTransform.rotation() = fmat::rotationZ(headingShift);

    }

    $nodeclass ReturnToInitialHeading : WaypointWalkNode : constructor {
      setMC(VRmixin::pilot->getWaypointwalk_id());
      $endnodemethod;

      virtual void doStart() {
	static const float allowableAngularError = 0.10f;
	$reference LocalizationUtility::initialHeading;
	const AngTwoPi curHeading = VRmixin::theAgent->getOrientation();
	const AngSignPi turnAmount = AngSignPi(initialHeading - curHeading);
	if ( fabs(turnAmount) < allowableAngularError ) {
	  postStateCompletion();
	  return;
	}
	MMAccessor<WaypointWalkMC> wp_acc(getMC());
	wp_acc->getWaypointList().clear();
#ifdef TGT_IS_CREATE
	float const va = 0.9f;  // optimal turn speed for CREATE odometry
	wp_acc->addEgocentricWaypoint(0, 0, turnAmount, true, 0, va);
#else
	// no other headless robots supported yet, so just repeat the Create values for now
	float const va = 0.9f;
	wp_acc->addEgocentricWaypoint(0, 0, turnAmount, true, 0, va);
#endif
	motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
      }
    }

#endif // TGT_HAS_HEAD

    $nodeclass Localize : VisualRoutinesStateNode {
      static float const minConfidentWeight;
      virtual void doStart();
    }

    $setupmachine{     // LocalizationUtility

      take: TakeInitialPicture
      take =F=> outofoptions
      take =MAP=> checkEnough

      checkEnough: CheckEnoughLandmarks
      checkEnough =S=> loc
      checkEnough =F=> ChooseGazePoints =N=> loop

      loop: PrepareForNextGazePoint[setMC(VRmixin::pilot->getWaypointwalk_id())]
      loop =C=> StateNode =T(1000)=> takenextpic
			loop =S=> loop   // skip this gaze point (maxTurn exceeded?) and go to the next
      loop =F=> outofoptions

      takenextpic: TakeNextPicture =MAP=> checkEnough2

      checkEnough2: CheckEnoughLandmarks
      checkEnough2 =S=> ReturnToInitialHeading[setMC(VRmixin::pilot->getWaypointwalk_id())] =C=> loc
      checkEnough2 =F=> loop

      loc: Localize =N=> PostMachineSuccess =N=> PostMachineCompletion

      outofoptions: ReturnToInitialHeading =C=> PostMachineFailure =N=> PostMachineCompletion  // ran out of gaze points to try
    }
  }

  //================ LocalizationMachine ================

  $nodeclass LocalizationMachine : StateNode {
    $setupmachine{
      SetupLandmarkExtractor =N=> loc

      loc: LocalizationUtility
      loc =S=> PostMachineSuccess
      loc =F=> PostMachineFailure
    }
  }

  //================ WalkMachine ================

  $nodeclass WalkMachine : StateNode {

    $nodeclass WalkMachine_WaypointWalker : WaypointWalkNode {
      virtual void doStart();
    }

    $setupmachine{
      startnode: StateNode =N=> {walker, collisionDispatch}
      walker: WalkMachine_WaypointWalker[setMC(VRmixin::pilot->getWaypointwalk_id())]
      walker =C=> ReportCompletion
      collisionDispatch: CollisionDispatch
      {collisionDispatch, walker} =PILOT(collisionDetected)=> TerminateDueToCollision
    }

  }

  //================ WaypointWalkMachine ================

  $nodeclass WaypointWalkMachine : StateNode {

    $nodeclass WaypointWalkMachine_WaypointWalker : WaypointWalkNode {
      virtual void doStart();
    }

    $setupmachine{
      startnode: StateNode =N=> {walker, collisionDispatch}
      walker: WaypointWalkMachine_WaypointWalker[setMC(VRmixin::pilot->getWaypointwalk_id())]
      walker =C=> ReportCompletion
      collisionDispatch: CollisionDispatch
      {collisionDispatch, walker} =PILOT(collisionDetected)=> TerminateDueToCollision
    }

  }		

  //=============== SetVelocityMachine ==================
  $nodeclass SetVelocityMachine : StateNode {

    $nodeclass SetVelocityMachine_Walker : WalkNode {
      virtual void doStart();
    }

    $setupmachine{
      startnode: StateNode =N=> {walker, collisionDispatch}
      walker: SetVelocityMachine_Walker[setMC(VRmixin::pilot->getWalk_id())]
      walker =C=> StateNode =T(2000)=> ReportCompletion  // allow time for legs to settle (Chiara)
      collisionDispatch: CollisionDispatch
      {collisionDispatch, walker} =PILOT(collisionDetected)=> TerminateDueToCollision
    }

  }

  //================ GoToShapeMachine ================

  $nodeclass GoToShapeMachine : VisualRoutinesStateNode {

    //! Check that the request's goToShape parameters are valid
    $nodeclass CheckParameters : StateNode {
      virtual void doStart();
    }

    $nodeclass SetUpPlanNode : StateNode : doStart {
        $reference Pilot::initPos;
        $reference Pilot::initHead;
        $reference Pilot::planReq;
        $reference Pilot::pushingObj;

        initPos = theAgent->getCentroid();
        initHead = theAgent->getOrientation();
        planReq = *VRmixin::pilot->curReq;
        pushingObj = false;
    }

    $nodeclass SetMaxTurn : StateNode : doStart {
        $reference Pilot::maxTurn;
        maxTurn = M_PI;
    }

    virtual void  setup() {   // setup GoToShapeMachine

      $statemachine{
        SetMaxTurn =N=> check

        check: CheckParameters
	check =F=> ReportCompletion(invalidRequest)
	check =S=> SetupLandmarkExtractor =N=> loc

	loc: LocalizationUtility
	loc =S=> setupplan
        loc =F=> setupplan  // if we can't localize, use last known position; maybe say something first?

	setupplan: SetUpPlanNode =N=> planpath

	planpath: PlanPath
	planpath =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::START_COLLIDES)=> ReportCompletion(startCollides)
	planpath =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::END_COLLIDES)=> ReportCompletion(endCollides)
	planpath =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::MAX_ITER)=> ReportCompletion(noPath)
	planpath =C=> constructnavplan
	planpath =F=> ReportCompletion  // if executePath == false, we're done

        collisionDispatch: CollisionDispatch

	constructnavplan: ConstructNavPlan
	constructnavplan =C=> {execute, collisionDispatch}

        execute: ExecutePlan =C=> PostMachineCompletion

	{collisionDispatch, execute} =PILOT(collisionDetected)=> term

        term: TerminateDueToCollision

       // term =C=> LostLocalizationUtility =C=> AdjustTurn(collisionType) =C=> {execute, collisionDispatch}
      }
		}
  } //GoToShapeMachine

  //================ PushObjectMachine ================

  $nodeclass PushObjectMachine : VisualRoutinesStateNode {

    $provide NavigationPlan initToObjPlan;
    $provide NavigationPlan objToDestPlan;


    $nodeclass SetMaxTurn(float value=M_PI) : StateNode : doStart {
      $reference Pilot::maxTurn;
      maxTurn = value;
    }

    $nodeclass CheckParameters : StateNode {
      virtual void doStart();
    }

    $nodeclass SetUpObjToDest : StateNode : doStart {
      $reference Pilot::initPos, Pilot::planReq, Pilot::plan, Pilot::pushingObj;
      plan.clear();
      initPos = VRmixin::pilot->curReq->objectShape->getCentroid();
      planReq = *VRmixin::pilot->curReq;
      planReq.objectShape->setObstacle(false);
      pushingObj = false;
    }

    $nodeclass BackOffDest : StateNode : doStart {
      $reference Pilot::plan;
      size_t s = plan.path.size();
      Point final = plan.path[s-1];
      Point prev = plan.path[s-2];
      Point vec = final-prev;
      float len = vec.xyNorm();
      float robotDiam = 150;  // *** Should take from bounding box
      // *** should also subtract half of object diameter
      // back off by robot diameter to leave object at the destination
      if ( len > robotDiam )
	plan.path[s-1] = prev + vec/len*(len-robotDiam);
    }

    $nodeclass AddAcquirePt : StateNode : doStart {
      $reference Pilot::plan;
      const Point delta = (plan.path[1] - plan.path[0]); 
      const Point deltaStep = delta.unitVector() * (-400);  // *** HACK: should compute from robot bounding box
      // *** HACK: should check this segment for collisions; don't assume it's safe
      const Point acquirePt = plan.path[0] + deltaStep;

      std::vector<DualCoding::Point>::iterator it;
      it = plan.path.begin();
      plan.path.insert(it, acquirePt);

      // Display path from object to destination
      NEW_SHAPE(plannedObjToDestPath, GraphicsData, new GraphicsData(worldShS));
      GraphicsData::xyPair U, V;
      for ( size_t i = 1; i < plan.path.size(); i++ ) {
	U.first = plan.path[i-1].coordX(); U.second = plan.path[i-1].coordY();
	V.first = plan.path[i].coordX(); V.second = plan.path[i].coordY();
	plannedObjToDestPath->add(new GraphicsData::LineElement(U, V, rgb(0,255,255)));
      }
    }

    $nodeclass SetUpInitToObj : StateNode : doStart {
      $reference Pilot::plan, Pilot::initPos, Pilot::initHead, Pilot::planReq, PushObjectMachine::objToDestPlan;
      objToDestPlan.clear();
      objToDestPlan.path = plan.path;
      plan.clear();

      initPos = theAgent->getCentroid();
      initHead = theAgent->getOrientation();
      planReq = *VRmixin::pilot->curReq;
      planReq.objectShape->setObstacle(true);

      NEW_SHAPE( dest, PointData, new PointData(worldShS, plan.path[0] ) );
      dest->setObstacle(false);
      planReq.targetShape = dest;
    }

    $nodeclass Walk : WaypointWalkNode : doStart {
      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();

#ifdef TGT_IS_CREATE
      wp_acc->addEgocentricWaypoint(-300, 0,  0, true, 100.f, 0.f);
#else
      wp_acc->addEgocentricWaypoint(-300, 0,  0, true, 100.f, 0.f);
#endif
      motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);				
    }

    $nodeclass SetUpExecute2 : StateNode : doStart {
      $reference Pilot::plan, Pilot::initPos, Pilot::initHead, Pilot::planReq, Pilot::maxTurn, Pilot::pushingObj;
      $reference PushObjectMachine::objToDestPlan, PushObjectMachine::initToObjPlan;

      initToObjPlan.clear();
      initToObjPlan.path = plan.path;
      initToObjPlan.steps = plan.steps;

      plan.clear();
      plan.path = objToDestPlan.path;

      PilotRequest &req = *VRmixin::pilot->curReq;
      initPos = theAgent->getCentroid();
      initHead = theAgent->getOrientation();
      req.turnSpeed = 0.25f;
      req.forwardSpeed = 50.f;
      planReq = req;
      maxTurn = 0.f;
      pushingObj = true;
      req.collisionAction = PilotTypes::collisionIgnore;
    }

    $nodeclass MoveTheObject : VisualRoutinesStateNode : doStart {
      const Point destination = VRmixin::pilot->curReq->targetShape->getCentroid();
      VRmixin::pilot->curReq->objectShape->setPosition(destination);
    }

    virtual void setup() { // set up PushObjectMachine
      $statemachine{
        SetMaxTurn =N=> checkparams

	  checkparams: CheckParameters
	  checkparams =S=> localize
	  checkparams =F=> ReportCompletion(invalidRequest)

	  localize: SetupLandmarkExtractor =N=>
	  locutil: LocalizationUtility
	  locutil =S=> plandelivery
	  locutil =F=> plandelivery // if we can't localize, use last known position; maybe say something first?

	  // Plan path to deliver object to destination
	  plandelivery: SetUpObjToDest =N=>
	  planpath2: PlanPath
	  planpath2 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::START_COLLIDES)=> ReportCompletion(startCollides)
	  planpath2 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::END_COLLIDES)=> ReportCompletion(endCollides)
	  planpath2 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::MAX_ITER)=> ReportCompletion(noPath)
	  planpath2 =C=> planacquire
	  planpath2 =F=> ReportCompletion  // if executePath == false, we're done

	  // constructnavplan1 : ConstructNavPlan
	  // constructnavplan1 =C=> {execute, collisionDispatch}

	  // Plan path from initial position to object acquire point
	  planacquire: BackOffDest =N=> AddAcquirePt =N=> SetUpInitToObj =N=>
	  planpath1: PlanPath
	  planpath1 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::START_COLLIDES)=> ReportCompletion(startCollides)
	  planpath1 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::END_COLLIDES)=> ReportCompletion(endCollides)
	  planpath1 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::MAX_ITER)=> ReportCompletion(noPath)
	  planpath1 =C=> ConstructNavPlan =C=> execute
	  planpath1 =F=> ReportCompletion  // if executePath == false, we're done

	  execute: ExecutePlan =C=> SetUpExecute2 =N=> 
	  ConstructNavPlan =C=>  ExecutePlan =C=> MoveTheObject =N=>
	  Walk[setMC(VRmixin::pilot->waypointwalk_id)] =C=> ReportCompletion
	  }
    }

  } // end PushObjectMachine

  //================ VisualSearchMachine ================

  $nodeclass VisualSearchMachine : StateNode {

    $nodeclass Search : MapBuilderNode : doStart {
      if (VRmixin::pilot->curReq->searchObjectExtractor == NULL) {
	cout << "Pilot received visualSearch request with invalid searchObjectExtractor field" << endl;
	VRmixin::pilot->requestComplete(invalidRequest);
      }
      else if (VRmixin::pilot->curReq->searchExitTest == NULL) {
	cout << "Pilot received visualSearch request with no exitTest" << endl;
	VRmixin::pilot->requestComplete(invalidRequest);
      }
      mapreq = *VRmixin::pilot->curReq->searchObjectExtractor;
    }

    $nodeclass Check : StateNode : doStart {
      if (VRmixin::pilot->curReq->searchExitTest())
	VRmixin::pilot->requestComplete(noError);
      else if (VRmixin::pilot->curReq->searchRotationAngle == 0)
	VRmixin::pilot->requestComplete(searchFailed);
      else
	postStateCompletion();
    }

    $nodeclass Rotate : WaypointWalkNode : doStart {
      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();
      wp_acc->addEgocentricWaypoint(0,0,VRmixin::pilot->curReq->searchRotationAngle,true,0.01f);
    }

    $setupmachine {
      startnode: Search() =MAP=> Check =C=>
	Rotate[setMC(VRmixin::pilot->waypointwalk_id)] =C=> startnode
    }		
  }



  //================ Dispatch ================
  // All the Pilot's little state machines are children of the
  // Dispatch node, so when a call to Dispatch::finish() causes it to
  // complete and exit, everything gets shut down.

  $nodeclass* Dispatch : StateNode : localizeMachine_(NULL),
    walkMachine_(NULL), waypointWalkMachine_(NULL), setVelocityMachine_(NULL),
    goToShapeMachine_(NULL), pushObjectMachine_(NULL), visualSearchMachine_(NULL) {

    StateNode *localizeMachine_, *walkMachine_, *waypointWalkMachine_, *setVelocityMachine_, 
      *goToShapeMachine_, *pushObjectMachine_, *visualSearchMachine_;

    void finish() {
      postStateCompletion();
    }

    virtual void doStart() {
      PilotTypes::RequestType_t reqType = VRmixin::pilot->curReq->getRequestType();
      switch ( reqType) {
      case localize:
        localizeMachine_->start();
	break;

      case walk:
	walkMachine_->start();
	break;

      case waypointWalk:
	waypointWalkMachine_->start();
	break;

      case setVelocity:
	setVelocityMachine_->start();
	break;

      case goToShape:
	goToShapeMachine_->start();
	break;

      case pushObject:
	pushObjectMachine_->start();
	break;

      case visualSearch:
	visualSearchMachine_->start();
	break;

      default:
	cout << "Illegal Pilot request type: " << reqType << endl;
	VRmixin::pilot->requestComplete(invalidRequest);
      }
    }

    virtual void setup() {
      $statemachine{
        dispatchDummyStart: StateNode  // dummy start node; never called

	reportSuccess: ReportCompletion
	reportFailure: ReportCompletion(someError)

	localizemachine: LocalizationMachine =S=> reportSuccess
	localizemachine =F=> ReportCompletion(cantLocalize)

	walkmachine: WalkMachine

	waypointwalkmachine: WaypointWalkMachine =C=> reportSuccess
	waypointwalkmachine =F=>  reportFailure

	setvelocitymachine: SetVelocityMachine // =N=> reportSuccess

	gotoshapemachine: GoToShapeMachine =C=> reportSuccess
	gotoshapemachine =F=> reportFailure

	pushobjectmachine: PushObjectMachine =C=> reportSuccess
	pushobjectmachine =F=> reportFailure

	visualsearchmachine: VisualSearchMachine =C=> reportSuccess
	visualsearchmachine =F=> reportFailure
      }

      startnode = NULL; // keeps the dummy node out of the Event Logger trace
      // Save these statenode pointers for use by dispatch,
      localizeMachine_ = localizemachine;
      walkMachine_ = walkmachine;
      waypointWalkMachine_ = waypointwalkmachine;
      setVelocityMachine_ = setvelocitymachine;
      goToShapeMachine_ = gotoshapemachine;
      pushObjectMachine_ = pushobjectmachine;
      visualSearchMachine_ = visualsearchmachine;
    }
  }

  virtual void setup() {   // Pilot's setup
    SharedObject<WaypointWalkMC> waypointwalk_mc;
    SharedObject<WalkMC> walk_mc;
#ifdef PILOT_USES_SHARED_MOTION_COMMANDS
    walk_id = motman->addPersistentMotion(walk_mc, MotionManager::kIgnoredPriority);
    waypointwalk_id = motman->addPersistentMotion(waypointwalk_mc, MotionManager::kIgnoredPriority);
#endif
    $statemachine {
    pilotWaitForDispatch: StateNode  // start node
	// The =C=> will shut down Dispatch which will shut down all its children
	dispatch: Dispatch =pilot_dispatch:C=> pilotWaitForDispatch
	}
    VRmixin::pilot->dispatch_ = dispatch;
  }

private:
  Pilot(const Pilot&); //!< copy constructor; do not call
  Pilot& operator=(const Pilot&); //!< assignment operator; do not call

  static PilotVerbosity_t verbosity;

  std::queue<PilotRequest*> requests;   //!< Queue of Pilot requests
  PilotRequest *curReq;
  unsigned int idCounter;   //!< Pilot request counter for assigning a unique id
  MotionManager::MC_ID walk_id;
  MotionManager::MC_ID waypointwalk_id;
  Dispatch *dispatch_;

  /*! @endcond */

};  // Pilot class

std::ostream& operator<<(std::ostream& os, const NavigationStep &step);
std::ostream& operator<<(std::ostream& os, const NavigationPlan &plan);

} // namespace

#endif