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

#include <queue>

#include "Behaviors/Nodes/WaypointWalkNode.h"
#include "Behaviors/Nodes/HeadPointerNode.h"

#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Crew/PilotRequest.h"
#include "Crew/MapBuilderNode.h"

#include "Localization/MarkerSensorModel.h"
#include "Motion/MMAccessor.h"
#include "Motion/WaypointWalkMC.h"
#include "Motion/WaypointEngine.h"
#include "Motion/WaypointList.h"
#include "Planners/Navigation/ShapeSpacePlanner.h"


#include "DualCoding/DualCoding.h"

using namespace std;

namespace DualCoding {

	class Pilot : public StateNode  {
  public:
    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 PVqueued = 1<<7;       

    static PilotVerbosity_t getVerbosity() { return verbosity; }
    static void setVerbosity(PilotVerbosity_t v) { verbosity = v; }
	  
	/*struct Path {
		  Path() : path() , localizationPoints() {};
	  	  std::vector<DualCoding::Point> path;
		  std::vector<DualCoding::Point> localizationPoints;
      }pathToTake;*/
		
	enum NavigationTaskType {
		localize,
		travel
	};
		
	struct NavigationTask {
		NavigationTask(): wayPoint(), targetCenter(),mapreq(), theta() {}; 
		DualCoding::Point wayPoint;
		DualCoding::Point targetCenter;
		DualCoding::MapBuilderRequest mapreq;
		AngTwoPi theta;
		NavigationTaskType navType;
	};
		
	struct NavigationPlan {
		NavigationPlan() : path(), navPlan() {};
		std::vector<DualCoding::Point> path;
		std::vector<NavigationTask> navPlan;
	}plan;
  
    Pilot() : StateNode("Pilot"), plan(),
	      requests(), curReq(NULL), idCounter(0), 
	      waypointwalk_id(MotionManager::invalid_MC_ID), dispatch_(NULL)
    {}

    virtual void doStart() {
      if ( verbosity & PVstart)
	cout << "Pilot starting up: waypointwalk_id=" << waypointwalk_id << endl;
      addNode(new RunMotionModel)->start();
    }

    virtual void doStop() {
      if ( verbosity & PVstart )
	cout << "Pilot is shutting down." << endl;
      abort();
      motman->removeMotion(waypointwalk_id);
      waypointwalk_id = MotionManager::invalid_MC_ID;
      // The following teardown() call is necessary because setup
      // hard-wires the MC_ID for all the walk nodes.  If we leave the
      // waypoint walk motion command active when Tekkotsu shuts down,
      // it crashes and generates a nasty backtrace.  But if we remove
      // the motion command, there's no easy way to put it back because
      // setup won't be called again unless we do this teardown() call.
      teardown();
    }

    virtual void teardown() {
      StateNode::teardown();
    }

    virtual void doEvent() {
      if ( event->getGeneratorID() == EventBase::timerEGID && event->getSourceID() == 9999 )
	executeRequest();
      else cout << "Pilot got unexpected event: " << event->getDescription() << endl;
    }

    void unwindForNextRequest() {
      erouter->addTimer(this,9999,1,false); // allow time to unwind the event stack before executing next request
    }
	
    //! Pilot's setAgent is called by its RunMotionModel behavior.  Or user can call directly.
    void setAgent(const Point &loc, AngTwoPi heading, bool quiet=false, bool updateWaypoint=true) {
      if ( updateWaypoint ) {
	MMAccessor<WaypointWalkMC> wp_acc(waypointwalk_id);
	wp_acc->setCurPos(loc.coordX(), loc.coordY(), heading);
      }
      VRmixin::mapBuilder->setAgent(loc, heading, quiet);
      // Setting the best particle (below) is a no-op if setAgent was
      // invoked by RunMotionModel, but not if the user called
      // setAgent directly.
      VRmixin::particleFilter->setBestParticle();
    }

    unsigned int executeRequest(BehaviorBase* requestingBehavior, const PilotRequest& req) {
      requests.push(new PilotRequest(req));
      const unsigned int reqID = ++idCounter;
      requests.back()->requestID = reqID;
      requests.back()->requestingBehavior = requestingBehavior;
      if ( curReq == NULL )
	unwindForNextRequest();
      else if ( verbosity & PVqueued )
	cout << "Pilot request " << requests.back()->requestID << " queued." << endl;
      return reqID;
    }

    void executeRequest() {
      if ( curReq == NULL && !requests.empty() ) {
	curReq = requests.front();
	if ( verbosity & PVexecute )
	  cout << "Pilot executing request " << curReq->requestID << endl;
	VRmixin::particleFilter->setBestParticle();
	dispatch_->start();
      }
    }

    void requestComplete(const bool result) {
      if ( curReq == NULL ) {
	cout << "Pilot::requestComplete had NULL curReq !!!!!!!!!!!!!!!!" << endl;
	return;
      }
      //if ( curReq->trackRequest != NULL )
      //  VRmixin::lookout.stopTrack();
      dispatch_->finish();
      const size_t reqID = curReq->requestID;
      const BehaviorBase* requester = curReq->requestingBehavior;
      delete curReq->mapBuilderRequest;
      delete curReq;
      curReq = NULL;
      requests.pop();
      // cout << "*** Pilot dropping waypointwalk priority" << endl;
      motman->setPriority(waypointwalk_id,MotionManager::kIgnoredPriority);
      // motman->setPriority(posture_id,MotionManager::kIgnoredPriority);
      if ( verbosity & PVcomplete )
	cout << "Pilot request " << reqID << " complete." << endl;
      erouter->postEvent(EventBase(EventBase::pilotEGID, reqID,
				   EventBase::statusETID, 0, "Pilot Completion", float(result)));
      erouter->postEvent(EventBase(EventBase::pilotEGID,
				   (requester==NULL) ? reqID : (size_t)requester, 
				   EventBase::deactivateETID, 0, "Pilot Completion", float(result)));
      unwindForNextRequest();
    }

    void abort() {
      if ( verbosity & PVabort )
	cout << "Pilot aborting." << endl;
      dispatch_->stop();
      curReq = NULL;
      while (!requests.empty()) {
	delete requests.front();
	requests.pop();
      }
      motman->setPriority(waypointwalk_id,MotionManager::kIgnoredPriority);
    }

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

    class RunMotionModel : public StateNode {
      virtual void doStart(){
	//cout<<"RunMotionModel doStart Begun\n";
	erouter->addTimer(this, 1, 250, true);
      }
  
      virtual void doEvent(){
	if ( event->getGeneratorID() == EventBase::timerEGID) {
	  //cout<<"RunMotionModel doEvent begun \n"; 
	  VRmixin::particleFilter->updateMotion();
	  const PFShapeLocalization::particle_type &best = VRmixin::particleFilter->getBestParticle();
		//std::cout<<"best particle x: "<<best.x<<" y: "<<best.y<<" theta: "<<best.theta<<std::endl;
	  VRmixin::pilot->setAgent(Point(best.x,best.y,0,allocentric), best.theta, true, false);
	}
      } 
    };

#shortnodeclass ReportSuccess : StateNode : doStart
    if ( Pilot::getVerbosity() & Pilot::PVsuccess )
      cout << "Pilot result: Success" << endl;
    VRmixin::pilot->requestComplete(true);

#shortnodeclass ReportFailure : StateNode : doStart
    if ( Pilot::getVerbosity() & Pilot::PVfailure )
      cout << "Pilot result: Failure" << endl;
    VRmixin::pilot->requestComplete(false);

#shortnodeclass WalkMachine : WaypointWalkNode : doStart
    PilotRequest &req = *VRmixin::pilot->curReq;
    MMAccessor<WaypointWalkMC> wp_acc(getMC());
    wp_acc->getWaypointList().clear();
#if defined(TGT_CREATE) || defined(TGT_CALLIOPE)
    float vx = (req.fwdSpeed > 0) ? req.fwdSpeed : 50.f;
    float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
    if ( req.da != 0 ) {
      wp_acc->addEgocentricWaypoint(0, 0, req.da, true, vx, va);
    }
    if ( req.dx != 0 ) {
      wp_acc->addEgocentricWaypoint(req.dx, 0, (req.dx >= 0) ? 0 : -M_PI , true, vx, va);
    }
#else
    float vx = (req.fwdSpeed > 0) ? req.fwdSpeed : 15.f;
    float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
    wp_acc->addEgocentricWaypoint(req.dx, req.dy, req.da, true, vx, va);
#endif
    // cout << "*** Pilot raising waypointwalk priority" << endl;
    motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
    wp_acc->go();
    

#nodeclass WaypointWalkMachine : WaypointWalkNode : doStart
    PilotRequest &req = *VRmixin::pilot->curReq;
    MMAccessor<WaypointWalkMC> wp_acc(getMC());
    if(req.clearWaypoints)
      wp_acc->clearWaypointList();
    wp_acc->appendWaypoints(*req.waypointList);
    wp_acc->go();
#endnodeclass
		

#nodeclass GoToShapeMachine : VisualRoutinesStateNode : nextLocation()
	// distance in mm that we can get within a given point and be "there"
	static const float ALLOWABLE_DISTANCE_ERROR = 50;
	static const float ALLOWABLE_ANGULAR_ERROR = 0.15;
		
	struct Location {
		Location() : nextPoint(), angleToTurn(0) {};
		fmat::Column<2> nextPoint;
		AngSignPi angleToTurn;
	} nextLocation;
		
		static bool isLandmarkViewable(DualCoding::Point *currentPoint, AngTwoPi orientation, DualCoding::Point centerPoint){
			//check if there is a marker in view to localize on after you turn
			float bestDistance = 1000000;
			Shape<MarkerData> landMark;
			
			NEW_SHAPEVEC(landmarks, MarkerData, select_type<MarkerData>(worldShS));
			DualCoding::Point pR = *currentPoint;
			std::cout<<"length of landmarks : "<<landmarks.size()<<std::endl;
			GET_SHAPE(worldBounds, PolygonData, worldShS);
			SHAPEVEC_ITERATE(landmarks, MarkerData, lm) {
				DualCoding::Point pM = lm->getCentroid();
				fmat::SubVector<2> heading = fmat::SubVector<2>(pM.coords) - fmat::SubVector<2>(pR.coords);
				AngSignPi angle = (atan2(heading[1], heading[0]) - orientation);
				
				// is the camera within our possible field of view (in front of us)?
				if (fabs(angle) > Pi/2.0)
					continue;
				
				// are we further away than the closest one so far?
				if (pR.xyDistanceFrom(pM) >= bestDistance)
					continue;
				
				// do we intersect with the world bounds when looking at marker?
				LineData lineOfSight(worldShS, pR, pM);
				if (worldBounds.isValid() && worldBounds->intersectsLine(lineOfSight))
					continue;
				
				// this marker is viewable and the closest so far!
				landMark = lm;
				bestDistance = pR.xyDistanceFrom(pM);							
			} END_ITERATE;


			// there's a marker in sight!
			if (landMark.isValid()) {
				// get the point in local space
				Shape<MarkerData> localCopy = mapBuilder->importWorldToLocal(landMark);
				centerPoint = localCopy->getCentroid(); 
				localShS.deleteShape(localCopy);
				return true;
			}
			else 
				return false;
		};
	
	#shortnodeclass Punt : StateNode : doStart
		postParentCompletion();		
		
    #nodeclass PlanPath : VisualRoutinesStateNode : doStart
		PilotRequest &req = *VRmixin::pilot->curReq;
		const ShapeRoot &target = req.targetShape;
		
		if ( ! target.isValid() ) {
			postStateFailure();
			return;
		}
		
	    const Point targetLoc = target->getCentroid();
		std::cout << "Planning path to point: " << targetLoc << std::endl;
		
		GET_SHAPE(worldBounds, PolygonData, worldShS);
		ShapeSpacePlanner planner = ShapeSpacePlanner(worldShS, worldBounds, 255);
		// get current location
		DualCoding::Point position = theAgent->getCentroid();
	    NavigationPlan &plan = getParent()->getParent()->parentAs<Pilot>()->plan;
		plan.path = planner.planPath(position, targetLoc);

		if ( plan.path.size() > 0 ) {
			NEW_SHAPE(plannedPath, PolygonData, new PolygonData(worldShS, plan.path, false));
			plannedPath->setColor(rgb(0,0,255));

			if (req.executePath) {
				postStateCompletion();
				return;
			} else {
				postStateFailure();
				return;
			}
		} else {
			std::cout<<"Collision Detected\n";
			postStateFailure();
		}
    #endnodeclass
		
	#nodeclass AnalyzePath : VisualRoutinesStateNode : doStart
		NavigationPlan &plan = getParent()->getParent()->parentAs<Pilot>()->plan;
		const int maxDistanceBetween = 500;
		const float maxAngleBetween = 0;
		AngTwoPi orientation = theAgent->getOrientation();
		DualCoding::Point currentPoint(theAgent->getCentroid());
		std::cout<<"planned path\n";
		for(int j = 0; j<plan.path.size(); j++){
			std::cout<<plan.path[j]<<"\t";
		}std::cout<<"\n";
		
		//! If the size of the path is zero then do not add any localization points
		if (plan.path.size() == 0) {
			postStateFailure();
			return;
		}
		
		std::cout<<"length of path is : "<<plan.path.size()<<std::endl;
		for (unsigned int i = 1; i<plan.path.size(); i++) {
			//std::cout<<"current point: "<<currentPoint<<"\n";
			std::cout<<"current orientation: "<<orientation<<"\n";

			// Find angle between current point and next point
			fmat::Column<3> p0 = currentPoint.getCoords();
			fmat::Column<3> p1 = plan.path[i].getCoords();
			fmat::Column<2> next = fmat::SubVector<2>(p1) - fmat::SubVector<2>(p0);
			// If the angle between the two points is great enough to require the robot to turn add it as an localization point
			AngSignPi angleBetween = atan2(next[1], next[0]) - orientation;
			//std::cout<<"angleBetween : "<<angleBetween<<'\n';
			
			if (fabs(angleBetween) > maxAngleBetween) {
			//	std::cout<<"adding a turning point to  navPlan\n";
				NavigationTask navTask;
				navTask.navType = travel;
				navTask.theta = angleBetween;
				orientation = orientation+angleBetween;
				plan.navPlan.push_back(navTask);
				
				// Check if there is a marker viewable at this point
				DualCoding::Point *tempLocation = &currentPoint;
				DualCoding::Point centerPoint;
				
				if (isLandmarkViewable(tempLocation, orientation, centerPoint)) {
					NavigationTask task;
				//	std::cout<<"adding localization point to navPlan\n";
					task.navType = localize;
					task.targetCenter = centerPoint;
					plan.navPlan.push_back(task);
				}
			} 
					
		    //Check the distance from the current point to the next
			float distanceBetween = currentPoint.xyDistanceFrom(plan.path[i]);
			//std::cout<<"distanceBetween :"<<distanceBetween<<'\n';
		
			if(distanceBetween > maxDistanceBetween){
				// estimate the number of times to localize 
				int numberOfLocalizations = distanceBetween/maxDistanceBetween;
				int remainder = (int)distanceBetween%maxDistanceBetween;
				// if the rest of the Distance is not far enough
				if (remainder < maxDistanceBetween/2)
					--numberOfLocalizations;
				
				if (numberOfLocalizations > 0) {
					DualCoding::Point lastPoint(currentPoint.coordX(), currentPoint.coordY());
					int xStep = (plan.path[i].coordX() - currentPoint.coordX())/numberOfLocalizations;
					int yStep = (plan.path[i].coordY() - currentPoint.coordY())/numberOfLocalizations;
					
					for (int i = 1; i>numberOfLocalizations; i++) {
						// find the next localization point between the two points in the path
						DualCoding::Point nextPoint(xStep+lastPoint.coordX(),yStep+lastPoint.coordY());
						NavigationTask navTask;
						navTask.navType = travel;
					
						if (i!=(numberOfLocalizations - 1)) {
							navTask.wayPoint=nextPoint;
						} else {
							navTask.wayPoint = plan.path[i];
						}
						
						//std::cout<<"adding a waypoint to navPlan\n";
						plan.navPlan.push_back(navTask);

						// Check if there is a marker viewable at this point
						DualCoding::Point *tempLocation = &currentPoint;
						DualCoding::Point centerPoint;
						
						if (isLandmarkViewable(tempLocation, orientation, centerPoint)) {
							NavigationTask task;
							//std::cout<<"adding localization point to navPlan\n";
							task.navType = localize;
							task.targetCenter = centerPoint;
							plan.navPlan.push_back(task);
						}
						lastPoint = nextPoint;
					}// end for
				} else {
					//std::cout<<"adding a waypoint to navPlan\n";
					NavigationTask navtask;
					navtask.navType = travel;
					navtask.wayPoint = plan.path[i];
					plan.navPlan.push_back(navtask);
					
					// Check if there is a marker viewable at this point
					DualCoding::Point *tempLocation = &currentPoint;
					DualCoding::Point centerPoint;
					
					if (isLandmarkViewable(tempLocation, orientation, centerPoint)) {
						NavigationTask task;
						//std::cout<<"adding localization point to navPlan\n";
						task.navType = localize;
						task.targetCenter = centerPoint;
						plan.navPlan.push_back(task);
					}
				}
			} else {
				//std::cout<<"adding a waypoint to navPlan\n";
				NavigationTask navtask;
				navtask.navType = travel;
				navtask.wayPoint = plan.path[i];
				plan.navPlan.push_back(navtask);
				
				// Check if there is a marker viewable at this point
				DualCoding::Point *tempLocation = &currentPoint;
				DualCoding::Point centerPoint;
				
				if (isLandmarkViewable(tempLocation, orientation, centerPoint)) {
					NavigationTask task;
					//std::cout<<"adding localization point to navPlan\n";
					task.navType = localize;
					task.targetCenter = centerPoint;
					plan.navPlan.push_back(task);
				}
			}
			currentPoint = plan.path[i];
		}
		postStateCompletion();
	#endnodeclass
		
	#nodeclass ExecutePlan : VisualRoutinesStateNode : doStart
		/*NavigationPlan &plan = getParent()->getParent()->parentAs<Pilot>()->plan;

		postStateCompletion();*/
		// check to see if we're close to the next waypoint
		DualCoding::Point position = theAgent->getCentroid();
		NavigationPlan &plan = getParent()->getParent()->parentAs<Pilot>()->plan;
		
		for (int i = 0; i<plan.navPlan.size(); i++) {
			std::cout<<"i = : "<<i<<" NavType :"<<plan.navPlan[i].navType<<std::endl;
		}
		
		if (plan.navPlan.size() == 0) {
			//std::cout << "Warning: empty path in FollowPathNode::CheckLocation::doStart" << std::endl;
			postStateCompletion();
			return;
		}
		
		//localization or travel
	   if (plan.navPlan[0].navType == travel) {
		   
		   while (position.xyDistanceFrom(plan.navPlan[0].wayPoint) < ALLOWABLE_DISTANCE_ERROR) {
				std::cout << "Point " << plan.navPlan[0].wayPoint << " reached." << std::endl;
				plan.navPlan.erase(plan.navPlan.begin(),plan.navPlan.begin()+1);
			   
			    if (plan.navPlan[0].navType == localize)
					return;
			   
				if (plan.navPlan.size() == 0) {
					postStateCompletion(); 
					return;
				}
			}
		   
			fmat::Column<3> pos = position.getCoords();
			fmat::Column<3> next = plan.navPlan[0].wayPoint.getCoords();
		    Location &nextLocation = parentAs<GoToShapeMachine>()->nextLocation;
		    nextLocation.nextPoint = fmat::SubVector<2>(next) - fmat::SubVector<2>(pos);
		    nextLocation.angleToTurn = (atan2(nextLocation.nextPoint[1], nextLocation.nextPoint[0]) - theAgent->getOrientation());

			//cout << "Heading to next point: " << nextLocation.nextPoint << endl;
			// are we facing the next point?
			if (fabs(nextLocation.angleToTurn) > ALLOWABLE_ANGULAR_ERROR) {
				//	cout << "Turning to face next point!" << endl;
				//	cout << "Angle is: " << nextLocation.angleToTurn << endl;
				postStateSignal(true);
				return;
			} else {
				//	cout << "Advancing to next point!" << endl;
				nextLocation.nextPoint *= fmin( 100.0/nextLocation.nextPoint.norm(), 1.0 );
				postStateSignal(false);
			}
		} else {
			postStateSignal(plan.navPlan[0].targetCenter);
			} 
		#endnodeclass
			
		#nodeclass PointHead : HeadPointerNode : doStart
			NavigationPlan &plan = getParent()->getParent()->parentAs<Pilot>()->plan;
			getMC()->lookAtPoint(plan.navPlan[0].targetCenter.coordX(), plan.navPlan[0].targetCenter.coordY(), plan.navPlan[0].targetCenter.coordZ());
			postStateCompletion();
		#endnodeclass
		
		#nodeclass MapRequest : VisualRoutinesStateNode : doStart
			PilotRequest &req = *VRmixin::pilot->curReq;			
			bool camera, local;
			camera = local = false;
		
			for (int i = 0; i< req.landmarks.size(); i++){
				if (req.landmarks[i].localizeByCamera()) {
					camera = true;
				} else {
					local = true;
				}	
				
				if (camera == true && local == true) {
					MapBuilderRequest mapreq(MapBuilderRequest::cameraMap);
					return;
				}
			}
			if (camera = true) {
				MapBuilderRequest mapreq(MapBuilderRequest::cameraMap);
				return;
			} else {
				MapBuilderRequest mapreq(MapBuilderRequest::localMap);
			}
		#endnodeclass
			
		#nodeclass TakeSnapshot(DualCoding::MapBuilderRequest mapreq) : VisualRoutinesStateNode : doStart
			//get the sensor model for the type of landmark to use for localization
			//current version works with objects who's parent class is MarkerData
			mapBuilder->executeRequest(mapreq);
			NEW_SHAPEVEC(markers, MarkerData, select_type<MarkerData>(camShS));
			LocalizationParticle pre = VRmixin::particleFilter->getBestParticle();
			VRmixin::particleFilter->updateSensor(true);
		 	VRmixin::particleFilter->update();
			LocalizationParticle post = VRmixin::particleFilter->getBestParticle();
			//filter.displayParticles(worldShS);
			// update the agent location
			pilot->setAgent(Point(post.x,post.y), post.theta);

			postStateCompletion();
	#endnodeclass
		
 	#nodeclass Walk : WaypointWalkNode : doStart
		PilotRequest &req = *VRmixin::pilot->curReq;
		Location &nextLocation = parentAs<GoToShapeMachine>()->nextLocation;
		MMAccessor<WaypointWalkMC> wp_acc(getMC());
		wp_acc->getWaypointList().clear();
		bool turnOnly = extractSignal<bool>(event);

		#if defined(TGT_CREATE) || defined(TGT_CALLIOPE)
				float vx = (req.fwdSpeed > 0) ? req.fwdSpeed : 50.f;
				float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
				//std::cout<<"vx : "<<vx<<" va : "<<va<<" turnOnly : "<<turnOnly<<std::endl;
				if ( turnOnly ) {
					wp_acc->addEgocentricWaypoint(0, 0, fmin(nextLocation.angleToTurn, 0.5), true, vx, va);
				}
				else {
					//std::cout<<"nexPoint.norm() : "<<nextLocation.nextPoint.norm()<<std::endl;
					wp_acc->addEgocentricWaypoint(nextLocation.nextPoint.norm(), 0,  0, true, vx, va);
				}
		#else
				float vx = (req.fwdSpeed > 0) ? req.fwdSpeed : 15.f;
				float va = (req.turnSpeed > 0) ? req.turnSpeed : 0.1f;
		        if ( turnOnly ) {
					wp_acc->addEgocentricWaypoint(0, 0, fmin(nextLocation.angleToTurn, 0.5), true, vx, va);
				}
				else {
					wp_acc->addEgocentricWaypoint(nextLocation.nextPoint.norm(), 0,  0, true, vx, va);
				}
		#endif
		// cout << "*** Pilot raising waypointwalk priority" << endl;
		motman->setPriority(VRmixin::pilot->getWaypointwalk_id(),MotionManager::kStdPriority);
		wp_acc->go();
	#endnodeclass
		
	#nodemethod setup
		MapBuilderRequest mapreq(MapBuilderRequest::localMap);
		mapreq.markerTypes = MarkerData::allMarkerTypes();
		mapreq.immediateRequest = true;
		mapreq.rawY = true;
		mapreq.addAllObjectColors(markerDataType);
		mapreq.immediateRequest = true;
		mapreq.clearVerbosity = MapBuilder::MBVprojectionFailed;
		#statemachine
			plan: PlanPath =C=> analyze: AnalyzePath
			plan =F=> Punt
		
			analyze =C=> execute: ExecutePlan
			analyze =F=> Punt

			execute =C=> Punt
		#endstatemachine
	#endnodemethod
#endnodeclass //GoToShapeMachine

#nodeclass VisualSearchMachine : StateNode
#nodeclass Search : MapBuilderNode : doStart
    if ( pilot->curReq->mapBuilderRequest == NULL ) {
      cout << "Pilot received visualSearch request with invalid mapBuilderRequest field" << endl;
      pilot->requestComplete(false);
    }
    else if ( pilot->curReq->exitTest == NULL ) {
      cout << "Pilot received visualSearch request with no exitTest" << endl;
      pilot->requestComplete(false);
    }
    mapreq = *pilot->curReq->mapBuilderRequest;
#endnodeclass

#shortnodeclass Check : StateNode : doStart
    if ( VRmixin::pilot->curReq->exitTest() )
      VRmixin::pilot->requestComplete(true);
    else if ( VRmixin::pilot->curReq->searchRotationAngle == 0 )
      VRmixin::pilot->requestComplete(false);
    else
      postStateCompletion();
		 
#shortnodeclass Rotate : WaypointWalkNode : doStart
    MMAccessor<WaypointWalkMC> wp_acc(getMC());
    wp_acc->getWaypointList().clear();
    wp_acc->addEgocentricWaypoint(0,0,VRmixin::pilot->curReq->searchRotationAngle,true,0.01f);
    wp_acc->go();

#shortnodemethod setup   // set up the VisualSearchMachine
#statemachine
    startnode: Search() =MAP=> Check =C=>
    Rotate[setMC(VRmixin::pilot->waypointwalk_id)] =C=> startnode
#endstatemachine

#endnodeclass

	       // 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 : walkMachine_(NULL), goToShapeMachine_(NULL), visualSearchMachine_(NULL), waypointWalkMachine_(NULL)
    StateNode *walkMachine_, *goToShapeMachine_, *visualSearchMachine_, *waypointWalkMachine_;

    void finish() {
      postStateCompletion(); 
    }

#nodemethod doStart
    PilotRequest::PilotRequestType_t reqType = VRmixin::pilot->curReq->getRequestType();
    switch ( reqType) {
    case PilotRequest::walk:
      walkMachine_->start();
      break;
    case PilotRequest::goToShape:
      goToShapeMachine_->start();
      break;
    case PilotRequest::visualSearch:
      visualSearchMachine_->start();
      break;
    case PilotRequest::waypointWalk:
      waypointWalkMachine_->start();
      break;
    default:
      cout << "Illegal Pilot request type: " << reqType << endl;
      VRmixin::pilot->requestComplete(false);
    }
				
#nodemethod setup
#statemachine
    startnode: StateNode("Dispatch Dummy Start")
      reportSuccess: ReportSuccess
      reportFailure: ReportFailure

      walkmachine: WalkMachine[setMC(VRmixin::pilot->waypointwalk_id)] =pilot_walkmachine_succeed:C=> reportSuccess
      walkmachine =pilot_walkmachine_fail:F=> reportFailure

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

      visualsearchmachine: VisualSearchMachine =C=> reportSuccess
      visualsearchmachine =F=> reportFailure
		
      waypointwalkmachine: WaypointWalkMachine[setMC(VRmixin::pilot->waypointwalk_id)] =C=> reportSuccess
      waypointwalkmachine =F=>  reportFailure

#endstatemachine

			   // Save these statenode pointers for use by dispatch,
			   // and set up the walks to use a common motion command
      walkMachine_ = walkmachine;
    goToShapeMachine_ = gotoshapemachine;
    visualSearchMachine_ = visualsearchmachine;
    waypointWalkMachine_ = waypointwalkmachine;
#endnodemethod
#endnodeclass

    virtual void setup() {   // Pilot's setup
      SharedObject<WaypointWalkMC> waypointwalk_mc;
      waypointwalk_id = motman->addPersistentMotion(waypointwalk_mc, MotionManager::kIgnoredPriority);
#statemachine
    startnode: StateNode("Pilot Dummy Start")
	dispatch: Dispatch =pilot_dispatch:C=> startnode
#endstatemachine
	VRmixin::pilot->dispatch_ = dispatch;
    }

    static const unsigned int invalid_Pilot_ID = -1U;

  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 waypointwalk_id;
    Dispatch *dispatch_;
	  


  };  // Pilot class

}; // namespace

#endif