//-*-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/PostureNode.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 "Vision/VisualOdometry/VisualOdometry.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();
  }

  $nodeclass RunOpticalFlow : 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::isWalkingFlag = false;
    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;
    
    //! 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 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);
    }

    $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>(preTravelStep)=> pretravel
      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
      
      pretravel: Turn[setMC(VRmixin::pilot->waypointwalk_id)] =C=> execute

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

      collisionDispatch: CollisionDispatch
      {collisionDispatch, localizer, walker, turner} =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();
    }

    $nodeclass SetWalking : StateNode : doStart {
      VRmixin::isWalkingFlag = true;
      // cout << "Turning On Walk" << endl;
    }

    $setupmachine{
    startnode: SetWalking =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();
    }

    $nodeclass SetWalking : StateNode : doStart {
      VRmixin::isWalkingFlag = true;
      // cout << "Turning On Walk" << endl;
    }

    $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: ClearOldPath =N=> 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
  
  $nodeclass ClearOldPath : VisualRoutinesStateNode : doStart {
    GET_SHAPE(plannedInitToObjPath, GraphicsData, worldShS);
    plannedInitToObjPath.deleteShape();
    GET_SHAPE(plannedObjToDestPath, GraphicsData, worldShS);
    plannedObjToDestPath.deleteShape();
    GET_SHAPE(plannedPath, GraphicsData, worldShS);
    plannedPath.deleteShape();
  }
  
  $nodeclass LookForObject : MapBuilderNode : doStart {
    //MapBuilderRequest *m = VRmixin::pilot->curReq->objectExtractor;
    //if ( m == NULL )
    //  cancelThisRequest();
    //else
    //  mapreq = *m;
      
    MapBuilderRequest *m = new MapBuilderRequest(MapBuilderRequest::localMap);
    std::vector<Point> ptVec;
    Point pt1 = Point(100, 100, 0, allocentric);
    Point pt2 = Point(100, -100, 0, allocentric);
    Point pt3 = Point(-100, -100, 0, allocentric);
    Point pt4 = Point(-100, 100, 0, allocentric);
    Point objPt = VRmixin::pilot->curReq->objectShape->getCentroid();
    ptVec.push_back(objPt);
    ptVec.push_back(objPt+pt1);
    ptVec.push_back(objPt+pt2);
    ptVec.push_back(objPt+pt3);
    ptVec.push_back(objPt+pt4);
    ptVec.push_back(objPt);
    NEW_SHAPE_N(polygon, PolygonData, new PolygonData(worldShS, ptVec, false));
    polygon->setObstacle(false);
    m->searchArea = polygon;
    switch ( VRmixin::pilot->curReq->objectShape->getType() ) {
    case blobDataType:
      m->addObjectColor(blobDataType, VRmixin::pilot->curReq->objectShape->getColor());
      m->addBlobOrientation(VRmixin::pilot->curReq->objectShape->getColor(), BlobData::pillar);
      //m->addObjectColor(blobDataType, "blue");
      //m->addBlobOrientation("blue", BlobData::pillar);
      break;
    case ellipseDataType:
      m->addObjectColor(ellipseDataType, VRmixin::pilot->curReq->objectShape->getColor());
      break;
    default:
      std::cout << "Object type is not supported yet" << std::endl;
      return;
    }
    mapreq = *m;
  }
  
  $nodeclass LookForTarget : MapBuilderNode : doStart {
    //MapBuilderRequest *m = VRmixin::pilot->curReq->objectExtractor;
    //if ( m == NULL )
    //  cancelThisRequest();
    //else
    //  mapreq = *m;
      
    MapBuilderRequest *m = new MapBuilderRequest(MapBuilderRequest::localMap);
    std::vector<Point> ptVec;
    Point pt1 = Point(100, 100, 0, allocentric);
    Point pt2 = Point(100, -100, 0, allocentric);
    Point pt3 = Point(-100, -100, 0, allocentric);
    Point pt4 = Point(-100, 100, 0, allocentric);
    Point ctPt = VRmixin::pilot->curReq->targetShape->getCentroid();
    ptVec.push_back(ctPt);
    ptVec.push_back(ctPt+pt1);
    ptVec.push_back(ctPt+pt2);
    ptVec.push_back(ctPt+pt3);
    ptVec.push_back(ctPt+pt4);
    ptVec.push_back(ctPt);
    NEW_SHAPE_N(polygon, PolygonData, new PolygonData(worldShS, ptVec, false));
    polygon->setObstacle(false);
    m->searchArea = polygon;
    switch ( VRmixin::pilot->curReq->objectShape->getType() ) {
    case blobDataType:
      m->addObjectColor(blobDataType, VRmixin::pilot->curReq->objectShape->getColor());
      m->addBlobOrientation(VRmixin::pilot->curReq->objectShape->getColor(), BlobData::pillar);
      //m->addObjectColor(blobDataType, "blue");
      //m->addBlobOrientation("blue", BlobData::pillar);
      break;
    case ellipseDataType:
      m->addObjectColor(ellipseDataType, VRmixin::pilot->curReq->objectShape->getColor());
      break;
    default:
      std::cout << "Object type is not supported yet" << std::endl;
      return;
    }
    mapreq = *m;
  }

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

  $nodeclass PushObjectMachine : VisualRoutinesStateNode {
  
    //static const int backupDist = 600;
    //static const int obstDist = 150;
    //static const int robotDiam = 150;
    //static const int objSize = 50;
    static int backupDist;
    static int obstDist;
    static int robotDiam;
    static int objSize;
    
    static float prePushTurnSpeed;
    static float prePushForwardSpeed;
    static float pushTurnSpeed;
    static float pushForwardSpeed;

    $provide NavigationPlan initToObjPlan;
    $provide NavigationPlan objToDestPlan;
    
    $provide Point startPos;
    $provide Point objPos;
    $provide Point endPos;
    
    $provide AngTwoPi startOri;
    $provide AngTwoPi acquireOri;
    $provide AngTwoPi backupOri;
    
    $provide Point backupPt;
    $provide Point acquirePt;
    
    $provide bool useSimpleWayObjToDest;
    $provide bool useSimpleWayInitToObj;

    static Point FindFreeSpace(ShapeRoot &targetShape, int targetDistance, int obstacleDistance, AngTwoPi orientation) {
      float diagonalLength = sqrt(worldSkS.getWidth()*worldSkS.getWidth() + worldSkS.getHeight()*worldSkS.getHeight());
      float scale = diagonalLength / (obstacleDistance * 10);
      float setx = (worldSkS.getHeight()/2) - targetShape->getCentroid().coordX() * scale;
      float sety = (worldSkS.getWidth()/2) - targetShape->getCentroid().coordY() * scale;
      worldSkS.setTmat(scale, setx, sety);
      bool isObs = targetShape->isObstacle();
      targetShape->setObstacle(false);
      
      NEW_SKETCH_N(obstacles, bool, visops::zeros(worldSkS));
      NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(worldShS));
      SHAPEVEC_ITERATE(ellipses, EllipseData, ellipse) {
        if (ellipse->isObstacle()) {
          NEW_SKETCH_N(s1, bool, ellipse->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS));
      SHAPEVEC_ITERATE(blobs, BlobData, blob) {
        if (blob->isObstacle()) {
          NEW_SKETCH_N(s1, bool, blob->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      NEW_SHAPEVEC(lines, LineData, select_type<LineData>(worldShS));
      SHAPEVEC_ITERATE(lines, LineData, line) {
        if (line->isObstacle()) {
          NEW_SKETCH_N(s1, bool, line->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      
      NEW_SKETCH_N(dist, float, visops::edist(obstacles));
      NEW_SKETCH_N(cand, bool, dist > obstacleDistance*scale);
      //NEW_SKETCH_N(cand, bool, (dist > distance*scale) & (dist < (distance+10)*scale));
      
      //NEW_SHAPE(point, PointData, new PointData(worldShS, targetPoint));
      //NEW_SKETCH_N(target, bool, point->getRendering());
      NEW_SKETCH_N(target, bool, targetShape->getRendering());
      NEW_SKETCH_N(tdist, float, visops::ebdist(target, !cand, 2*diagonalLength));
      NEW_SKETCH_N(cdist, bool, (tdist >= targetDistance*scale) & (tdist <= targetDistance*scale+1));
      //NEW_SKETCH_N(cdist, bool, tdist2 & cand);
      
      AngTwoPi ori = orientation + AngTwoPi(M_PI);
      //float length = sqrt(worldSkS.getWidth()*worldSkS.getWidth() + worldSkS.getHeight()*worldSkS.getHeight()) / worldSkS.getScaleFactor();
      float length = diagonalLength / scale;
      Point bp(targetShape->getCentroid().coordX()+length*std::cos(ori), targetShape->getCentroid().coordY()+length*std::sin(ori), 0, allocentric);
      NEW_SHAPE(line, LineData, new LineData(worldShS, targetShape->getCentroid(), bp));
      line->setColor("red");
      NEW_SKETCH_N(ls, bool, line->getRendering());
      NEW_SKETCH_N(lsdist, float, visops::edist(ls));
      NEW_SKETCH_N(ldist, float, visops::edist(ls) * cdist);
      line.deleteShape();
      
      int pos = ldist->findMinPlus();
      if (pos==-1) return Point(0,0,0,unspecified);
      NEW_SKETCH_N(final, bool, visops::zeros(ldist));
      final[pos] = true;
      final->setColor("green");
      targetShape->setObstacle(isObs);
      Point pt = final->indexPoint(pos);
      std::cout << "Found free space at point: " << pt << std::endl;
      return pt;
    }
    
    static Point FindFreeSpace2(ShapeRoot &targetShape, int targetDistance, int obstacleDistance, AngTwoPi orientation) {
      float diagonalLength = sqrt(worldSkS.getWidth()*worldSkS.getWidth() + worldSkS.getHeight()*worldSkS.getHeight());
      float scale = diagonalLength / (obstacleDistance * 10);
      float setx = (worldSkS.getHeight()/2) - targetShape->getCentroid().coordX() * scale;
      float sety = (worldSkS.getWidth()/2) - targetShape->getCentroid().coordY() * scale;
      worldSkS.setTmat(scale, setx, sety);
      bool isObs = targetShape->isObstacle();
      targetShape->setObstacle(false);
      
      NEW_SKETCH(obstacles, bool, visops::zeros(worldSkS));
      NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(worldShS));
      SHAPEVEC_ITERATE(ellipses, EllipseData, ellipse) {
        if (ellipse->isObstacle()) {
          NEW_SKETCH(s1, bool, ellipse->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS));
      SHAPEVEC_ITERATE(blobs, BlobData, blob) {
        if (blob->isObstacle()) {
          NEW_SKETCH(s1, bool, blob->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      NEW_SHAPEVEC(lines, LineData, select_type<LineData>(worldShS));
      SHAPEVEC_ITERATE(lines, LineData, line) {
        if (line->isObstacle()) {
          NEW_SKETCH(s1, bool, line->getRendering());
          obstacles |= s1;
        }
      END_ITERATE }
      
      NEW_SKETCH(dist, float, visops::edist(obstacles));
      NEW_SKETCH(cand, bool, dist > obstacleDistance*scale);
      //NEW_SKETCH(cand, bool, (dist > distance*scale) & (dist < (distance+10)*scale));
      
      //NEW_SHAPE(point, PointData, new PointData(worldShS, targetPoint));
      //NEW_SKETCH(target, bool, point->getRendering());
      NEW_SKETCH(target, bool, targetShape->getRendering());
      NEW_SKETCH(tdist, float, visops::ebdist(target, !cand, 2*diagonalLength));
      NEW_SKETCH(cdist, bool, (tdist >= targetDistance*scale) & (tdist <= targetDistance*scale+1));
      //NEW_SKETCH(cdist, bool, tdist2 & cand);
      
      AngTwoPi ori = orientation + AngTwoPi(M_PI);
      //float length = sqrt(worldSkS.getWidth()*worldSkS.getWidth() + worldSkS.getHeight()*worldSkS.getHeight()) / worldSkS.getScaleFactor();
      float length = diagonalLength / scale;
      Point bp(targetShape->getCentroid().coordX()+length*std::cos(ori), targetShape->getCentroid().coordY()+length*std::sin(ori), 0, allocentric);
      NEW_SHAPE(line, LineData, new LineData(worldShS, targetShape->getCentroid(), bp));
      line->setColor("red");
      NEW_SKETCH(ls, bool, line->getRendering());
      NEW_SKETCH(lsdist, float, visops::edist(ls));
      NEW_SKETCH(ldist, float, visops::edist(ls) * cdist);
      line.deleteShape();
      
      int pos = ldist->findMinPlus();
      if (pos==-1) return Point(0,0,0,unspecified);
      NEW_SKETCH(final, bool, visops::zeros(ldist));
      final[pos] = true;
      final->setColor("green");
      targetShape->setObstacle(isObs);
      Point pt = final->indexPoint(pos);
      std::cout << "Found free space at point: " << pt << std::endl;
      return pt;
    }
	
    $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;
      $reference PushObjectMachine::startPos, PushObjectMachine::objPos, PushObjectMachine::endPos, PushObjectMachine::startOri;
      
	  //std::cout << "--------SetUpObjToDest" << std::endl;
      startPos = theAgent->getCentroid();
      objPos = VRmixin::pilot->curReq->objectShape->getCentroid();
      endPos = VRmixin::pilot->curReq->targetShape->getCentroid();
      startOri = theAgent->getOrientation();
      
      plan.clear();
      initPos = objPos;
      planReq = *VRmixin::pilot->curReq;
      planReq.objectShape->setObstacle(false);
      pushingObj = false;
      planReq.displayPath = false;
    }

    $nodeclass AddBackupPt : StateNode : doStart {
      $reference Pilot::plan;
      $reference PushObjectMachine::endPos, PushObjectMachine::backupPt, PushObjectMachine::backupOri;
      
	  //std::cout << "--------AddBackupPt" << std::endl;
      size_t s = plan.path.size();
      Point final = plan.path[s-1];
      Point prev = plan.path[s-2];
      Point vec = final-prev;
      AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
      Point temp = VRmixin::pilot->curReq->objectShape->getCentroid();
      VRmixin::pilot->curReq->objectShape->setPosition(final);
      //std::cout << "tempBackupOri: " << ori << std::endl;
      backupPt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
      VRmixin::pilot->curReq->objectShape->setPosition(temp);
      ReferenceFrameType_t t = backupPt.getRefFrameType();
      if (t == unspecified) {
        std::cout << "Not enough space at destination." << std::endl;
        postStateFailure();
        return;
      }
      Point vec2 = endPos - backupPt;
      backupOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
      //std::cout << "backupPt: " << backupPt << ", backupOri: " << backupOri << std::endl;
      postStateSuccess();
    }

    $nodeclass AddAcquirePt : StateNode : doStart {
      $reference Pilot::plan;
      $reference PushObjectMachine::objPos, PushObjectMachine::acquirePt, PushObjectMachine::acquireOri;
      
	  //std::cout << "--------AddAcquirePt" << std::endl;
      Point first = plan.path[0];
      Point next = plan.path[1];
      Point vec = next-first;
      AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
      //std::cout << "tempAcquireOri: " << ori << std::endl;
      acquirePt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
      ReferenceFrameType_t t = acquirePt.getRefFrameType();
      if (t == unspecified) {
        std::cout << "Not enough space for acquiring object." << std::endl;
        postStateFailure();
        return;
      }
      Point vec2 = objPos - acquirePt;
      acquireOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
      //std::cout << "acquirePt: " << acquirePt << ", acquireOri: " << acquireOri << std::endl;
      postStateSuccess();
    }
    
    $nodeclass SetUpObjToDest2 : StateNode : doStart {
      $reference Pilot::initPos, Pilot::initHead, Pilot::planReq, Pilot::plan, Pilot::pushingObj;
      $reference PushObjectMachine::backupPt, PushObjectMachine::objPos, PushObjectMachine::acquireOri, PushObjectMachine::backupOri;
      
	  //std::cout << "--------SetUpObjToDest2" << std::endl;
      plan.clear();
      
      initPos = objPos;
      initHead = acquireOri;
      planReq = *VRmixin::pilot->curReq;
      planReq.objectShape->setObstacle(false);
      pushingObj = true;
      NEW_SHAPE(backup, PointData, new PointData(worldShS, backupPt));
      backup->setObstacle(false);
      planReq.targetShape = backup;
      planReq.targetHeading = backupOri;
      planReq.displayPath = false;
    }
    
    $nodeclass SetUpInitToObj : StateNode : doStart {
      $reference Pilot::initPos, Pilot::initHead, Pilot::planReq, Pilot::plan, Pilot::pushingObj;
      $reference PushObjectMachine::startPos, PushObjectMachine::startOri, PushObjectMachine::acquirePt, PushObjectMachine::acquireOri;
      
	  //std::cout << "--------SetUpInitToObj" << std::endl;
      plan.clear();
      
      initPos = startPos;
      initHead = startOri;
      planReq = *VRmixin::pilot->curReq;
      planReq.objectShape->setObstacle(true);
      pushingObj = false;
      NEW_SHAPE(acquire, PointData, new PointData(worldShS, acquirePt));
      acquire->setObstacle(false);
      planReq.targetShape = acquire;
      planReq.targetHeading = acquireOri;
      planReq.displayPath = false;
      planReq.turnSpeed = prePushTurnSpeed;
      planReq.forwardSpeed = prePushForwardSpeed;
    }
    
    $nodeclass ChoosePathInitToObj : StateNode : doStart {
      $reference Pilot::plan;
      $reference PushObjectMachine::startPos, PushObjectMachine::objPos, PushObjectMachine::acquireOri;
      $reference PushObjectMachine::objToDestPlan;
      
	  //std::cout << "--------ChoosePathInitToObj" << std::endl;
      objToDestPlan.clear();
      objToDestPlan.path = plan.path;
      
      Point vec = objPos-startPos;
      AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
      float d = vec.xyNorm();
      float oriDiff = std::abs(AngSignPi(ori-acquireOri));
      //std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", acquireOri: " << acquireOri << std::endl;
      if (d < backupDist*1.2 && oriDiff < M_PI/180*45) {
        //FetchObj
        std::cout << "Use simple way (straight line) from init to obj" << std::endl;
        postStateSuccess();
      }
      else {
        //plan path
        std::cout << "Use complex way (need path planning) from init to obj" << std::endl;
        postStateFailure();
      }
    }
    
    /*$nodeclass ChoosePathObjToDest : StateNode : doStart {
      //$reference PushObjectMachine::endPos, PushObjectMachine::backupOri;
      
	  std::cout << "--------ChoosePathObjToDest" << std::endl;
      Point curPos = theAgent->getCentroid();
      //AngTwoPi curHead = theAgent->getOrientation();
      Point vec = endPos-curPos;
      AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
      float d = vec.xyNorm();
      float oriDiff = std::abs(AngSignPi(ori-backupOri));
      std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", backupOri: " << backupOri << std::endl;
      if (d < backupDist*1.05 && oriDiff < M_PI/180*30) {
        //AdjustPush
        std::cout << "Use simple way (straight line) from obj to dest" << std::endl;
        postStateSuccess();
      }
      else {
        //plan path
        std::cout << "Use complex way (need path planning) from obj to dest" << std::endl;
        postStateFailure();
      }
    }*/
    
    $nodeclass ChoosePathObjToDest : StateNode : doStart {
      $reference Pilot::plan;
      $reference PushObjectMachine::endPos, PushObjectMachine::backupPt, PushObjectMachine::backupOri;
      $reference PushObjectMachine::initToObjPlan;
      
	  //std::cout << "--------ChoosePathObjToDest" << std::endl;
      initToObjPlan.clear();
      initToObjPlan.path = plan.path;
      initToObjPlan.steps = plan.steps;
      
      Point curPos = theAgent->getCentroid();
      //AngTwoPi curHead = theAgent->getOrientation();
      Point vec = endPos-curPos;
      AngTwoPi ori = AngTwoPi(std::atan2(vec.coordY(), vec.coordX()));
      Point temp = VRmixin::pilot->curReq->objectShape->getCentroid();
      VRmixin::pilot->curReq->objectShape->setPosition(endPos);
      //std::cout << "tempBackupOri: " << ori << std::endl;
      backupPt = FindFreeSpace(VRmixin::pilot->curReq->objectShape, backupDist, obstDist, ori);
      VRmixin::pilot->curReq->objectShape->setPosition(temp);
      ReferenceFrameType_t t = backupPt.getRefFrameType();
      if (t == unspecified) {
        std::cout << "Not enough space at destination." << std::endl;
        //postStateFailure();
        return;
      }
      Point vec2 = endPos - backupPt;
      backupOri = AngTwoPi(std::atan2(vec2.coordY(), vec2.coordX()));
      //std::cout << "backupPt: " << backupPt << ", backupOri: " << backupOri << std::endl;
      float d = vec.xyNorm();
      float oriDiff = std::abs(AngSignPi(ori-backupOri));
      //std::cout << "d: " << d << ", oriDiff:" << oriDiff << ", ori: " << ori << ", backupOri: " << backupOri << std::endl;
      if (d < backupDist*1.2 && oriDiff < M_PI/180*45) {
        //AdjustPush
        std::cout << "Use simple way (straight line) from obj to dest" << std::endl;
        postStateSuccess();
      }
      else {
        //plan path
        std::cout << "Use complex way (need path planning) from obj to dest" << std::endl;
        postStateFailure();
      }
    }
    
    $nodeclass SetUpExecute2 : StateNode : doStart {
      $reference Pilot::plan, Pilot::initPos, Pilot::initHead, Pilot::planReq, Pilot::maxTurn, Pilot::pushingObj;
      $reference PushObjectMachine::objToDestPlan;

	  //std::cout << "--------SetUpExecute2" << std::endl;
      plan.clear();
      plan.path = objToDestPlan.path;

      initPos = theAgent->getCentroid();
      initHead = theAgent->getOrientation();
      PilotRequest &req = *VRmixin::pilot->curReq;
      req.turnSpeed = pushTurnSpeed;
      req.forwardSpeed = pushForwardSpeed;
      planReq = req;
      maxTurn = 0.f;
      pushingObj = true;
      req.collisionAction = PilotTypes::collisionIgnore;
      planReq.displayPath = false;
    }
    
    $nodeclass DisplayStraightLineInitToObj : StateNode : doStart {
      $reference PushObjectMachine::objPos, PushObjectMachine::startPos;
      
	  //std::cout << "--------DisplayStraightLineInitToObj" << std::endl;
      // Display path (straight line) from init to obj
      NEW_SHAPE(plannedInitToObjPath, GraphicsData, new GraphicsData(worldShS));
      GraphicsData::xyPair U, V;
      U.first = startPos.coordX(); U.second = startPos.coordY();
      V.first = objPos.coordX(); V.second = objPos.coordY();
      plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
      //NEW_SHAPE(plannedInitToObjPath2, LineData, new LineData(worldShS, startPos, objPos));
      //plannedInitToObjPath2->setColor("red");
    }
    
    $nodeclass DisplayStraightLineObjToDest : StateNode : doStart {
      $reference PushObjectMachine::objPos, PushObjectMachine::endPos;
      
	  //std::cout << "--------DisplayStraightLineObjToDest" << std::endl;
      // Display path (straight line) from obj to dest
      NEW_SHAPE(plannedObjToDestPath, GraphicsData, new GraphicsData(worldShS));
      GraphicsData::xyPair U, V;
      U.first = objPos.coordX(); U.second = objPos.coordY();
      V.first = endPos.coordX(); V.second = endPos.coordY();
      plannedObjToDestPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
      //NEW_SHAPE(plannedObjToDestPath2, LineData, new LineData(worldShS, objPos, endPos));
      //plannedObjToDestPath2->setColor("red");
    }
    
    $nodeclass DisplayPathInitToObj : StateNode : doStart {
      $reference Pilot::plan;
      $reference PushObjectMachine::objPos;
      
	  //std::cout << "--------DisplayPathInitToObj" << std::endl;
      // Display path from init to obj
      NEW_SHAPE(plannedInitToObjPath, 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();
	plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
      }
      U.first = plan.path[plan.path.size()-1].coordX(); U.second = plan.path[plan.path.size()-1].coordY();
      V.first = objPos.coordX(); V.second = objPos.coordY();
      plannedInitToObjPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
    }
    
    $nodeclass DisplayPathObjToDest : StateNode : doStart {
      $reference Pilot::plan;
      $reference PushObjectMachine::endPos;
      
	  //std::cout << "--------DisplayPathObjToDest" << std::endl;
      // Display path from obj to dest
      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("p", U, V, rgb(0,255,255)));
      }
      U.first = plan.path[plan.path.size()-1].coordX(); U.second = plan.path[plan.path.size()-1].coordY();
      V.first = endPos.coordX(); V.second = endPos.coordY();
      plannedObjToDestPath->add(new GraphicsData::LineElement("p", U, V, rgb(0,255,255)));
    }

    $nodeclass WalkBackward : WaypointWalkNode : doStart {
	  //std::cout << "--------WalkBackward" << std::endl;
      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();

#ifdef TGT_IS_CREATE
      wp_acc->addEgocentricWaypoint(-backupDist+robotDiam, 0,  0, true, pushForwardSpeed, 0.f);
#else
      wp_acc->addEgocentricWaypoint(-backupDist+robotDiam, 0,  0, true, pushForwardSpeed, 0.f);
#endif
      motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
    }
    
    $nodeclass WalkForward : WaypointWalkNode : doStart {
	  //std::cout << "--------WalkForward" << std::endl;
      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();

#ifdef TGT_IS_CREATE
      wp_acc->addEgocentricWaypoint(backupDist-robotDiam, 0,  0, true, pushForwardSpeed, 0.f);
#else
      wp_acc->addEgocentricWaypoint(backupDist-robotDiam, 0,  0, true, pushForwardSpeed, 0.f);
#endif
      motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);				
    }
    
    $nodeclass FetchObj : WaypointWalkNode : doStart {
      $reference PushObjectMachine::objPos;
      
      std::cout << "--------Fetch the object" << std::endl;
      std::cout << "curPos: " << theAgent->getCentroid() << ", curHead: " << theAgent->getOrientation() << std::endl;
      PilotRequest &req = *VRmixin::pilot->curReq;
      req.collisionAction = PilotTypes::collisionIgnore;
      
      AngSignPi turnAng;
      float dist;
      if ( req.objectMatcher == NULL ) {
        //std::cout << "No objectMatcher specified" << std::endl;
	Point curPos = theAgent->getCentroid();
        AngTwoPi curHead = theAgent->getOrientation();
        Point vec = objPos - curPos;
        float x = vec.coordX(), y = vec.coordY();
        turnAng = AngSignPi(std::atan2(y, x)) - curHead;
        dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
      } else {
	ShapeRoot localObj = (*req.objectMatcher)(req.objectShape);
	if ( ! localObj.isValid() ) {
	  std::cout << "Can't find the object" << std::endl;
	  Point curPos = theAgent->getCentroid();
          AngTwoPi curHead = theAgent->getOrientation();
          Point vec = objPos - curPos;
          float x = vec.coordX(), y = vec.coordY();
          turnAng = AngSignPi(std::atan2(y, x)) - curHead;
          dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
	  //postStateFailure();
	  //return;
	} else {
	  Point ctPt = localObj->getCentroid();
          float x = ctPt.coordX(), y = ctPt.coordY();
          turnAng = AngSignPi(std::atan2(y, x));
          dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
        }
      }
      
      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();
#ifdef TGT_IS_CREATE
      std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
      //std::cout << "Turn " << turnAng << " radians" << std::endl;
      wp_acc->addEgocentricWaypoint(0, 0, turnAng, true, 0.f, pushTurnSpeed);
      std::cout << "Walk forward " << dist << " mm" << std::endl;
      wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
#else
      std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
      //std::cout << "Turn " << turnAng << " radians" << std::endl;
      wp_acc->addEgocentricWaypoint(0, 0, turnAng, true, 0.f, pushTurnSpeed);
      std::cout << "Walk forward " << dist << " mm" << std::endl;
      wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
#endif
      motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
    }
    
    $nodeclass AdjustPush : WaypointWalkNode : doStart {
      $reference PushObjectMachine::endPos;
      
      std::cout << "--------Adjust before pushing finish" << std::endl;
      std::cout << "curPos: " << theAgent->getCentroid() << ", curHead: " << theAgent->getOrientation() << std::endl;
      PilotRequest &req = *VRmixin::pilot->curReq;
      req.collisionAction = PilotTypes::collisionIgnore;
      
      Point curPos = theAgent->getCentroid();
      AngTwoPi curHead = theAgent->getOrientation();
      Point vec = endPos - curPos;
      float x = vec.coordX(), y = vec.coordY();
      AngSignPi turnAng = AngSignPi(std::atan2(y, x)) - AngSignPi(curHead);
      float dist = std::sqrt(x*x+y*y) - robotDiam - objSize;
      
      MMAccessor<WaypointWalkMC> wp_acc(getMC());
      wp_acc->getWaypointList().clear();
#ifdef TGT_IS_CREATE
      std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
      //std::cout << "Turn " << turnAng << " radians" << std::endl;
      wp_acc->addEgocentricWaypoint(0, 0, turnAng, true, 0.f, pushTurnSpeed);
      std::cout << "Walk forward " << dist << " mm" << std::endl;
      wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
#else
      std::cout << "Turn " << float(turnAng)/M_PI*180.f << " degree" << std::endl;
      //std::cout << "Turn " << turnAng << " radians" << std::endl;
      wp_acc->addEgocentricWaypoint(0, 0, turnAng, true, 0.f, pushTurnSpeed);
      std::cout << "Walk forward " << dist << " mm" << std::endl;
      wp_acc->addEgocentricWaypoint(dist, 0, 0, true, pushForwardSpeed, 0.f);
#endif
      motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
    }

    $nodeclass CheckObjectPos : VisualRoutinesStateNode : doStart {
      $reference PushObjectMachine::endPos;
      
	  //std::cout << "--------CheckObjectPos" << std::endl;
      PilotRequest &req = *VRmixin::pilot->curReq;
      if ( req.objectMatcher == NULL ) {
	//std::cout << "No objectMatcher specified" << std::endl;
	VRmixin::pilot->curReq->objectShape->setPosition(endPos);
	postStateSuccess();
      } else {
	ShapeRoot localObj = (*req.objectMatcher)(req.objectShape);
	if ( ! localObj.isValid() ) {
	  std::cout << "Can't find the object" << std::endl;
	  postStateFailure();
	} else {
	  localObj->applyTransform(VRmixin::mapBuilder->localToWorldMatrix, unspecified);
	  Point pt = Point(localObj->getCentroid().coordX(), localObj->getCentroid().coordY(), 0, allocentric);
	  std::cout << "end position: " << pt << std::endl;
	  VRmixin::pilot->curReq->objectShape->setPosition(pt);
	  if (pt.xyDistanceFrom(endPos) < 100) {
	    std::cout << "Push finish" << std::endl;
	    postStateSuccess();
	  }
	  else {
	    std::cout << "To much error on end position" << std::endl;
	    postStateSuccess();
	  }
	}
      }
    }

    $nodeclass MoveObjectPos : VisualRoutinesStateNode : doStart {
      $reference PushObjectMachine::endPos;

	  //std::cout << "--------MoveObjectPos" << std::endl;
      //Should using vision first
      VRmixin::pilot->curReq->objectShape->setPosition(endPos);
    }

    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: ClearOldPath =N=> SetUpObjToDest =N=>
	  planpath3: PlanPath
	  planpath3 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::START_COLLIDES)=> ReportCompletion(startCollides)
	  planpath3 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::END_COLLIDES)=> ReportCompletion(endCollides)
	  planpath3 =S<GenericRRTBase::PlanPathResultCode>(GenericRRTBase::MAX_ITER)=> ReportCompletion(noPath)
	  planpath3 =C=> plandelivery2
	  planpath3 =F=> ReportCompletion  // if executePath == false, we're done

	  // constructnavplan1 : ConstructNavPlan
	  // constructnavplan1 =C=> {execute, collisionDispatch}
	  
	  plandelivery2: AddBackupPt =S=> addacquire: AddAcquirePt =S=> SetUpObjToDest2 =N=> planpath2
	  plandelivery2 =F=> ReportCompletion(noSpace)
	  addacquire =F=> ReportCompletion(noSpace)
	  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
	  
	  // Plan path from initial position to object acquire point
	  planacquire: ChoosePathInitToObj
	  planacquire =S=> DisplayStraightLineInitToObj =N=> lookforobj
	  planacquire =F=> 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=> execute
	  planpath1 =F=> ReportCompletion  // if executePath == false, we're done

	  execute: ConstructNavPlan =C=> DisplayPathInitToObj =N=> ExecutePlan =C=> lookforobj
	  
	  lookforobj: LookForObject
	  lookforobj =F=> PostureNode("Push.pos") =C=> fetch
	  lookforobj =C=> PostureNode("Push.pos") =C=> fetch
	  
	  fetch: FetchObj =C=> planpush: ChoosePathObjToDest
	  planpush =S=> DisplayStraightLineObjToDest =N=> adjustpush
	  planpush =F=> SetUpExecute2 =N=> execute2
	  execute2: ConstructNavPlan =C=> DisplayPathObjToDest =N=> ExecutePlan =C=> adjustpush  // can't use localize2
	  
	  // using vision to adjust and localize at the end?
	  //localize2: SetupLandmarkExtractor =N=>
	  //locutil2: LocalizationUtility
	  //locutil2 =S=> laststep
	  //locutil2 =F=> laststep  // if we can't localize, use last known position; maybe say something first?
	  
	  //adjustpush: AdjustPush =C=> MoveTheObject =N=> WalkBackward[setMC(VRmixin::pilot->waypointwalk_id)] =C=> ReportCompletion
	  adjustpush: AdjustPush =C=> WalkBackward[setMC(VRmixin::pilot->waypointwalk_id)] =C=> lookforobj2
	  
	  lookforobj2: LookForTarget
	  lookforobj2 =F=> PostureNode("Push.pos") =C=> checkobj
	  lookforobj2 =C=> PostureNode("Push.pos") =C=> checkobj
	  
	  checkobj: CheckObjectPos
	  checkobj =S=> complete
	  checkobj =F=> complete
	  complete: 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
    }		
  }


  $nodeclass* SetOdometryMachine : StateNode : turn1_(NULL), turn2_(NULL) {

    $nodeclass TurnHead(float angle) : HeadPointerNode, CurrVisualOdometry {
    
      virtual void preStart() {
	HeadPointerNode::preStart();
 #ifdef TGT_HAS_HEAD
	getMC()->setMaxSpeed(RobotInfo::PanOffset, 1.5f);
 #endif
      }

      virtual void doStart() {
	HeadPointerNode::doStart();
	update(true);
 #ifdef TGT_HAS_HEAD
	getMC()->setJointValue(RobotInfo::PanOffset, mathutils::deg2rad(angle));
 #endif
      }

      virtual void doEvent() {
	HeadPointerNode::doEvent();
	postStateCompletion();
      }

      virtual void doStop() {
	update(true);
	std::cout << "Translation: " << getTranslation() << std::endl;
      }
    }

    TurnHead *turn1_;
    TurnHead *turn2_;

    virtual void doStop() {
      // CreateMotionModel<LocalizationParticle> *model = 
      //   (CreateMotionModel<LocalizationParticle>*)VRmixin::particleFilter->getMotionModel();
      // model->getOdo().setConversionParameters(turn1_->getTranslation()/15.0f, 0.0f);
      if ( VRmixin::imageOdometry != NULL ) {
	std::cout << "Setting visual odometry parameters" << std::endl;
	VRmixin::imageOdometry->setConversionParameters(turn1_->getTranslation()/5.0f, 0.0f);
      }
    }

    virtual void setup() {
      $statemachine {
        turn1: TurnHead(5.0f)
	turn2: TurnHead(0.0f)

	turn1 =T(2000)=> turn2 =T(2000)=> ReportCompletion
      }

      turn1_ = turn1;
      turn2_ = turn2;
    }
  }


  //================ 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),
    setOdometryMachine_(NULL) {

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

    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;

      case setOdometry:
	setOdometryMachine_->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

	setOdometryMachine: SetOdometryMachine =C=> reportSuccess
      }

      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;
      setOdometryMachine_ = setOdometryMachine;
    }
  }

  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