KoduInterpreter.h

Go to the documentation of this file.

// INCLUDES
// c++
#include <cctype>
#include <iostream>
#include <queue>
#include <vector>

// tekkotsu
#include "Behaviors/StateMachine.h"
#include "DualCoding/VRmixin.h"
#include "Motion/WalkMC.h"
#include "Events/EventRouter.h"

// tekkodu
#include "Kodu/KoduIncludes.h"
#include "Events/Kodu/KoduSayEvent.h"
#include "Events/Kodu/KoduGiveEvent.h"

// General Functions
#include "Kodu/General/GeneralFncs.h"
#include "Kodu/General/GeneralMacros.h"
#include "Kodu/General/KoduState.h"

// Kodu Parsing
#include "Kodu/Parsing/Parser.h"

#include "Kodu/Keepers/ScoreKeeper.h"
#include "Kodu/Keepers/ObjectKeeper.h"
#include "Kodu/KoduPage.h"
#include "Kodu/KoduRule.h"


class KoduInterpreter : public VisualRoutinesStateNode {
 public:
  KoduInterpreter(const std::string &nodename="KoduInterpreter") : VisualRoutinesStateNode(nodename), theWorld(NULL), thisAgent(NULL), discover(NULL), dropActRef(NULL), grabActRef(NULL), giveActRef(NULL), recvActRef(NULL), motionActRef(NULL), pageSwitchActRef(NULL), playActRef(NULL), sayActRef(NULL), scoreActRef(NULL), multiplexorRef(NULL), needToHaltExecution(false), evaluatorRef(NULL), runnerRef(NULL), koduconfig(NULL), elistener(NULL), kodustate(NULL) {}
    

    




    
    
    
  ~KoduInterpreter() {
    //std::cout << "Destructing Kodu Game...\n";
    // null the KoduAgent pointer
    thisAgent = NULL;

    // delete the world instance
    //std::cout << "Destroying Kodu World.\n";
    GeneralFncs::deletePtr(theWorld);
    GeneralFncs::deletePtr(discover);
    GeneralFncs::deletePtr(koduconfig);
    GeneralFncs::deletePtr(elistener);
    GeneralFncs::deletePtr(kodustate);

        
    //std::cout << "Kodu Game destruction complete!\n";
  }

  class KoduEventListener : public StateNode {
  private:
    Kodu::KoduState* _kodustate;
    Kodu::KoduAgent* _thisAgent;

  public:
    KoduEventListener(Kodu::KoduState* st, Kodu::KoduAgent* ka) : _kodustate(st), _thisAgent(ka) {}

    KoduEventListener(const KoduEventListener &other); //!< don't call this

    KoduEventListener& operator=(const KoduEventListener &other); //!< don't call this

    void listenTo(int hostAddr) {
      erouter->addRemoteListener(this, hostAddr, EventBase::koduEGID);
    }

    virtual void doEvent() {
      //Ignore self-messages
      if (event->getHostID() == -1) return;

      if (dynamic_cast<const KoduSayEvent*>(event) != NULL) {
        const KoduSayEvent* sayevent = dynamic_cast<const KoduSayEvent*>(event);
        Kodu::KoduState::utterance utt = { sayevent->getHostID(), sayevent->getPhrase() };
        _kodustate->addUtterance(utt);
      }
      if (dynamic_cast<const KoduGiveEvent*>(event) != NULL) {
        const KoduGiveEvent* giveevent = dynamic_cast<const KoduGiveEvent*>(event);
        if (!_thisAgent->agentIsReceiving && !_thisAgent->agentWantsToGiveObject) {
          _thisAgent->setIsReceiving();
          _thisAgent->agentReceivingFrom = giveevent->getHostID();
                    _thisAgent->gotGiveInformation = false;
        }
                else if (_thisAgent->agentWantsToGiveObject) {
          if (giveevent->getHostID() == _thisAgent->giveTargetObject->getHostAddr()) {
                        _thisAgent->gotGiveReady = true;
          }
                    else {
                        std::cout << "Give event host id (" << giveevent->getHostID() << ") differed from expected"
                                  << _thisAgent->giveTargetObject->getHostAddr() << std::endl;
                    }
                }
                else if (_thisAgent->agentIsReceiving) {
                    if (giveevent->getHostID() == _thisAgent->agentReceivingFrom) {
                        _thisAgent->gotGiveInformation = true;
                        _thisAgent->giveAngleToTurn = giveevent->getTurn();
                        _thisAgent->giveObjType = giveevent->getObjType();
                    }
                }
      }
    }

  };

  static void new_robot_callback(player_identity* ident, void* arg) {
    KoduEventListener* el = (KoduEventListener*)arg;
    el->listenTo(ident->hostAddr);
  }

  class InitializeAgent : public StateNode {
   public:
    InitializeAgent(const std::string &nodename="InitializeAgent") : StateNode(nodename) {}

    class CreateWorld : public StateNode {
     public:
      CreateWorld(const std::string &nodename="CreateWorld") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
        KoduDiscover* &discover = getAncestor<KoduInterpreter>()->discover;
        KoduConfig* &koduconfig = getAncestor<KoduInterpreter>()->koduconfig;
        KoduEventListener* &elistener = getAncestor<KoduInterpreter>()->elistener;
        Kodu::KoduState* &kodustate = getAncestor<KoduInterpreter>()->kodustate;
    
          //std::cout << "[" << getName() << "]: creating world\n";
          // create an instance of the Kodu World
          if ((theWorld = new Kodu::KoduWorld()) == NULL) {
            //std::cout << "!!! there was an error creating the World! Terminating...\n";
            postStateFailure();
            return;
          }
          if ( (kodustate = new Kodu::KoduState()) == NULL ){
            //std::cout << "!!! there was an error creating kodu state! Terminating...\n";
            postStateFailure();
            return;
          }
          // create a pointer to the agent instance in KoduWorld
          thisAgent = &(theWorld->thisAgent);
    
          if ((koduconfig = new KoduConfig()) == NULL) {
            std::cout << "!!! Couldn't create config object\n";
            postStateFailure();
            return;
          }
    
          if (koduconfig->loadFile("config/KoduConfig.xml") == 0) {
            std::cout << "!!! Couldn't load kodu config from ms/config/KoduConfig.xml\n";
            postStateFailure();
            return;
          }
    
          player_identity ident;
          ident.type = koduconfig->kodu_type;
    
          if ((discover = new KoduDiscover(ident, koduconfig->interface)) == NULL) {
            std::cout << "!!! Couldn't create discovery object\n";
            postStateFailure();
            return;
          }
    
          theAgent->setHostAddr(ident.hostAddr);
    
          if ((elistener = new KoduEventListener(kodustate, thisAgent)) == NULL) {
            std::cout << "!!! Couldn't create KoduEventListener\n";
            postStateFailure();
            return;
          }
          elistener->start();
          discover->setNewRobotCallback(new_robot_callback, (void*)elistener);
          postStateCompletion();
      }
    };

    class ParseKode : public StateNode {
     public:
      ParseKode(const std::string &nodename="ParseKode") : StateNode(nodename) {}
      virtual void doStart() {
          //std::cout << "[" << getName() << "]: parsing kode\n";
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          // parses and creates Kodu program
          if (Kodu::Parser::parseAndCreateKoduProgram(thisAgent->pages) == false) {
            std::cerr << "!!! Error parsing and creating Kodu program.\n";
            postStateFailure();
            return;
          }
          // std::cout << "parsing complete.\n";
          postStateSuccess();
      }
    };

    class LookAround : public MapBuilderNode {
     public:
      LookAround(const std::string &nodename="LookAround") : MapBuilderNode(nodename,MapBuilderRequest::worldMap) {}
      virtual void doStart() {
          std::cout << "[" << getName() << "]: looking for objects.\n";
          // search for red, blue, and green cylinders
          mapreq.addObjectColor(cylinderDataType, "red");
          mapreq.addObjectColor(cylinderDataType, "blue");
          mapreq.addObjectColor(cylinderDataType, "green");
          mapreq.addObjectColor(agentDataType, "red");
          mapreq.setAprilTagFamily();
                
          // get the gaze points from the agent
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          std::vector<Point> searchPoints(thisAgent->getGazePoints());
    
          /*
            const float kSearchRadius = 1500.0f;
            const float kZval = 300.0f;
            // now add the approx. search points for the april tags (star constellation)
            // direct left
            searchPoints.push_back(Point(cos(deg2rad( 90.0f)) * kSearchRadius,
            sin(deg2rad( 90.0f)) * kSearchRadius,
            kZval, egocentric));
    
            // forward left
            searchPoints.push_back(Point(cos(deg2rad( 35.0f)) * kSearchRadius,
            sin(deg2rad( 35.0f)) * kSearchRadius,
            kZval, egocentric));
    
            // forward
            searchPoints.push_back(Point(kSearchRadius, 0.0f, kZval, egocentric));
    
            // forward right
            searchPoints.push_back(Point(cos(deg2rad(-35.0f)) * kSearchRadius,
            sin(deg2rad(-35.0f)) * kSearchRadius,
            kZval, egocentric));
    
            // direct right
            searchPoints.push_back(Point(cos(deg2rad(-90.0f)) * kSearchRadius,
            sin(deg2rad(-90.0f)) * kSearchRadius,
            kZval, egocentric));
          */
                
          // create a polygon, and have the robot look at the vertices of the polygon
          NEW_SHAPE(gazePoints, PolygonData, new PolygonData(localShS, searchPoints, false));
          mapreq.searchArea = gazePoints;
          mapreq.removePts = false;
      }
    };

    class PointHeadFwd : public HeadPointerNode {
     public:
      PointHeadFwd(const std::string &nodename="PointHeadFwd") : HeadPointerNode(nodename) {}
      virtual void doStart() {
          // look ahead at point {x, y, z}
          getMC()->lookAtPoint(1400.0f, 0, 0);
      }
    };

    class OrganizeWorld : public StateNode {
     public:
      OrganizeWorld(const std::string &nodename="OrganizeWorld") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
          std::cout << "[" << getName() << "]: organizing the world.\n";
          // get all the shapes that are probably false positives, or not considered as a
          // world shape, and remove them from the worldShS
          std::cout << "deleting some shapes.\n";
          NEW_SHAPEROOTVEC(shapesToDelete, subset(worldShS, Kodu::IsNotWorldShape()));
          worldShS.deleteShapes(shapesToDelete);
          // search for the north star
          std::cout << "searching for north star... ";
          NEW_SHAPEROOTVEC(constellation, subset(worldShS, Kodu::IsStar()));
          // check if the robot observed the constellation
          if (!constellation.empty()) {
            std::cout << "found the North Star!\n";
            // add the constellation (points) to the world state
            theWorld->setStarConstellation(constellation);
            // generate the world bounds
            theWorld->generateWorldBoundsPolygon();
            // remove the constellation from the world shape space (prevents the pilot from
            // periodically stopping the robot---I want the interpreter to controll that)
            worldShS.deleteShapes(constellation);
          } else {
            std::cout << "could not find North Star (creating an artificial point)\n";
            // nothing in the constellation was seen (generate the world bounds)
            theWorld->generateWorldBoundsPolygon();
          }
          // set the world bounds in the pilot
          std::cout << "setting world bounds\n";
          VRmixin::pilot->setWorldBounds(theWorld->getWorldBoundsPolygon());
          // clear the landmark vector in the pilot
          std::cout << "clearing default landmarks\n";
          VRmixin::pilot->setDefaultLandmarks(std::vector<ShapeRoot>());
          postStateCompletion();
      }
    };

    virtual void setup() {
     CreateWorld *newWorld = new CreateWorld("newWorld");
     addNode(newWorld);

     ParseKode *parse = new ParseKode("parse");
     addNode(parse);

     LookAround *look = new LookAround("look");
     addNode(look);

     ParkArm *initArm = new ParkArm("initArm");
     addNode(initArm);

     PointHeadFwd *lookFwd = new PointHeadFwd("lookFwd");
     addNode(lookFwd);

     OrganizeWorld *orgWorld = new OrganizeWorld("orgWorld");
     addNode(orgWorld);

     SpeechNode *speechnode1 = new SpeechNode("speechnode1","error creating world");
     addNode(speechnode1);

     PostMachineFailure *postmachinefailure1 = new PostMachineFailure("postmachinefailure1");
     addNode(postmachinefailure1);

     PostMachineFailure *postmachinefailure2 = new PostMachineFailure("postmachinefailure2");
     addNode(postmachinefailure2);

     PostMachineCompletion *postmachinecompletion1 = new PostMachineCompletion("postmachinecompletion1");
     addNode(postmachinecompletion1);

     StateNode *statenode1 = new StateNode("statenode1");
     addNode(statenode1);

     startnode = newWorld;

     newWorld->addTransition(new CompletionTrans("completiontrans1",parse));
     newWorld->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans1",speechnode1,StateNode::failureSignal));
     speechnode1->addTransition(new CompletionTrans("completiontrans2",postmachinefailure1));

     SignalTrans<StateNode::SuccessOrFailure> *signaltrans2 = new SignalTrans<StateNode::SuccessOrFailure>("signaltrans2",look,StateNode::successSignal);
     signaltrans2->addDestination(initArm);
     parse->addTransition(signaltrans2);

     parse->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans3",postmachinefailure2,StateNode::failureSignal));
     look->addTransition(new CompletionTrans("completiontrans3",lookFwd));
     lookFwd->addTransition(new CompletionTrans("completiontrans4",orgWorld));
     orgWorld->addTransition(new CompletionTrans("completiontrans5",postmachinecompletion1));
     initArm->addTransition(new CompletionTrans("completiontrans6",statenode1));
    }
  };

  class WalkMonitor : public StateNode {
   public:
    WalkMonitor(const std::string &nodename="WalkMonitor") : StateNode(nodename), lastRecordedState() {}

    virtual void doStart() {
      if (VRmixin::isWalkingFlag == true) {
        // get the robot's current state
        Kodu::PosOrientState currState(VRmixin::theAgent->getCentroid(),
                                       VRmixin::theAgent->getOrientation());
        // get the difference between points and orientation
        Kodu::PosOrientState stateDiff = currState - lastRecordedState;
        // motion along the x-axis
        float xyNorm = stateDiff.position.xyNorm();
        // motion through turning
        float arcLen = std::fabs(stateDiff.orientation) * 130.0f;
        // increment the total distance travelled
    Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
        thisAgent->distanceTravelled = thisAgent->distanceTravelled + xyNorm + arcLen;
        lastRecordedState = currState;
        std::cout << "[" << getName() << "]: accumulated dist. = " 
                  << thisAgent->distanceTravelled << "mm\n";
      }
    }
    Kodu::PosOrientState lastRecordedState;   // from $provide
  };

  class PerceptualMultiplexor : public StateNode {
   public:
    PerceptualMultiplexor(const std::string &nodename="PerceptualMultiplexor") : StateNode(nodename), currentTask(NULL), inRecovery(false) {}

    ~PerceptualMultiplexor() {
      currentTask = NULL;
    }

    enum MultiPlexorTransType_t {
      MPT_MAP_BUILDER = 0,
      MPT_PILOT
    };

    // Returns whether or not the perceptual multiplexor is in recovery mode
    bool isInRecovery() const { return inRecovery; }

    class MultiplexorStart : public StateNode {
     public:
      MultiplexorStart(const std::string &nodename="MultiplexorStart") : StateNode(nodename) {}
  
        Kodu::PerceptualTaskBase* getNextExecutableTask() {
      Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                  
          unsigned int tasksToExecute = thisAgent->ptasks.size();
          // std::cout << "checking task #'s ";
          while (tasksToExecute > 0) {
            // std::cout << thisAgent->ptasks.front()->getTaskId() << "; ";
            if (thisAgent->ptasks.front()->canExecute(*theWorld)) {
              return thisAgent->ptasks.front();
            }
            tasksToExecute--;
            thisAgent->ptasks.push(thisAgent->ptasks.front());
            thisAgent->ptasks.pop();
          }
          std::cout << std::endl;
          return NULL;
        }
  
        virtual void doStart() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
      Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
                  
          // std::cout << "=== [" << getName() << "] ===\n";
          if (currentTask == NULL) {
            if (thisAgent->ptasks.empty()) {
              std::cout << "No tasks to execute; Multiplexor exiting.\n";
              postParentCompletion();
              return;
            }
                  
            std::cout << thisAgent->ptasks.size() << " perceptual tasks to (possibly) execute!\n";
            currentTask = getNextExecutableTask();
            if ( currentTask == NULL ) {
              //std::cout << "no executable tasks left; exiting.\n";
              postParentCompletion();
              return;
            }
            // pop the current task off of the queue
            thisAgent->ptasks.pop();
          }
  
          std::cout << "Executing task #" << currentTask->getTaskId() << " ";
          // check the task type to decide which task runner to use
          switch (currentTask->getType()) {
          case Kodu::PT_VIS_BUMP_DETECTION:
            std::cout << "(Bump Detection).\n";
            postStateSignal<MultiPlexorTransType_t>(MPT_MAP_BUILDER);
            return;
  
          case Kodu::PT_GRIPPER_VIS_MONITOR:
            std::cout << "(Gripper Monitor).\n";
            postStateSignal<MultiPlexorTransType_t>(MPT_MAP_BUILDER);
            return;
  
          case Kodu::PT_VIS_LOCALIZATION:
            std::cout << "(Localization).\n";
            postStateSignal<MultiPlexorTransType_t>(MPT_PILOT);
            return;
  
          case Kodu::PT_VIS_NAV_ERR_MON:
            std::cout << "(VisualNavErrorMon).\n";
            postStateSignal<MultiPlexorTransType_t>(MPT_MAP_BUILDER);
            return;
  
          default:
            std::cout << "MultiplexorStart: unidentified task found... exiting.\n";
            postParentCompletion();
            return;
          }
        }
    };

    /**
     * The state machine that:
     * 1) handles MapBuilder requests, and
     * 2) examines the results from the request
     **/
    class MapBuilderTaskRunner : public StateNode {
     public:
      MapBuilderTaskRunner(const std::string &nodename="MapBuilderTaskRunner") : StateNode(nodename) {}
  
      class ExecuteMapBuilderTask : public MapBuilderNode {
       public:
        ExecuteMapBuilderTask(const std::string &nodename="ExecuteMapBuilderTask") : MapBuilderNode(nodename) {}
        virtual void doStart() {
              std::cout << "[" << getName() << "]: looking for objects.\n";
          Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
              // (generate and) get the mapbuilder request
              mapreq = currentTask->getMapBuilderRequest();
        }
      };
  
      class ExamineMapBuilderResults : public StateNode {
       public:
        ExamineMapBuilderResults(const std::string &nodename="ExamineMapBuilderResults") : StateNode(nodename) {}
        virtual void doStart() {
          Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
              // examine the results from the mapbuilder request
              //cout << " Examine results: currentTask=" << (void*)currentTask << " id=" <<  currentTask->getTaskId() << endl;
              currentTask->examineTaskResults();
              postParentCompletion();
        }
      };
                                          
      virtual void setup() {
       ExecuteMapBuilderTask *executeRequest = new ExecuteMapBuilderTask("executeRequest");
       addNode(executeRequest);

       ExamineMapBuilderResults *examineResults = new ExamineMapBuilderResults("examineResults");
       addNode(examineResults);

       startnode = executeRequest;

       executeRequest->addTransition(new CompletionTrans("completiontrans7",examineResults));
      }
    };

    // The PilotNode class used for executing Pilot request (such as localization)
    class PilotTaskRunner : public StateNode {
     public:
      PilotTaskRunner(const std::string &nodename="PilotTaskRunner") : StateNode(nodename) {}
  
      class ExecutePilotTask : public PilotNode {
       public:
        ExecutePilotTask(const std::string &nodename="ExecutePilotTask") : PilotNode(nodename) {}
        virtual void doStart() {
          Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
              std::cout << "[" << getName() << "]: submitting pilot request for localization\n";
              pilotreq = currentTask->getPilotRequest();
              if ( pilotreq.requestType > 5 )
                cout << "!!!!!!!!!  Bad pilot request" << endl;
        }
      };
  
      class SetTaskSuccess : public StateNode {
       public:
        SetTaskSuccess(const std::string &nodename="SetTaskSuccess") : StateNode(nodename) {}
        virtual void doStart() {
          Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
              std::cout << "[" << getName() << "]: setting pilot task status ==> SUCCESSFUL\n";
              currentTask->setTaskStatus(Kodu::PerceptualTaskBase::TS_SUCCESSFUL);
              NEW_SHAPEROOTVEC(stars, subset(worldShS, Kodu::IsStar()));
              worldShS.deleteShapes(stars);
              postParentCompletion();
        }
      };
  
      class SetTaskFailure : public StateNode {
       public:
        SetTaskFailure(const std::string &nodename="SetTaskFailure") : StateNode(nodename) {}
        virtual void doStart() {
          Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
              std::cout << "[" << getName() << "]: setting pilot task status ==> FAILURE\n";
              currentTask->setTaskStatus(Kodu::PerceptualTaskBase::TS_FAILURE);
              NEW_SHAPEROOTVEC(stars, subset(worldShS, Kodu::IsStar()));
              worldShS.deleteShapes(stars);
              postParentCompletion();
        }
      };
  
      virtual void setup() {
       ExecutePilotTask *executeRequest = new ExecutePilotTask("executeRequest");
       addNode(executeRequest);

       SetTaskSuccess *setTaskSuccess = new SetTaskSuccess("setTaskSuccess");
       addNode(setTaskSuccess);

       SetTaskFailure *setTaskFailure = new SetTaskFailure("setTaskFailure");
       addNode(setTaskFailure);

       startnode = executeRequest;

       executeRequest->addTransition(new PilotTrans("pilottrans1",setTaskSuccess,PilotTypes::noError));
       executeRequest->addTransition(new PilotTrans("pilottrans2",setTaskFailure,PilotTypes::cantLocalize));
      }
    };
        
    class MultiplexorEnd : public StateNode {
     public:
      MultiplexorEnd(const std::string &nodename="MultiplexorEnd") : StateNode(nodename) {}
      virtual void doStart() {
        KoduConditionEvaluator* &evaluatorRef = getAncestor<KoduInterpreter>()->evaluatorRef;
        bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
        Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
        Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
                
          std::cout << "Task #" << currentTask->getTaskId() << " status: ";
          // check if the task is complete
          if (currentTask->taskIsComplete(*theWorld)) {
            std::cout << "complete! (removing task).\n";
            // check if the task failed
            if (currentTask->getStatus() == Kodu::PerceptualTaskBase::TS_FAILURE) {
        bool &inRecovery = getAncestor<PerceptualMultiplexor>()->inRecovery;
              inRecovery = true;
              postStateFailure();
              return;
            }
            // delete the task
            GeneralFncs::deletePtr(currentTask);
          }
          // else, the task is not complete so add it to the end of the queue
          else {
            std::cout << "NOT complete. (adding to end of queue).\n";
            thisAgent->ptasks.push(currentTask);
          }
          currentTask = NULL;
          // check if there are more tasks on the queue to execute
          if (thisAgent->ptasks.empty()) {
            postParentCompletion();
          }
          else {
            // std::cout << "continuing to next task.\n";
            postStateCompletion();
          }
          // this means the evaluator was halted in order to recover, so restart the evaluator
          if (needToHaltExecution == true) {
            needToHaltExecution = false;
            evaluatorRef->start();
          }
      }
    };

    class FailureRecovery : public StateNode {
     public:
      FailureRecovery(const std::string &nodename="FailureRecovery") : StateNode(nodename) {}
              
      class PauseInterpretation : public StateNode {
       public:
        PauseInterpretation(const std::string &nodename="PauseInterpretation") : StateNode(nodename) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          KoduConditionEvaluator* &evaluatorRef = getAncestor<KoduInterpreter>()->evaluatorRef;
          KoduActionRunner* &runnerRef = getAncestor<KoduInterpreter>()->runnerRef;
          MotionActionRunner* &motionActRef = getAncestor<KoduInterpreter>()->motionActRef;
          DropActionRunner* &dropActRef = getAncestor<KoduInterpreter>()->dropActRef;
          GrabActionRunner* &grabActRef = getAncestor<KoduInterpreter>()->grabActRef;
          bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
                      
              std::cout << "[" << getName() << "]: pausing the evaluator.\n";
              static unsigned int waitCount = 0;
              
              // if the halt flag is not set (meaning it is false), set it to true
              if (needToHaltExecution == false) {
                std::cout << "Requesting the evaluator to halt.\n";
                needToHaltExecution = true;
              }
                      
              // stop the drop action runner
              if (dropActRef->isActive()) {
                std::cout << "Stopping the Grasper (drop action).\n";
                dropActRef->stop();
              }
      
              // stop the grab action runner
              if (grabActRef->isActive()) {
                std::cout << "Stopping the Grasper (grab action).\n";
                grabActRef->stop();
              }
      
              // abort/stop any node that may use motion (e.g. Pilot, Grasper)
              if (thisAgent->isExecutingMotionAction()) {
                std::cout << "Aborting current pilot request.\n";
                VRmixin::pilot->pilotAbort();
                //thisAgent->agentIsWalking = false;
                thisAgent->motionComplete();
                motionActRef->stop();
              }
      
              // make sure all are inactive before recovery proceeds
              if (!evaluatorRef->isActive() && !thisAgent->isExecutingMotionAction()
                  && !runnerRef->isActive() && !grabActRef->isActive())
                {
                  if ((++waitCount) == 2) {
                    waitCount = 0;
                    postStateCompletion();
                  }
                }
              // else, wait for the timeout transition
              else {
                PRINT_ATTRS("Evaluator is active?", evaluatorRef->isActive());
                PRINT_ATTRS("Runner is active?", runnerRef->isActive());
                PRINT_ATTRS("Agent is walking?", thisAgent->isExecutingMotionAction());
                PRINT_ATTRS("Grab Action is active?", grabActRef->isActive());
                waitCount = 0;
              }
        }
      };
  
      class StartRecovery : public StateNode {
       public:
        StartRecovery(const std::string &nodename="StartRecovery") : StateNode(nodename) {}
        virtual void doStart() {
          Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
              std::cout << "[" << getName() << "]\n";
              std::cout << "Perceptual Recovery starting...\n";
              // the signal type decides which recovery is started
              postStateSignal<Kodu::PerceptualTaskType_t>(currentTask->getType());
        }
      };
  
      class ObjectManipRecovery : public StateNode {
       public:
        ObjectManipRecovery(const std::string &nodename="ObjectManipRecovery") : StateNode(nodename), gTask(NULL), grasperTarget(), objectTagId(-1) {}
            
            ~ObjectManipRecovery() {
              gTask = NULL;
            }
    
        class InitiateManipRecovery : public StateNode {
         public:
          InitiateManipRecovery(const std::string &nodename="InitiateManipRecovery") : StateNode(nodename) {}
      
          class GetObjectInfo : public StateNode {
           public:
            GetObjectInfo(const std::string &nodename="GetObjectInfo") : StateNode(nodename) {}
            virtual void doStart() {
              Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
              Kodu::VisualGripperMonitorTask* &gTask = getAncestor<ObjectManipRecovery>()->gTask;
              int &objectTagId = getAncestor<ObjectManipRecovery>()->objectTagId;
                      
                      std::cout << "[" << getName() << "]: getting the gripper monitor task\n";
                      // cast the current task, and point to it
                      gTask = static_cast<Kodu::VisualGripperMonitorTask*>(currentTask);
                      objectTagId = gTask->getTagId();
                      postStateCompletion();
            }
          };
      
          class OpenGripper : public ArmNode {
           public:
            OpenGripper(const std::string &nodename="OpenGripper") : ArmNode(nodename) {}
            virtual void doStart() {
                      std::cout << "[" << getName() << "]: opening the gripper\n";
                      // opens the gripper in case it detected a false positive (the object is
                      // still in the gripper, but the robot detected a failure)
                      getMC()->openGripper(0.8f);
            }
          };
      
          virtual void setup() {
           GetObjectInfo *getInfo = new GetObjectInfo("getInfo");
           addNode(getInfo);

           OpenGripper *openGrip = new OpenGripper("openGrip");
           addNode(openGrip);

           PostMachineCompletion *postmachinecompletion2 = new PostMachineCompletion("postmachinecompletion2");
           addNode(postmachinecompletion2);

           startnode = getInfo;

           getInfo->addTransition(new CompletionTrans("completiontrans8",openGrip));
           openGrip->addTransition(new CompletionTrans("completiontrans9",postmachinecompletion2));
          }
        };
    
        class PrepareForItemRecovery : public StateNode {
         public:
          PrepareForItemRecovery(const std::string &nodename="PrepareForItemRecovery") : StateNode(nodename), reverseDist(0.0f) {}
      
          class Reverse : public PilotNode {
           public:
            Reverse(const std::string &nodename="Reverse") : PilotNode(nodename,PilotTypes::walk) {}
            virtual void doStart() {
                      std::cout << "[" << getName() << "]: ";
                      //$reference KoduInterpreter::thisAgent;
              Kodu::VisualGripperMonitorTask* &gTask = getAncestor<ObjectManipRecovery>()->gTask;
              float &reverseDist = getAncestor<PrepareForItemRecovery>()->reverseDist;
                      // get the current and last positions
                      Point lastPos = gTask->getLastSuccessfulState().position;
                      Point currPos = VRmixin::theAgent->getCentroid();
                      // get the gripper object's radius
                      //float gripperObjectRadius = Kodu::objectRadius(thisAgent->gripperObject);
                      // get the distance the robot travelled between position states
                      float positionMag = (currPos - lastPos).xyNorm();
                      // calculate how far the robot should reverse
                      //reverseDist = (gripperObjectRadius * 1.25f) + positionMag;
                      reverseDist = 75.0f + positionMag;
                      // set the distance the robot should reverse
                      pilotreq.dx = -1.0f * reverseDist;
                      std::cout << "reversing by " << reverseDist << "mm ("
                                << 75.0f << " + " << positionMag << ")\n";
                      //<< gripperObjectRadius << " + " << positionMag << ")\n";
            }
          };
      
          class LocateLostObject : public MapBuilderNode {
           public:
            LocateLostObject(const std::string &nodename="LocateLostObject") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
            virtual void doStart() {
              Kodu::VisualGripperMonitorTask* &gTask = getAncestor<ObjectManipRecovery>()->gTask;
                      std::cout << "[" << getName() << "]: "
                                << "creating mapbuilder request to find object\n";
                      // get the robot's state when it last saw the april tag in the correct location
                      Kodu::PosOrientState lastState = gTask->getLastSuccessfulState();
                      float lastOrient = lastState.orientation;
                                  
                      // get the robot's current (position, orientation) state
                      Kodu::PosOrientState currState(VRmixin::theAgent->getCentroid(),
                                                     static_cast<float>(VRmixin::theAgent->getOrientation()));
                      float currOrient = currState.orientation;
                                  
                      // get the difference between the last and current orientations
                      float orientDiff = AngTwoPi(lastOrient - currOrient);
                      std::cout << "currOrient = " << currOrient << " rads (= " 
                                << rad2deg(currOrient) << " degs); lastOrient = "
                                << lastOrient << " rads (= " << rad2deg(lastOrient)
                                << " degs); orientDiff = " << orientDiff << " rads (= "
                                << rad2deg(orientDiff) << " degs)\n";
          
                      const Shape<CylinderData>& kLostCyl =
                        ShapeRootTypeConst(gTask->getGripperObject(), CylinderData);
                                  
                      // create constants that will be used to tell the robot where to search
                      // the over approx. max reach the gripper has (measured from the robot's center)
                      float const kMaxGripperReach = 400.0f;
                      // the cylinder's height
                      float const kCylHeight = kLostCyl->getHeight();
                      // the cylinder's radius
                      float const kCylRadius = kLostCyl->getRadius();
                      // the distance the robot reversed
              float &reverseDist = getAncestor<PrepareForItemRecovery>()->reverseDist;
                      // the max distance the object should be away from the robot's centroid
                      float dropRadius = kMaxGripperReach + kCylRadius + reverseDist;
                      // the max distance an object can be for the robot to consider
                      // it as the lost object
                      float maxSearchRadius = dropRadius + (kCylRadius * 2.0f);
                                  
                      // find out what the robot was doing when the object fell out of the gripper
                      float const kInitAngle = gTask->getTagCentroid().atanYX();
                      float thetaIncrement = 0.0f;
                      float const kIncrementVal = deg2rad(15.0f);
                      if (std::fabs(lastOrient - currOrient) > M_PI) {
                        std::cout << "Robot crossed over the 360 deg mark (abs(orientDiff) = "
                                  << std::fabs(lastOrient - currOrient) << ")\n";
                        if (currOrient < lastOrient) {
                          std::cout << "currOrient (" << currOrient << ") < lastOrient ("
                                    << lastOrient << ")\n";
                          currOrient = currOrient + (2 * M_PI);
                        } else {
                          std::cout << "currOrient (" << currOrient << ") > lastOrient ("
                                    << lastOrient << ")\n";
                          lastOrient = lastOrient + (2 * M_PI);
                        }
                        std::cout << "new values: currOrient = " << currOrient
                                  << "; lastOrient = " << lastOrient << std::endl;
                      }
                                  
                      float absOrientDiff = std::fabs(lastOrient - currOrient);
                      if (currOrient > lastOrient) {
                        thetaIncrement = -1.0f * kIncrementVal;
                        std::cout << "Robot was turning left; ";
                      } else if (currOrient < lastOrient) {
                        thetaIncrement = kIncrementVal;
                        std::cout << "Robot was turning right; ";
                      } else {
                        thetaIncrement = 0.0f;
                        std::cout << "The object should be in front of the gripper; ";
                      }
                      std::cout << "thetaIncrement = " << thetaIncrement << " rads (= "
                                << rad2deg(thetaIncrement) << " degs)\n";
          
                      // start from the current orientation and progressively look to the last state
                      std::vector<Point> pts;
                      int const kNumbOfPoints = (int)(std::ceil(absOrientDiff / kIncrementVal));
                      pts.reserve(kNumbOfPoints + 1);
                      std::cout << "the robot will have to look " << (kNumbOfPoints + 1)
                                << " time(s).\n";
                      float currAngle = kInitAngle;
                      std::cout << "currAngle = " << currAngle << " rads (= " << (rad2deg(currAngle))
                                << " degs) ";
                                  
                      // approx. position of the object if it was still in the gripper
                      pts.push_back(Point(cos(currAngle) * dropRadius, sin(currAngle) * dropRadius,
                                          kCylHeight, egocentric));
                      std::cout << "begin pt = " << pts[0] << std::endl;
          
                      // generate the rest of the look points
                      for (int i = 0; i < kNumbOfPoints; i++) {
                        currAngle += thetaIncrement;
                        std::cout << "currAngle = " << currAngle << " rads (= "
                                  << (rad2deg(currAngle)) << " degs) ";
                        pts.push_back(Point(cos(currAngle) * dropRadius,
                                            sin(currAngle) * dropRadius,
                                            kCylHeight, egocentric));
                        std::cout << "added pt #" << pts.size() << " = " << pts[pts.size() - 1]
                                  << std::endl;
                      }
          
                      // create the gaze polygon (the points the robot should look at)
                      NEW_SHAPE(objSearchPoints, PolygonData, new PolygonData(localShS, pts, false));
                      objSearchPoints->setViewable(true);
                      objSearchPoints->setObstacle(false);
                                  
                      // create the mapbuilder request
                      mapreq.searchArea = objSearchPoints;
                      mapreq.clearLocal = true;
                      mapreq.maxDist = maxSearchRadius;
                      //mapreq.pursueShapes = true;
                      mapreq.removePts = false;
                      mapreq.setAprilTagFamily();
                      mapreq.addObjectColor(kLostCyl->getType(), kLostCyl->getColor());
            }
          };
      
          class SetGrasperTarget : public StateNode {
           public:
            SetGrasperTarget(const std::string &nodename="SetGrasperTarget") : StateNode(nodename) {}
            virtual void doStart() {
              Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
              int &objectTagId = getAncestor<ObjectManipRecovery>()->objectTagId;
          
                      std::cout << "[" << getName() << "]: " << "setting grasper target, id=" << objectTagId << "!\n";
          
                      ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
                      NEW_SHAPEROOTVEC(cyls, subset(localShS, Kodu::IsShapeOfType(cylinderDataType)));
                                  
                      if (tag.isValid()) {
                        if (!cyls.empty()) {
                          std::cout << "tag is valid!\n";
                          grasperTarget = Kodu::getClosestObjectToPoint(cyls, tag->getCentroid());
                        }
                        // tag was seen, but not the cylinder
                        else {
                          const Shape<CylinderData>& kGripCyl
                            = ShapeRootTypeConst(thisAgent->gripperObject, CylinderData);
                          const Point& kTagPt = tag->getCentroid();
                          float cylHeight = kGripCyl->getHeight();
                          float cylRadius = kGripCyl->getRadius();
                          Point lclCylPt = Point(kTagPt.coordX(), kTagPt.coordY(),
                                                 (kTagPt.coordZ() / 2.0f), egocentric);
                                          
                          NEW_SHAPE(duplicate, CylinderData,
                                    new CylinderData(localShS, lclCylPt, cylHeight, cylRadius));
                          grasperTarget = duplicate;
                        }
                      }
          
                      // the tag was not visible
                      // if (!cyls.empty()) {
                      //     std::cout << "tag is NOT valid. getting the closest match.\n";
                      //     grasperTarget = find_if(localShS, Kodu::IsShapeOfType(cylinderDataType));
                      // }
          
                      if (grasperTarget.isValid()) {
                        std::cout << "grasper target was valid!\n";
                        grasperTarget = VRmixin::mapBuilder->importLocalShapeToWorld(grasperTarget);
                        postStateSuccess();
                      } else {
                        std::cout << "grasper target was not valid!!!\n";
                        postStateFailure();
                      }
            }
          };
      
          class FaceTarget : public PilotNode {
           public:
            FaceTarget(const std::string &nodename="FaceTarget") : PilotNode(nodename,PilotTypes::walk) {}
            virtual void doStart() {
              ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
                      std::cout << "[" << getName() << "]: turning by ";
                      float bearingToTarget = Kodu::bearingFromAgentToObject(grasperTarget);
                      std::cout << bearingToTarget << " rads (= " << rad2deg(bearingToTarget)
                                << " degs)\n";
                      pilotreq.da = bearingToTarget;
            }
          };
      
          virtual void setup() {
           Reverse *rvrs = new Reverse("rvrs");
           addNode(rvrs);

           LocateLostObject *find = new LocateLostObject("find");
           addNode(find);

           SetGrasperTarget *setTarget = new SetGrasperTarget("setTarget");
           addNode(setTarget);

           FaceTarget *faceTarget = new FaceTarget("faceTarget");
           addNode(faceTarget);

           PostMachineSuccess *postmachinesuccess1 = new PostMachineSuccess("postmachinesuccess1");
           addNode(postmachinesuccess1);

           PostMachineFailure *postmachinefailure3 = new PostMachineFailure("postmachinefailure3");
           addNode(postmachinefailure3);

           startnode = rvrs;

           rvrs->addTransition(new CompletionTrans("completiontrans10",find));
           find->addTransition(new CompletionTrans("completiontrans11",setTarget));
           setTarget->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans4",faceTarget,StateNode::successSignal));
           faceTarget->addTransition(new CompletionTrans("completiontrans12",postmachinesuccess1));
           setTarget->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans5",postmachinefailure3,StateNode::failureSignal));
          }
          float reverseDist;   // from $provide
        };
    
            // ASSUMPTION:
            // --- there is only one tag in view (need to use ID to select correct object)
        class RetrieveLostObject : public GrasperNode {
         public:
          RetrieveLostObject(const std::string &nodename="RetrieveLostObject") : GrasperNode(nodename,GrasperRequest::grasp) {}
          virtual void doStart() {
            ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
                  std::cout << "[" << getName() << "]: going to retrieve the lost object...\n";
                  graspreq.object = grasperTarget;
          }
        };
    
        class VerifyObjectWasGrabbed : public StateNode {
         public:
          VerifyObjectWasGrabbed(const std::string &nodename="VerifyObjectWasGrabbed") : StateNode(nodename) {}
      
          class LookAtTheGripper : public MapBuilderNode {
           public:
            LookAtTheGripper(const std::string &nodename="LookAtTheGripper") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
            virtual void doStart() {
                      std::cout << "[" << getName() << "]: looking at tag\n";
                      mapreq.clearLocal = true;
                      // search for an april tag
                      mapreq.setAprilTagFamily();
                      // look at the gripper
                      fmat::Column<3> gripperLoc;
          #if defined(TGT_IS_CALLIOPE2)
                      gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
          #endif
                      NEW_SHAPE(gazePoint, PointData, new PointData(localShS,
                                                                    Point(gripperLoc[0], gripperLoc[1], gripperLoc[2], egocentric)));
                      mapreq.searchArea = gazePoint;
            }
          };
      
          class VerifyObjectInGripper : public StateNode {
           public:
            VerifyObjectInGripper(const std::string &nodename="VerifyObjectInGripper") : StateNode(nodename) {}
            virtual void doStart() {
              Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
              Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                      std::cout << "[" << getName() << "]: verifying object was grabbed.\n";
                      
                      // check if a particular tag was seen
                      NEW_SHAPE(tag, AprilTagData,
                                find_if<AprilTagData>(localShS,
                                                      Kodu::HasAprilTagID(theWorld->getTagIdForShape(thisAgent->gripperObject.getId()))));
                      
                      // if the reference is not valid, post a failure
                      if (!tag.isValid()) {
                        std::cout << "THE TAG WAS NOT SEEN (FAILURE)!!!\n";
                        postStateFailure();
                        return;
                      }
          
                      // ********** temp fix
                      // 
                      // get the gripper location
                      fmat::Column<3> gripperLoc;
          #if defined(TGT_IS_CALLIOPE2)
                      gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
          #endif
                      Point gripperPoint(gripperLoc[0], gripperLoc[1], gripperLoc[2], egocentric);
                      
                      // check if the object is in the gripper
                      if (tag->getCentroid().xyDistanceFrom(gripperPoint)
                          < (Kodu::objectRadius(thisAgent->gripperObject) * 1.25f)) {
                        std::cout << "successfully grabbed object\n";
                        postParentSuccess();
                      } else {
                        std::cout << "failed to grab object... dist = "
                                  << tag->getCentroid().xyDistanceFrom(gripperPoint)
                                  << "mm; reattempting.\n";
                        postParentFailure();
                      }
                      // ***********
            }
          };
      
          virtual void setup() {
           LookAtTheGripper *lookAtGripper = new LookAtTheGripper("lookAtGripper");
           addNode(lookAtGripper);

           VerifyObjectInGripper *verifyObjGrabbed = new VerifyObjectInGripper("verifyObjGrabbed");
           addNode(verifyObjGrabbed);

           startnode = lookAtGripper;

           lookAtGripper->addTransition(new CompletionTrans("completiontrans13",verifyObjGrabbed));
          }
        };
    
        class PrepareForAnotherGrasp : public StateNode {
         public:
          PrepareForAnotherGrasp(const std::string &nodename="PrepareForAnotherGrasp") : StateNode(nodename), objFutureEgoPos() {}
      
          class NeedToLookAround : public StateNode {
           public:
            NeedToLookAround(const std::string &nodename="NeedToLookAround") : StateNode(nodename) {}
            virtual void doStart() {
              int &objectTagId = getAncestor<ObjectManipRecovery>()->objectTagId;
                                  
                      std::cout << "[" << getName() << "]: checking if robot needs to look around\n";
                      // get a particular april tag
                      ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
                      postStateSignal<bool>(!tag.isValid());
            }
          };
      
          class LookForTag : public MapBuilderNode {
           public:
            LookForTag(const std::string &nodename="LookForTag") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
            virtual void doStart() {
                      std::cout << "[" << getName() << "]: attempting to locate object (again)\n";
                      // generate points to look at (near the robot's body)
                      // these points could also be generated by looking at the gripper's y-pos
                      std::vector<Point> pts;
                      pts.push_back(Point(  300,    0,    0, egocentric));
                      pts.push_back(Point(  500,    0,    0, egocentric));
                      pts.push_back(Point(  500, -200,    0, egocentric));
                      pts.push_back(Point(  300, -200,    0, egocentric));
                      NEW_SHAPE(gazePoints, PolygonData, new PolygonData(localShS, pts, false));
                                  
                      // construct the mapbuilder request
                      mapreq.searchArea = gazePoints;
                      mapreq.setAprilTagFamily();
                      mapreq.clearLocal = true;
            }
          };
      
          class RepositionBody : public PilotNode {
           public:
            RepositionBody(const std::string &nodename="RepositionBody") : PilotNode(nodename,PilotTypes::walk) {}
            virtual void doStart() {
              Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              int &objectTagId = getAncestor<ObjectManipRecovery>()->objectTagId;
              Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
                                  
                      std::cout << "[" << getName() << "]: walking distance = ";
                      // get a particular april tag
                      ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
          
                      // get the gripper location
                      fmat::Column<3> gripperLoc;
          #if defined(TGT_IS_CALLIOPE2)
                      gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
          #endif
                      // where I would really want the objetc to be is the radial reach of the robot
                      // plus 50 (or 60) mm
                      float objDesiredXPos = gripperLoc[0] + 150.0f;
                                  
                      const Point& kTagPt = tag->getCentroid();
                      float objRadius = Kodu::objectRadius(thisAgent->gripperObject);
                                  
                      pilotreq.dx = kTagPt.coordX() - objRadius - objDesiredXPos;
                      std::cout << pilotreq.dx << "mm\n";
                                  
                      // reposition the gripper target object
                      objFutureEgoPos = Point(objDesiredXPos, kTagPt.coordY(),
                                              (kTagPt.coordZ() / 2.0f), egocentric);
                      pilotreq.collisionAction = collisionIgnore;
            }
          };
      
          class FindObjectAgain : public MapBuilderNode {
           public:
            FindObjectAgain(const std::string &nodename="FindObjectAgain") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
            virtual void doStart() {
              Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
          
                      std::cout << "[" << getName() << "]: looking for gripper object (again)\n";
                      // search for the object again using the point calculated previously
                      NEW_SHAPE(gazePoint, PointData, new PointData(localShS, objFutureEgoPos));
                      mapreq.searchArea = gazePoint;
                      const ShapeRoot& targetShape = thisAgent->gripperObject;
                      mapreq.addAttributes(targetShape);
                      mapreq.maxDist = objFutureEgoPos.xyNorm()
                        + (Kodu::objectRadius(targetShape) * 2.5f);
                      mapreq.pursueShapes = true;
            }
          };
      
          class RepositionGripperObject : public StateNode {
           public:
            RepositionGripperObject(const std::string &nodename="RepositionGripperObject") : StateNode(nodename) {}
            virtual void doStart() {
              Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
              Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
                                  
                      std::cout << "[" << getName() << "]: attempting to reposition gripper object.\n";
                      NEW_SHAPEROOTVEC(objs, subset(localShS,
                                                    Kodu::IsShapeOfType(thisAgent->gripperObject->getType())));
                      ShapeRoot targetShape = Kodu::getClosestObjectToPoint(objs, objFutureEgoPos);
                      // check if the target shape is valid
                      if (targetShape.isValid()) {
                        std::cout << "target shape is valid. setting its allocentric position.\n";
                        if (grasperTarget.isValid()) {
                          Point objAllocentricPt = targetShape->getCentroid();
                          objAllocentricPt.applyTransform(VRmixin::mapBuilder->localToWorldMatrix,
                                                          allocentric);
                          std::cout << "Repositioning the object to from "
                                    << grasperTarget->getCentroid() << " to "
                                    << objAllocentricPt << std::endl;
                          grasperTarget->setPosition(objAllocentricPt);
                        } else {
                          std::cout << "importing shape " << targetShape << " to the worldShS.\n";
                          grasperTarget =VRmixin::mapBuilder->importLocalShapeToWorld(targetShape);
                          std::cout << "target @ " << grasperTarget->getCentroid() << std::endl;
                        }
                        postStateSuccess();
                      }
                      // should only fail if the robot did not see any cylinders
                      else {
                        std::cout << "target shape is NOT VALID!!!! Don't know what to do!\n";
                        grasperTarget = ShapeRoot();
                        postStateFailure();
                      }
            }
          };
      
          virtual void setup() {
           NeedToLookAround *needToLook = new NeedToLookAround("needToLook");
           addNode(needToLook);

           LookForTag *lookForTag = new LookForTag("lookForTag");
           addNode(lookForTag);

           RepositionBody *reposBody = new RepositionBody("reposBody");
           addNode(reposBody);

           FindObjectAgain *findObjAgain = new FindObjectAgain("findObjAgain");
           addNode(findObjAgain);

           RepositionGripperObject *reposGripObj = new RepositionGripperObject("reposGripObj");
           addNode(reposGripObj);

           PostMachineSuccess *postmachinesuccess2 = new PostMachineSuccess("postmachinesuccess2");
           addNode(postmachinesuccess2);

           SpeechNode *speechnode2 = new SpeechNode("speechnode2","error shape was not valid");
           addNode(speechnode2);

           PostMachineFailure *postmachinefailure4 = new PostMachineFailure("postmachinefailure4");
           addNode(postmachinefailure4);

           startnode = needToLook;

           needToLook->addTransition(new SignalTrans<bool>("signaltrans6",lookForTag,true));
           lookForTag->addTransition(new CompletionTrans("completiontrans14",reposBody));
           needToLook->addTransition(new SignalTrans<bool>("signaltrans7",reposBody,false));
           reposBody->addTransition(new CompletionTrans("completiontrans15",findObjAgain));
           findObjAgain->addTransition(new CompletionTrans("completiontrans16",reposGripObj));
           reposGripObj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans8",postmachinesuccess2,StateNode::successSignal));
           reposGripObj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans9",speechnode2,StateNode::failureSignal));
           speechnode2->addTransition(new CompletionTrans("completiontrans17",postmachinefailure4));
          }
          Point objFutureEgoPos;   // from $provide
        };
    
            // ASSUMPTION: only one tag is in view
        class GetTagLocation : public StateNode {
         public:
          GetTagLocation(const std::string &nodename="GetTagLocation") : StateNode(nodename) {}
          virtual void doStart() {
            Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
            Kodu::VisualGripperMonitorTask* &gTask = getAncestor<ObjectManipRecovery>()->gTask;
                  std::cout << "[" << getName() << "]: getting april tag's location...\n";
        
                  int tagId = theWorld->getTagIdForShape(theWorld->thisAgent.gripperObject.getId());
                  ShapeRoot objtag = find_if(localShS, Kodu::HasAprilTagID(tagId));
        
                  if (objtag.isValid()) {
                    std::cout << "repositioning tag's location to " << objtag->getCentroid()
                              << std::endl;
                    gTask->relocateTagCentroid(objtag->getCentroid());
                    std::cout << "changing task status to IN PROGRESS\n";
                    gTask->setTaskStatus(Kodu::PerceptualTaskBase::TS_IN_PROGRESS);
                    postStateSuccess();
                  } else {
                    std::cout << "object tag is NOT VALID!!!\n";
                    postStateFailure();
                  }
                  // book keeping
                  gTask = NULL;
                  // ********** possible temp fix
            ShapeRoot &grasperTarget = getAncestor<ObjectManipRecovery>()->grasperTarget;
                  std::cout << "grasper target valid? ";
                  if (grasperTarget.isValid()) {
                    std::cout << "yes\n";
                    std::cout << "grasper target's matches the gripper target?";
            Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                    if (grasperTarget.getId() == thisAgent->gripperObject.getId()) {
                      std::cout << "yes, not deleting... making it not an obstacle.\n";
                      grasperTarget->setObstacle(false);
                    } else {
                      std::cout << "no, deleting the object.\n";
                      worldShS.deleteShape(grasperTarget);
                    }
                  } else {
                    std::cout << "no, THIS SHOULD NOT HAPPEN!!!\n";
                  }
                  // ***********
          }
        };
    
        virtual void setup() {
         InitiateManipRecovery *initMRec = new InitiateManipRecovery("initMRec");
         addNode(initMRec);

         PrepareForItemRecovery *prep4ItemRcvry = new PrepareForItemRecovery("prep4ItemRcvry");
         addNode(prep4ItemRcvry);

         RetrieveLostObject *retrvObj = new RetrieveLostObject("retrvObj");
         addNode(retrvObj);

         VerifyObjectWasGrabbed *verifyRetrv = new VerifyObjectWasGrabbed("verifyRetrv");
         addNode(verifyRetrv);

         PrepareForAnotherGrasp *prep4Grasp = new PrepareForAnotherGrasp("prep4Grasp");
         addNode(prep4Grasp);

         GetTagLocation *getLoc = new GetTagLocation("getLoc");
         addNode(getLoc);

         SpeechNode *speechnode3 = new SpeechNode("speechnode3","could not find the object");
         addNode(speechnode3);

         PostMachineFailure *postmachinefailure5 = new PostMachineFailure("postmachinefailure5");
         addNode(postmachinefailure5);

         PostMachineSuccess *postmachinesuccess3 = new PostMachineSuccess("postmachinesuccess3");
         addNode(postmachinesuccess3);

         startnode = initMRec;

         initMRec->addTransition(new CompletionTrans("completiontrans18",prep4ItemRcvry));
         prep4ItemRcvry->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans10",retrvObj,StateNode::successSignal));
         prep4ItemRcvry->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans11",prep4Grasp,StateNode::failureSignal));
         prep4Grasp->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans12",retrvObj,StateNode::successSignal));
         prep4Grasp->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans13",speechnode3,StateNode::failureSignal));
         speechnode3->addTransition(new CompletionTrans("completiontrans19",postmachinefailure5));
         retrvObj->addTransition(new GrasperTrans("graspertrans1",verifyRetrv));
         verifyRetrv->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans14",getLoc,StateNode::successSignal));
         getLoc->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans15",postmachinesuccess3,StateNode::successSignal));
         verifyRetrv->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans16",prep4Grasp,StateNode::failureSignal));
         prep4Grasp->addTransition(new CompletionTrans("completiontrans20",retrvObj));
        }
    
          private:
            DISALLOW_COPY_ASSIGN(ObjectManipRecovery);
        Kodu::VisualGripperMonitorTask* gTask;   // from $provide
        ShapeRoot grasperTarget;   // from $provide
        int objectTagId;   // from $provide
      };
  
      class LocalizeAgent : public StateNode {
       public:
        LocalizeAgent(const std::string &nodename="LocalizeAgent") : StateNode(nodename) {}
    
        class CreateLocalizeTask : public StateNode {
         public:
          CreateLocalizeTask(const std::string &nodename="CreateLocalizeTask") : StateNode(nodename) {}
          virtual void doStart() {
            Kodu::PerceptualTaskBase* &currentTask = getAncestor<PerceptualMultiplexor>()->currentTask;
            Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
                std::cout << "[" << getName() << "]\n";
                std::cout << "Deleting task #" << currentTask->getTaskId() << std::endl;
                std::cout << "creating a localization task ";
                if (currentTask->getType() == Kodu::PT_VIS_LOCALIZATION) {
                  // remove old task
                  GeneralFncs::deletePtr(currentTask);
                  // make the localization task the new current task
                  // ********* temp fix
                  std::cout << " with max amount of stars ("
                            << theWorld->getStarConstellation().size() << ")";
                  currentTask = new Kodu::VisualLocalizationTask(theWorld->getStarConstellation(),
                                                                 theWorld->getStarConstellation().size());
                  // *********
                } else {
                  // remove old task
                  GeneralFncs::deletePtr(currentTask);
                  // make the localization task the new current task
                  currentTask = new Kodu::VisualLocalizationTask(theWorld->getStarConstellation());
                }
                std::cout << "\n";
                postParentCompletion();
          }
        };
    
        virtual void setup() {
         CreateLocalizeTask *createTask = new CreateLocalizeTask("createTask");
         addNode(createTask);

         startnode = createTask;

        }
      };
  
      class EndRecovery : public StateNode {
       public:
        EndRecovery(const std::string &nodename="EndRecovery") : StateNode(nodename) {}
        virtual void doStart() {
          bool &inRecovery = getAncestor<PerceptualMultiplexor>()->inRecovery;
            std::cout << "[" << getName() << "]: ending recovery...\n";
            inRecovery = false;
            postParentCompletion();
        }
      };
  
      virtual void setup() {
       PauseInterpretation *pause = new PauseInterpretation("pause");
       addNode(pause);

       StartRecovery *startRec = new StartRecovery("startRec");
       addNode(startRec);

       LocalizeAgent *locAgent = new LocalizeAgent("locAgent");
       addNode(locAgent);

       ObjectManipRecovery *manipRec = new ObjectManipRecovery("manipRec");
       addNode(manipRec);

       EndRecovery *endRec = new EndRecovery("endRec");
       addNode(endRec);

       SpeechNode *bumpRec = new SpeechNode("bumpRec","unhandled recovery for bump detection");
       addNode(bumpRec);

       SpeechNode *speechnode4 = new SpeechNode("speechnode4","unhandled failure recovery for manipulation");
       addNode(speechnode4);

       startnode = pause;

       pause->addTransition(new TimeOutTrans("timeouttrans1",pause,300));
       pause->addTransition(new CompletionTrans("completiontrans21",startRec));
       startRec->addTransition(new SignalTrans<Kodu::PerceptualTaskType_t>("signaltrans17",locAgent,Kodu::PT_VIS_LOCALIZATION));
       startRec->addTransition(new SignalTrans<Kodu::PerceptualTaskType_t>("signaltrans18",manipRec,Kodu::PT_GRIPPER_VIS_MONITOR));
       startRec->addTransition(new SignalTrans<Kodu::PerceptualTaskType_t>("signaltrans19",locAgent,Kodu::PT_VIS_NAV_ERR_MON));
       startRec->addTransition(new SignalTrans<Kodu::PerceptualTaskType_t>("signaltrans20",bumpRec,Kodu::PT_VIS_BUMP_DETECTION));
       manipRec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans21",endRec,StateNode::successSignal));
       manipRec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans22",speechnode4,StateNode::failureSignal));
       locAgent->addTransition(new CompletionTrans("completiontrans22",endRec));
      }
    };

    virtual void setup() {
     MultiplexorStart *startMP = new MultiplexorStart("startMP");
     addNode(startMP);

     MapBuilderTaskRunner *runMBTask = new MapBuilderTaskRunner("runMBTask");
     addNode(runMBTask);

     PilotTaskRunner *runPilotTask = new PilotTaskRunner("runPilotTask");
     addNode(runPilotTask);

     MultiplexorEnd *endMP = new MultiplexorEnd("endMP");
     addNode(endMP);

     FailureRecovery *recovery = new FailureRecovery("recovery");
     addNode(recovery);

     startnode = startMP;

     startMP->addTransition(new SignalTrans<MultiPlexorTransType_t>("signaltrans23",runMBTask,MPT_MAP_BUILDER));
     startMP->addTransition(new SignalTrans<MultiPlexorTransType_t>("signaltrans24",runPilotTask,MPT_PILOT));
     runMBTask->addTransition(new CompletionTrans("completiontrans23",endMP));
     runPilotTask->addTransition(new CompletionTrans("completiontrans24",endMP));
     endMP->addTransition(new CompletionTrans("completiontrans25",startMP));
     endMP->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans25",recovery,StateNode::failureSignal));
     recovery->addTransition(new CompletionTrans("completiontrans26",startMP));
    }

  private:
    DISALLOW_COPY_ASSIGN(PerceptualMultiplexor);
    Kodu::PerceptualTaskBase* currentTask;   // from $provide
    bool inRecovery;   // from $provide
  };
    
  class KoduConditionEvaluator : public StateNode {
   public:
    KoduConditionEvaluator(const std::string &nodename="KoduConditionEvaluator") : StateNode(nodename), cycleCount(0) {}

    //! Enables child rules with the once modifier enabled (who have already ran) to run (again)
    void resetChildRulesWithOnceEnabled(const unsigned int kParentRuleNumer) {
    Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;

      Kodu::KoduPage* page = thisAgent->getCurrentPage();
      Kodu::KoduRule* rule = NULL;
      const int kRuleCount = page->getRuleCount();
      for (int childRule_i = kParentRuleNumer + 1; childRule_i < kRuleCount; childRule_i++) {
        // get rule# i
        rule = page->getRule(childRule_i);
        // check if this rule's parent is kParentRuleNumber
        // if it is, set actionCanRun flag to true
        // else break--this rule has no more child actions
        if (rule->getParentNumber() == kParentRuleNumer)
          rule->action->setAgentCanUsePrimitive(true);
        else
          break;
      }
    }

    virtual void doStart() {
    PerceptualMultiplexor* &multiplexorRef = getAncestor<KoduInterpreter>()->multiplexorRef;
    bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
    Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
    Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
    Kodu::KoduState* &kodustate = getAncestor<KoduInterpreter>()->kodustate;

      //std::cout << "\n=== [Kodu Condition Evaluator] ===\n";
      // check if the evaluator needs to halt execution
      if (needToHaltExecution) {
        //std::cout << "halting evaluator execution\n";
        postStateFailure();
        return;
      }

      // pointers to the current page, rule, condition, and action (respectively)
      // check if the current page is NULL
      Kodu::KoduPage* page = thisAgent->getCurrentPage();
      if ( page == NULL ) {
        //std::cerr << "!!! A NULL page was found.\n";
        postStateFailure();
        return;
      }

      if ( (cycleCount % 20) == 0 )
        std::cout << "GAME CYCLE #" << cycleCount << ", PAGE #" << page->getPageNumber() << "\n";
      ++cycleCount;

      // check if there are rules on this page
      const int numbOfRules = page->getRuleCount();
      if (numbOfRules == 0) {
        //std::cout << "Reached an empty page. Stopping interpreter...\n";
        postStateFailure();
        return;
      }
           
      Kodu::MotionCommand newMotionCmd = Kodu::MotionCommand();

      // iterate over all the rules of a page
      for (int i = 0; i < numbOfRules; i++) {
        Kodu::KoduRule* rule = page->getRuleInPos(i);
        if (rule == NULL) {
          std::cerr << "!!! A NULL rule was found. Tried accessing Page["
                    << (page->getPageNumber() - 1) << "], Rule[" << i << "]\n";
          postStateFailure();
          return;
        }

        const unsigned int kParentId = rule->getParentNumber();
        const unsigned int kRuleId = rule->getRuleNumber();

        // check if this rule has parent rule and if its condition evaluated true
        if (rule->isIndented() && page->getRule(kParentId)->condLastEvalResult == false) {
          rule->condLastEvalResult = false;
          continue;
        }
                
        //cout << "kRuleId=" << kRuleId << " with parent=" << kParentId << endl;

        // point to the current condition and action
        Kodu::KoduCondition* cond = rule->condition;
        Kodu::KoduAction* act = rule->action;

        //Give condition and action access to state
        cond->setKoduState(*kodustate);
        act->setKoduState(*kodustate);
                
        // check if the agent can evaluate the condition
        if (!cond->agentCanUsePrimitive()) {
          // std::cout << "cannot evaluate condition... continuing.\n";
          continue;
        }
                
        // TODO (05/July/2013) Figure out how to cancel a running pilot request
        if (cond->evaluate(*theWorld) == true) {
          // resets the action has already ran flag
          // std::cout << "Condition Evaluated to true\n"; 
          if (rule->condLastEvalResult == false) {
            resetChildRulesWithOnceEnabled(kRuleId);
          }
                    
          // states condition evaluated true (no matter if action can execute or not)
          rule->setConditionEvalResult(true);
                    
          // check if the action can run
          if (!(act->agentCanUsePrimitive())) {
            std::cout << "Can't use this action\n";
            continue;
          }
          // check if this action:
          // 1) [implicit] can use the once modifier, and
          // 2) has the once modifier enabled
          if (act->onceModIsEnabled()) {
            act->setAgentCanUsePrimitive(false);
          }
                    
          // check the action type
          //std::cout << "checking actions: ";
          switch(act->getActionType()) {
            // Kodu Action Do Nothing
          case Kodu::KoduAction::AT_DO_NOTHING:
            {
              //      std::cout << "Do nothing.\n";
              break;
            }

            // Kodu Action Drop
          case Kodu::KoduAction::AT_DROP:
            {
              std::cout << "drop; ";
              // a drop action can only execute if the agent is:
              // 1) holding an object,
              // 2) not executing another manipulation action
              // 3) not executing a motion action
              if (thisAgent->isHoldingAnObject() && !thisAgent->isExecutingManipAction()
                  && !thisAgent->isExecutingMotionAction())
                {
                  std::cout << "Agent wants to drop item.\n";
                  // signal that a drop action is executing (prevents any motion or 
                  // other manipulation actions from executing)
                  thisAgent->signalDropActionStart();
                }
              break;
            }

            // Kodu Action Grab
          case Kodu::KoduAction::AT_GRAB:
            {
              std::cout << "grab; ";
              // a grab action can only execute if the agent is:
              // 1) not executing a motion action,
              // 2) not executing another manipulation action
              // 3) the target shape is valid
              // 4) the target shapes is not the same shape in the gripper
              if (!thisAgent->isExecutingMotionAction()
                  && !thisAgent->isExecutingManipAction())
                {
                  // get the target object
                  ShapeRoot targetObject =
                    static_cast<Kodu::KoduActionGrab*>(act)->getTargetObject();
                                
                  if (targetObject.isValid() && targetObject != thisAgent->gripperObject)
                    {
                      // signal that a grab action wants to execute (prevents any motion
                      // or other manipulation actions from executing)
                      thisAgent->signalGrabActionStart();
                      thisAgent->gripperObject = targetObject;
                      std::cout << "Selected an object " << targetObject
                                << " to be grabbed.\n";
                    }
                }
              break;
            }

            // Kodu Action Give
          case Kodu::KoduAction::AT_GIVE:
            {
              std::cout << "give; ";
              if (!thisAgent->isExecutingMotionAction() && !thisAgent->isExecutingManipAction()
                  && thisAgent->isHoldingAnObject()) {
                                
                Kodu::KoduActionGive* giveAction = static_cast<Kodu::KoduActionGive*>(act);
                ShapeRoot targetAgent = giveAction->getTargetObject();
                // Make sure the given target is an agentdata, and then set it as the target and signal a give
                if (targetAgent != Kodu::ObjectKeeper::invalidObject && targetAgent->isType(DualCoding::agentDataType)
                    && targetAgent.isValid()) {
                  thisAgent->giveTargetObject = ShapeRootType(targetAgent,AgentData);
                  thisAgent->signalGiveActionStart();
                }
              }
              break;
            }

            // Kodu Action Motion
          case Kodu::KoduAction::AT_MOTION:
            {
              if (! newMotionCmd.isValid() ) {
                newMotionCmd = static_cast<Kodu::KoduActionMotion*>(act)->getMotionCommand();
                // std::cout << "motion: " << newMotionCmd << std::endl;
                if ( thisAgent->currMotionCmd != newMotionCmd ) {
                  cout << "agent had " << thisAgent->currMotionCmd << " newMotionCmd=" << newMotionCmd << endl;
                  if ( thisAgent->currMotionCmd.isValid() ) {
    MotionActionRunner* &motionActRef = getAncestor<KoduInterpreter>()->motionActRef;
                    motionActRef->stop();
                  }

                  // TODO
                  // should the interpreter check if the robot is executing a 
                  // manip action here or in the action runner node?
                  
                  std::cout << "Adding motion command " << newMotionCmd << std::endl;
                  thisAgent->setMotionCommand(newMotionCmd);
                } else if ( newMotionCmd.isDriven() ) { //Update Velocities
                  std::cout << "Adjusting drive velocity" << std::endl; 
                  float xvel = kodustate->joysticky;
                  float avel = kodustate->joystickx;
                  float speed = 125.0;
                  float angvel = M_PI/8.0;
                  pilot->changeVelocity(speed*xvel, 0.0, angvel*avel);
                }
              } 
              break;
            }

            // Kodu Action Page Switch
          case Kodu::KoduAction::AT_PAGE_SWITCH:
            {
              std::cout << "page switch; ";
              // get the new requested page number (agent will switch to it immediately)
              if (!thisAgent->hasNewPageNumber()) {
                thisAgent->newReqdPage =
                  static_cast<Kodu::KoduActionPageSwitch*>(act)->getPageNumber();
                std::cout << "Switching to page " << thisAgent->newReqdPage << std::endl;
              }
              break;
            }

            // Kodu Action Play
          case Kodu::KoduAction::AT_PLAY:
            {
              std::cout << "play; ";
              // get the sound file
              std::string soundFile =
                static_cast<Kodu::KoduActionPlay*>(act)->getSoundFile();
              // add the sound file to the play queue
              thisAgent->playQueue.push(soundFile);
              std::cout << "Added the sound file \"" << soundFile << "\".\n";
              break;
            }

            // Kodu Action Say
          case Kodu::KoduAction::AT_SAY:
            {
              std::cout << "say; ";
              // make sure the agent does not have anything to say first
              if (!thisAgent->hasTextToSay()) {
                // give the string to the agent
                thisAgent->stringToSpeak =
                  static_cast<Kodu::KoduActionSay*>(act)->getStringToSpeak();
                std::cout << "Added literal string: \""
                          << thisAgent->stringToSpeak << "\".\n";
              }
              break;
            }

            // Kodu Action Score
          case Kodu::KoduAction::AT_SCORING:
            {
              std::cout << "score; ";
              // add the score change to the score queue
              thisAgent->scoreQueue.push(
                                         static_cast<Kodu::KoduActionScore*>(act)->getScoreChange());
              std::cout << "Added a score action type.\n";
              break;
            }

          default:
            {
              std::cerr << "Unrecognized action: \""
                        << act->getPrimitiveType() << "\"\n";
              break;
            }
          }
          // check if the agent needs to switch pages
          if (thisAgent->hasNewPageNumber())
            break;
        } else {
          // states rule evaluated false
          rule->setConditionEvalResult(false);
          // check the condition type...
          // if it is not a timer condition, then set the "action can run" flag to true
          if (!cond->getConditionType() != Kodu::KoduCondition::CT_TIMER) {
            act->setAgentCanUsePrimitive(true);
          }
          /**
           * check if the condition is type KoduConditionBump
           * if so, stop evaluating the condition, and create a perceptual task for it.
           * once the robot is close enough to detect a bump, the task will visually detect
           * bump. if the detection was successful, the agent would be able to evaluate the
           * rule again. if not, the rule is disabled for some time (temp fix)
           **/
          if (cond->getConditionType() == Kodu::KoduCondition::CT_BUMP
              && !thisAgent->isExecutingManipAction())
            {
              thisAgent->ptasks.push(new Kodu::VisualBumpDetectionTask(static_cast<Kodu::KoduConditionBump*>(cond), theWorld));
            }
        }
        // invalidate object
        Kodu::ObjectKeeper::isValid = false;
        cond = NULL;
        act = NULL;
      }
      kodustate->clearUtterances();

      if ( thisAgent->currMotionCmd.isValid() && !newMotionCmd.isValid() ) {
        cout << "Current motion no longer supported by rule: aborting." << endl;
        pilot->pilotAbort();
      }

      //PRINT_ATTRS("P-Task queue empty?", thisAgent->ptasks.empty());
      //PRINT_ATTRS("Multiplexor active?", multiplexorRef->isActive());
      if (!thisAgent->ptasks.empty() && !multiplexorRef->isActive()){
        //std::cout << "starting multiplexor...\n";
        multiplexorRef->start();
      }
      postStateCompletion();
    }

    unsigned int cycleCount;   // from $provide
  };

  //! Kodu Action Runner
  class KoduActionRunner : public StateNode {
   public:
    KoduActionRunner(const std::string &nodename="KoduActionRunner") : StateNode(nodename) {}
    virtual void doStart() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
      DropActionRunner* &dropActRef = getAncestor<KoduInterpreter>()->dropActRef;
      GrabActionRunner* &grabActRef = getAncestor<KoduInterpreter>()->grabActRef;
      GiveActionRunner* &giveActRef = getAncestor<KoduInterpreter>()->giveActRef;
      ReceiveActionRunner* &recvActRef = getAncestor<KoduInterpreter>()->recvActRef;
      MotionActionRunner* &motionActRef = getAncestor<KoduInterpreter>()->motionActRef;
      PageSwitchActionRunner* &pageSwitchActRef = getAncestor<KoduInterpreter>()->pageSwitchActRef;
      PlayActuator* &playActRef = getAncestor<KoduInterpreter>()->playActRef;
      SayActuator* &sayActRef = getAncestor<KoduInterpreter>()->sayActRef;
      ScoreActuator* &scoreActRef = getAncestor<KoduInterpreter>()->scoreActRef;
          
      //std::cout << "=== [Kodu Action Runner] ===\n";
      if (thisAgent->wantsToDropObject())
        dropActRef->start();
  
      if (thisAgent->wantsToGrabObject())
        grabActRef->start();
  
      if (thisAgent->wantsToGiveObject())
        giveActRef->start();
  
      if (thisAgent->agentIsReceiving)
        recvActRef->start();
  
      if (thisAgent->currMotionCmd.isValid() && !thisAgent->hasNewPageNumber()
          && !thisAgent->isExecutingManipAction())
        motionActRef->start();
  
      if (thisAgent->hasSoundsToPlay())
        playActRef->start();
          
      if (thisAgent->hasTextToSay())
        sayActRef->start();
              
      if (thisAgent->hasNewScoreChanges())
        scoreActRef->start();
  
      if (thisAgent->hasNewPageNumber())
        pageSwitchActRef->start();
    }
  };

  // ================================ Action Handlers ========================================= //

  // DropActionRunner

  class DropActionRunner : public StateNode {
   public:
    DropActionRunner(const std::string &nodename="DropActionRunner") : StateNode(nodename), gTask(NULL) {}

    ~DropActionRunner() {
      gTask = NULL;
    }

    class DropActionStart : public StateNode {
     public:
      DropActionStart(const std::string &nodename="DropActionStart") : StateNode(nodename) {}
      virtual void doStart() {
          std::cout << "[" << getName() << "]: initiating drop action.\n";
          std::cout << "Searching for the gripper monitor task.\n";
          // locate the gripper monitor task
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
        Kodu::VisualGripperMonitorTask* &gTask = getAncestor<DropActionRunner>()->gTask;
          gTask = NULL;
          unsigned int tasksToExecute = thisAgent->ptasks.size();
          while (tasksToExecute > 0) {
            if (thisAgent->ptasks.front()->getType() == Kodu::PT_GRIPPER_VIS_MONITOR) {
              gTask = static_cast<Kodu::VisualGripperMonitorTask*>(thisAgent->ptasks.front());
              thisAgent->ptasks.pop();
            } else {
              thisAgent->ptasks.push(thisAgent->ptasks.front());
              thisAgent->ptasks.pop();
            }
            tasksToExecute--;
          }
    
          // should never happen, but just in case...
          if (gTask == NULL) {
            std::cout << "THIS IS A SERIOUS ERROR!!!---the gripper task should not be null!\n";
            postStateFailure();
            return;
          }
          std::cout << "GripMonTask id = " << gTask->getTaskId() << std::endl;
          postStateCompletion();
      }
    };

    class ExecuteDrop : public StateNode {
     public:
      ExecuteDrop(const std::string &nodename="ExecuteDrop") : StateNode(nodename), objFutureEgoPos(), kReverseDist(120.0f) {}
  
      class DropItem : public ArmNode {
       public:
        DropItem(const std::string &nodename="DropItem") : ArmNode(nodename) {}
        virtual void doStart() {
              std::cout << "[" << getName() << "]: releasing the item.\n";
              // open the gripper --- this value is assuming the cylinder's diameter < 120mm
              getMC()->openGripper(0.45f);
        }
      };
  
      class LookAtObjectAprilTag : public MapBuilderNode {
       public:
        LookAtObjectAprilTag(const std::string &nodename="LookAtObjectAprilTag") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
        virtual void doStart() {
              std::cout << "[" << getName() << "]: looking for released object's april tag\n";
          Kodu::VisualGripperMonitorTask* &gTask = getAncestor<DropActionRunner>()->gTask;
              // create a mapbuilder request to locate the april tag one more time.
              // will be used to identify the correct object if more than one are in view
              // once the object is released.
              mapreq.setAprilTagFamily();
              NEW_SHAPE(gazePoint, PointData, new PointData(localShS, gTask->getTagCentroid()));
              mapreq.searchArea = gazePoint;
              mapreq.clearLocal = true;
        }
      };
  
      class RecordAprilTagPos : public StateNode {
       public:
        RecordAprilTagPos(const std::string &nodename="RecordAprilTagPos") : StateNode(nodename) {}
        virtual void doStart() {
              std::cout << "[" << getName() << "]: attempting to record april tag's position.\n";
              // find the object's april tag position and log it
          Kodu::VisualGripperMonitorTask* &gTask = getAncestor<DropActionRunner>()->gTask;
              NEW_SHAPE(objTag, AprilTagData, find_if<AprilTagData>(localShS,
                                                                    Kodu::HasAprilTagID(gTask->getTagId())));
              // check if the shape is valid (that means the tag was found)
              if (objTag.isValid()) {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          Point &objFutureEgoPos = getAncestor<ExecuteDrop>()->objFutureEgoPos;
          float &kReverseDist = getAncestor<ExecuteDrop>()->kReverseDist;
                std::cout << "found the tag @ " << objTag->getCentroid() << "; logging it\n";
                const Point& kTagCurrPos = objTag->getCentroid();
                const float kCylHeight
                  = ShapeRootTypeConst(thisAgent->gripperObject, CylinderData)->getHeight();
                objFutureEgoPos = Point(kTagCurrPos.coordX() + kReverseDist, kTagCurrPos.coordY(),
                                        (kCylHeight / 2.0f), egocentric);
                postStateSuccess();
              }
              // else, report an error... lighting could be an issue if this did happen though
              else {
                std::cout << "DID NOT FIND THE APRIL TAG!!!\n";
                postStateFailure();
              }
        }
      };
  
      class ReverseBody : public PilotNode {
       public:
        ReverseBody(const std::string &nodename="ReverseBody") : PilotNode(nodename,PilotTypes::walk) {}
        virtual void doStart() {
              std::cout << "[" << getName() << "]: reversing body by ";
          float &kReverseDist = getAncestor<ExecuteDrop>()->kReverseDist;
              pilotreq.dx = -1.0f * kReverseDist;
              std::cout << pilotreq.dx << "mm\n";
        }
      };
  
      class LocalizeAgent : public PilotNode {
       public:
        LocalizeAgent(const std::string &nodename, unsigned int _numbOfStarsRequested) : PilotNode(nodename), numbOfStarsRequested(_numbOfStarsRequested) {}
        unsigned int numbOfStarsRequested;  // cache the constructor's parameter
        virtual void doStart() {
              std::cout << "[" << getName() << "]: attempting to localize\n";
          Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
              // create a localization task, and execute it before adding the shape in the world
              Kodu::VisualLocalizationTask localizeTask(theWorld->getStarConstellation(),
                                                        numbOfStarsRequested);
              pilotreq = localizeTask.getPilotRequest();
        }
      };
  
      class RepositionReleasedObject : public StateNode {
       public:
        RepositionReleasedObject(const std::string &nodename="RepositionReleasedObject") : StateNode(nodename) {}
    
        class LookForObjectAgain : public MapBuilderNode {
         public:
          LookForObjectAgain(const std::string &nodename="LookForObjectAgain") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
          virtual void doStart() {
                  std::cout << "[" << getName() << "]: looking for object again\n";
            Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
            Point &objFutureEgoPos = getAncestor<ExecuteDrop>()->objFutureEgoPos;
                  // create the point the agent should look at
                  NEW_SHAPE(gazePoint, PointData, new PointData(localShS, objFutureEgoPos));
                  const float kCylRadius
                    = ShapeRootTypeConst(thisAgent->gripperObject, CylinderData)->getRadius();
        
                  // construct the mapbuilder request
                  mapreq.addAttributes(thisAgent->gripperObject);
                  mapreq.searchArea = gazePoint;
                  mapreq.maxDist = objFutureEgoPos.xyNorm() + (kCylRadius * 2.0f);
                  mapreq.pursueShapes = true;
                  mapreq.clearLocal = true;
          }
        };
    
        class RepositionObject : public StateNode {
         public:
          RepositionObject(const std::string &nodename="RepositionObject") : StateNode(nodename) {}
          virtual void doStart() {
                  std::cout << "[" << getName() << "]: attempting to 'reposition' the object\n";
            Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
            Point &objFutureEgoPos = getAncestor<ExecuteDrop>()->objFutureEgoPos;
                  // get all the shapes that are the same type as the object in the gripper
                  ShapeType_t objType = thisAgent->gripperObject->getType();
                  std::vector<ShapeRoot> localShapes(localShS.allShapes(objType));
        
                  // should not ever happen, but just in case...
                  if (localShapes.empty()) {
                    std::cout << "THIS IS A SERIOUS ERROR!\n";
                    postStateFailure();
                    return;
                  }
        
                  // get the closest object to the precalculated point
                  ShapeRoot closestMatch = Kodu::getClosestObjectToPoint(localShapes, objFutureEgoPos);
                  // apply tranformation to closest match (egocentric --> allocentric)
                  Point closestMatchPt(closestMatch->getCentroid());
                  closestMatchPt.applyTransform(VRmixin::mapBuilder->localToWorldMatrix, allocentric);
                  std::cout << "Repositioning the object to from "
                            << thisAgent->gripperObject->getCentroid() << " to "
                            << closestMatchPt << std::endl;
                            
                  // reposition the gripper object
                  thisAgent->gripperObject->setPosition(closestMatchPt);
                  thisAgent->gripperObject.setInvalid();
                  postStateSuccess();
          }
        };
    
        virtual void setup() {
         LookForObjectAgain *lookAgain = new LookForObjectAgain("lookAgain");
         addNode(lookAgain);

         RepositionObject *reposition = new RepositionObject("reposition");
         addNode(reposition);

         PostMachineSuccess *postmachinesuccess4 = new PostMachineSuccess("postmachinesuccess4");
         addNode(postmachinesuccess4);

         SpeechNode *speechnode5 = new SpeechNode("speechnode5","could not find any local shapes");
         addNode(speechnode5);

         PostMachineFailure *postmachinefailure6 = new PostMachineFailure("postmachinefailure6");
         addNode(postmachinefailure6);

         startnode = lookAgain;

         lookAgain->addTransition(new CompletionTrans("completiontrans27",reposition));
         reposition->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans26",postmachinesuccess4,StateNode::successSignal));
         reposition->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans27",speechnode5,StateNode::failureSignal));
         speechnode5->addTransition(new CompletionTrans("completiontrans28",postmachinefailure6));
        }
      };
  
      virtual void setup() {
       DropItem *drop = new DropItem("drop");
       addNode(drop);

       LookAtObjectAprilTag *lookAtTag = new LookAtObjectAprilTag("lookAtTag");
       addNode(lookAtTag);

       RecordAprilTagPos *recordATPos = new RecordAprilTagPos("recordATPos");
       addNode(recordATPos);

       ReverseBody *reverse = new ReverseBody("reverse");
       addNode(reverse);

       ParkArm *parkArm = new ParkArm("parkArm");
       addNode(parkArm);

       LocalizeAgent *localize2 = new LocalizeAgent("localize2",Kodu::VisualLocalizationTask::kMinStarsRequiredToLocalize);
       addNode(localize2);

       LocalizeAgent *localize3 = new LocalizeAgent("localize3",Kodu::VisualLocalizationTask::kMinStarsRequiredToLocalize + 1);
       addNode(localize3);

       RepositionReleasedObject *attempRepos = new RepositionReleasedObject("attempRepos");
       addNode(attempRepos);

       SpeechNode *speechnode6 = new SpeechNode("speechnode6","error occurred while dropping object");
       addNode(speechnode6);

       PostMachineFailure *postmachinefailure7 = new PostMachineFailure("postmachinefailure7");
       addNode(postmachinefailure7);

       SpeechNode *speechnode7 = new SpeechNode("speechnode7","could not record the tag position");
       addNode(speechnode7);

       PostMachineFailure *postmachinefailure8 = new PostMachineFailure("postmachinefailure8");
       addNode(postmachinefailure8);

       StateNode *statenode2 = new StateNode("statenode2");
       addNode(statenode2);

       SpeechNode *speechnode8 = new SpeechNode("speechnode8","localize failed");
       addNode(speechnode8);

       PostMachineFailure *postmachinefailure9 = new PostMachineFailure("postmachinefailure9");
       addNode(postmachinefailure9);

       PostMachineSuccess *postmachinesuccess5 = new PostMachineSuccess("postmachinesuccess5");
       addNode(postmachinesuccess5);

       PostMachineFailure *postmachinefailure10 = new PostMachineFailure("postmachinefailure10");
       addNode(postmachinefailure10);

       startnode = drop;

       drop->addTransition(new CompletionTrans("completiontrans29",lookAtTag));
       lookAtTag->addTransition(new CompletionTrans("completiontrans30",recordATPos));
       recordATPos->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans28",reverse,StateNode::successSignal));

       CompletionTrans *completiontrans31 = new CompletionTrans("completiontrans31",localize2);
       completiontrans31->addDestination(parkArm);
       reverse->addTransition(completiontrans31);

       drop->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans29",speechnode6,StateNode::failureSignal));
       speechnode6->addTransition(new CompletionTrans("completiontrans32",postmachinefailure7));
       recordATPos->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans30",speechnode7,StateNode::failureSignal));
       speechnode7->addTransition(new CompletionTrans("completiontrans33",postmachinefailure8));
       localize2->addTransition(new PilotTrans("pilottrans3",attempRepos,PilotTypes::noError));
       localize2->addTransition(new PilotTrans("pilottrans4",localize3,PilotTypes::cantLocalize));
       parkArm->addTransition(new CompletionTrans("completiontrans34",statenode2));
       localize3->addTransition(new PilotTrans("pilottrans5",attempRepos,PilotTypes::noError));
       localize3->addTransition(new PilotTrans("pilottrans6",speechnode8,PilotTypes::cantLocalize));
       speechnode8->addTransition(new CompletionTrans("completiontrans35",postmachinefailure9));
       attempRepos->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans31",postmachinesuccess5,StateNode::successSignal));
       attempRepos->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans32",postmachinefailure10,StateNode::failureSignal));
      }
      Point objFutureEgoPos;   // from $provide
      float kReverseDist;   // from $provide
    };

    class DropActionEnd : public StateNode {
     public:
      DropActionEnd(const std::string &nodename="DropActionEnd") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
        Kodu::VisualGripperMonitorTask* &gTask = getAncestor<DropActionRunner>()->gTask;
          std::cout << "[" << getName() << "]: finishing up drop action\n";
          // delete the april tag's created during localization
          std::vector<ShapeRoot> tagVec = worldShS.allShapes(aprilTagDataType);
          worldShS.deleteShapes(tagVec);
          // delete the gripper monitor task
          GeneralFncs::deletePtr(gTask);
          // states the object is no longer in the gripper, and release the hold on
          // executing manipulation actions (and implicitly motion too)
          thisAgent->manipulationComplete();
          postStateCompletion();
      }
    };

    virtual void setup() {
     DropActionStart *dStart = new DropActionStart("dStart");
     addNode(dStart);

     ExecuteDrop *exec = new ExecuteDrop("exec");
     addNode(exec);

     DropActionEnd *dEnd = new DropActionEnd("dEnd");
     addNode(dEnd);

     SpeechNode *speechnode9 = new SpeechNode("speechnode9","failure during drop execution");
     addNode(speechnode9);

     PostMachineCompletion *postmachinecompletion3 = new PostMachineCompletion("postmachinecompletion3");
     addNode(postmachinecompletion3);

     startnode = dStart;

     dStart->addTransition(new CompletionTrans("completiontrans36",exec));
     exec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans33",dEnd,StateNode::successSignal));
     exec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans34",speechnode9,StateNode::failureSignal));
     speechnode9->addTransition(new CompletionTrans("completiontrans37",dEnd));
     dEnd->addTransition(new CompletionTrans("completiontrans38",postmachinecompletion3));
    }

  private:
    DISALLOW_COPY_ASSIGN(DropActionRunner);
    Kodu::VisualGripperMonitorTask* gTask;   // from $provide
  };

  // GrabActionRunner

  class GrabActionRunner : public StateNode {
   public:
    GrabActionRunner(const std::string &nodename="GrabActionRunner") : StateNode(nodename), objectTagId(-1) {}
        
    class GrabActionStart : public StateNode {
     public:
      GrabActionStart(const std::string &nodename="GrabActionStart") : StateNode(nodename) {}
  
        // first check if the agent is near enough to the object
      class IsObjectNear : public StateNode {
       public:
        IsObjectNear(const std::string &nodename="IsObjectNear") : StateNode(nodename) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              std::cout << "[" << getName() << "]: is agent near object? ";
              if (Kodu::distanceInBetweenAgentAndObject(thisAgent->gripperObject)
                  < Kodu::KoduConditionBump::kMaxDistanceAwayToSenseBump)
                {
                  std::cout << "yes, it is!\n";
                  postStateSuccess();
                } else {
                std::cout << "no, it is not. making gripperObject reference invalid.\n";
                thisAgent->gripperObject = ShapeRoot();
                postStateFailure();
              }
        }
      };
  
        /**
         * ASSUMPTIONS:
         *  - the object was bumped before this action was executed.
         **/
      class GetTagId : public StateNode {
       public:
        GetTagId(const std::string &nodename="GetTagId") : StateNode(nodename) {}
        virtual void doStart() {
              std::cout << "[" << getName() << "]: getting tag id. (id = ";
          Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
          int &objectTagId = getAncestor<GrabActionRunner>()->objectTagId;
              // if the {world shape id, tag id} pair exists, log the tag id
              if (theWorld->shapeTagPairExists(theWorld->thisAgent.gripperObject.getId())) {
                objectTagId = theWorld->getTagIdForShape(theWorld->thisAgent.gripperObject.getId());
              }
              std::cout << objectTagId << ").\n";
              std::cout << "target id = " << theWorld->thisAgent.gripperObject.getId() << std::endl;
              postStateCompletion();
        }
      };
  
      virtual void setup() {
       IsObjectNear *isNear = new IsObjectNear("isNear");
       addNode(isNear);

       GetTagId *getTagId = new GetTagId("getTagId");
       addNode(getTagId);

       PostMachineSuccess *postmachinesuccess6 = new PostMachineSuccess("postmachinesuccess6");
       addNode(postmachinesuccess6);

       PostMachineFailure *postmachinefailure11 = new PostMachineFailure("postmachinefailure11");
       addNode(postmachinefailure11);

       startnode = isNear;

       isNear->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans35",getTagId,StateNode::successSignal));
       getTagId->addTransition(new CompletionTrans("completiontrans39",postmachinesuccess6));
       isNear->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans36",postmachinefailure11,StateNode::failureSignal));
      }
    };

    /**
     * Positions the body so that the robot can grab the object
     * ASSUMPTIONS:
     *  - the item is graspable, and there is nothing in between the agent and the target object
     **/
    class PrepareBody : public StateNode {
     public:
      PrepareBody(const std::string &nodename="PrepareBody") : StateNode(nodename) {}
  
        /**
         * Face the object.
         * ASSUMPTIONS:
         * - the arm is parked
         **/
      class FaceObject : public PilotNode {
       public:
        FaceObject(const std::string &nodename="FaceObject") : PilotNode(nodename,PilotTypes::walk) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              std::cout << "[" << getName() << "]: turning towards object\n";
              pilotreq.da = Kodu::bearingFromAgentToObject(thisAgent->gripperObject);
        }
      };
  
        /**
         * Reverse the body so the arm has enough space to swivel without hitting the object
         * ASSUMPTIONS:
         *  - the arm's length is less than or equal to kArmMaxLength
         **/
      class ReverseBody : public PilotNode {
       public:
        ReverseBody(const std::string &nodename="ReverseBody") : PilotNode(nodename,PilotTypes::walk) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              std::cout << "[" << getName() << "]: reversing body ";
              float const kArmMaxLength = 210.0f;
              float distInBtwObjects = Kodu::distanceInBetweenAgentAndObject(thisAgent->gripperObject);
              pilotreq.dx = -1.0f * kArmMaxLength;
              std::cout << std::fabs(pilotreq.dx) << "mm (" << kArmMaxLength << " + "
                        << distInBtwObjects << ")\n";
        }
      };
  
      virtual void setup() {
       FaceObject *face = new FaceObject("face");
       addNode(face);

       ReverseBody *rvrs = new ReverseBody("rvrs");
       addNode(rvrs);

       PostMachineCompletion *postmachinecompletion4 = new PostMachineCompletion("postmachinecompletion4");
       addNode(postmachinecompletion4);

       startnode = face;

       face->addTransition(new CompletionTrans("completiontrans40",rvrs));
       rvrs->addTransition(new CompletionTrans("completiontrans41",postmachinecompletion4));
      }
    };

    class ExecuteGrabAction : public StateNode {
     public:
      ExecuteGrabAction(const std::string &nodename="ExecuteGrabAction") : StateNode(nodename) {}
              
        /**
         * Attempt to grab the object
         * ASSUMPTIONS:
         *  - the robot's arm will not hit the object (the PrepareBody state machine worked)
         **/
      class GrabObject : public GrasperNode {
       public:
        GrabObject(const std::string &nodename="GrabObject") : GrasperNode(nodename,GrasperRequest::grasp) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              std::cout << "[" << getName() << "]: attempting to grab " << thisAgent->gripperObject
                        << std::endl;
              graspreq.object = thisAgent->gripperObject;
        }
      };
  
      class VerifyObjectWasGrabbed : public StateNode {
       public:
        VerifyObjectWasGrabbed(const std::string &nodename="VerifyObjectWasGrabbed") : StateNode(nodename) {}
    
        class LookAtTheGripper : public MapBuilderNode {
         public:
          LookAtTheGripper(const std::string &nodename="LookAtTheGripper") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
          virtual void doStart() {
                  std::cout << "[" << getName() << "]: looking at tag\n";
                  mapreq.clearLocal = true;
                  // search for an april tag
                  mapreq.setAprilTagFamily();
                  // look at the gripper
                  fmat::Column<3> gripperLoc;
        #if defined(TGT_IS_CALLIOPE2)
                  gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
        #endif
                  NEW_SHAPE(gazePoint, PointData, new PointData(localShS,
                                                                Point(gripperLoc[0], gripperLoc[1], gripperLoc[2], egocentric)));
                  mapreq.searchArea = gazePoint;
          }
        };
    
        class VerifyObjectInGripper : public StateNode {
         public:
          VerifyObjectInGripper(const std::string &nodename="VerifyObjectInGripper") : StateNode(nodename) {}
          virtual void doStart() {
            Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
            Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                  std::cout << "[" << getName() << "]: verifying object was grabbed.\n";
                            
                  // check if a particular tag was seen
                  NEW_SHAPE(tag, AprilTagData, 
                            find_if<AprilTagData>(localShS,
                                                  Kodu::HasAprilTagID(theWorld->getTagIdForShape(thisAgent->gripperObject.getId()))));
                            
                  // if the reference is not valid, post a failure
                  if (!tag.isValid()) {
                    std::cout << "THE TAG WAS NOT SEEN (FAILURE)!!!\n";
                    postStateFailure();
                    return;
                  }
        
                  // ********** temp fix
                  // 
                  // get the gripper location
                  fmat::Column<3> gripperLoc;
        #if defined(TGT_IS_CALLIOPE2)
                  gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
        #endif
                  Point gripperPoint(gripperLoc[0], gripperLoc[1], gripperLoc[2], egocentric);
        
                  // check if the object is in the gripper
                  if (tag->getCentroid().xyDistanceFrom(gripperPoint)
                      < (Kodu::objectRadius(thisAgent->gripperObject) * 1.25f))
                    {
                      std::cout << "successfully grabbed object\n";
                      postStateSuccess();
                    } else {
                    std::cout << "failed to grab object... dist = "
                              << tag->getCentroid().xyDistanceFrom(gripperPoint)
                              << "mm; reattempting.\n";
                    postStateFailure();
                  }
                  // ***********
          }
        };
    
        virtual void setup() {
         LookAtTheGripper *lookAtGripper = new LookAtTheGripper("lookAtGripper");
         addNode(lookAtGripper);

         VerifyObjectInGripper *verifyObjGrabbed = new VerifyObjectInGripper("verifyObjGrabbed");
         addNode(verifyObjGrabbed);

         PostMachineSuccess *postmachinesuccess7 = new PostMachineSuccess("postmachinesuccess7");
         addNode(postmachinesuccess7);

         PostMachineFailure *postmachinefailure12 = new PostMachineFailure("postmachinefailure12");
         addNode(postmachinefailure12);

         startnode = lookAtGripper;

         lookAtGripper->addTransition(new CompletionTrans("completiontrans42",verifyObjGrabbed));
         verifyObjGrabbed->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans37",postmachinesuccess7,StateNode::successSignal));
         verifyObjGrabbed->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans38",postmachinefailure12,StateNode::failureSignal));
        }
      };
  
      class PrepareForAnotherGrasp : public StateNode {
       public:
        PrepareForAnotherGrasp(const std::string &nodename="PrepareForAnotherGrasp") : StateNode(nodename), objFutureEgoPos() {}
    
        class NeedToLookAround : public StateNode {
         public:
          NeedToLookAround(const std::string &nodename="NeedToLookAround") : StateNode(nodename) {}
          virtual void doStart() {
            int &objectTagId = getAncestor<GrabActionRunner>()->objectTagId;
                            
                  std::cout << "[" << getName() << "]: checking if robot needs to look around\n";
                  // get a particular april tag
                  ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
                  postStateSignal<bool>(!tag.isValid());
          }
        };
    
        class LookAroundForObject : public MapBuilderNode {
         public:
          LookAroundForObject(const std::string &nodename="LookAroundForObject") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
          virtual void doStart() {
                  std::cout << "[" << getName() << "]: attempting to locate object (again)\n";
                  // generate points to look at (near the robot's body)
                  // these points could also be generated by looking at the gripper's y-pos
                  std::vector<Point> pts;
                  pts.push_back(Point(  300,    0,    0, egocentric));
                  pts.push_back(Point(  500,    0,    0, egocentric));
                  pts.push_back(Point(  500, -200,    0, egocentric));
                  pts.push_back(Point(  300, -200,    0, egocentric));
                  NEW_SHAPE(gazePoints, PolygonData, new PolygonData(localShS, pts, false));
                            
                  // construct the mapbuilder request
                  mapreq.searchArea = gazePoints;
                  mapreq.setAprilTagFamily();
                  mapreq.clearLocal = true;
          }
        };
    
        class RepositionBody : public PilotNode {
         public:
          RepositionBody(const std::string &nodename="RepositionBody") : PilotNode(nodename,PilotTypes::walk) {}
          virtual void doStart() {
            Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
            int &objectTagId = getAncestor<GrabActionRunner>()->objectTagId;
            Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
                  
                  std::cout << "[" << getName() << "]: walking distance = ";
                  // get a particular april tag
                  ShapeRoot tag = find_if(localShS, Kodu::HasAprilTagID(objectTagId));
        
                  // get the gripper location
                  fmat::Column<3> gripperLoc;
        #if defined(TGT_IS_CALLIOPE2)
                  gripperLoc = kine->linkToBase(GripperFrameOffset).translation();
        #endif
                  // where I would really want the objetc to be is the radial reach of the robot
                  // plus 50 (or 60) mm
                  float objDesiredXPos = gripperLoc[0] + 150.0f;
                            
                  const Point& kTagPt = tag->getCentroid();
                  float objRadius = Kodu::objectRadius(thisAgent->gripperObject);
                            
                  pilotreq.dx = kTagPt.coordX() - objRadius - objDesiredXPos;
                  std::cout << pilotreq.dx << "mm)\n";
                            
                  // reposition the gripper target object
                  objFutureEgoPos = Point(objDesiredXPos, kTagPt.coordY(), (kTagPt.coordZ() / 2.0f),
                                          egocentric);
                  pilotreq.collisionAction = collisionIgnore;
          }
        };
    
        class FindObjectAgain : public MapBuilderNode {
         public:
          FindObjectAgain(const std::string &nodename="FindObjectAgain") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
          virtual void doStart() {
            Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
            Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
        
                  std::cout << "[" << getName() << "]: looking for gripper object (again)\n";
                  // search for the object again using the point calculated previously
                  NEW_SHAPE(gazePoint, PointData, new PointData(localShS, objFutureEgoPos));
                  mapreq.searchArea = gazePoint;
                  const ShapeRoot& targetShape = thisAgent->gripperObject;
                  mapreq.addAttributes(targetShape);
                  mapreq.maxDist = objFutureEgoPos.xyNorm() + (Kodu::objectRadius(targetShape) * 2.5f);
                  mapreq.pursueShapes = true;
          }
        };
    
        class RepositionGripperObject : public StateNode {
         public:
          RepositionGripperObject(const std::string &nodename="RepositionGripperObject") : StateNode(nodename) {}
          virtual void doStart() {
            Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
            Point &objFutureEgoPos = getAncestor<PrepareForAnotherGrasp>()->objFutureEgoPos;
                            
                  std::cout << "[" << getName() << "]: attempting to reposition gripper object.\n";
                  NEW_SHAPEROOTVEC(objs, subset(localShS,
                                                Kodu::IsShapeOfType(thisAgent->gripperObject->getType())));
                  ShapeRoot targetShape = Kodu::getClosestObjectToPoint(objs, objFutureEgoPos);
                  // check if the target shape is valid
                  if (targetShape.isValid()) {
                    std::cout << "target shape is valid. setting its allocentric position.\n";
                    Point objAllocentricPt = targetShape->getCentroid();
                    objAllocentricPt.applyTransform(VRmixin::mapBuilder->localToWorldMatrix,
                                                    allocentric);
                    std::cout << "Repositioning the object to from "
                              << thisAgent->gripperObject->getCentroid() << " to "
                              << objAllocentricPt << std::endl;
                    thisAgent->gripperObject->setPosition(objAllocentricPt);
                    postStateSuccess();
                  }
                  // should only fail if the robot did not see any cylinders
                  else {
                    std::cout << "target shape is NOT VALID!!!! Don't know what to do!\n";
                    postStateFailure();
                  }
          }
        };
    
        virtual void setup() {
         NeedToLookAround *needToLook = new NeedToLookAround("needToLook");
         addNode(needToLook);

         LookAroundForObject *lookAround = new LookAroundForObject("lookAround");
         addNode(lookAround);

         RepositionBody *reposBody = new RepositionBody("reposBody");
         addNode(reposBody);

         FindObjectAgain *findObjAgain = new FindObjectAgain("findObjAgain");
         addNode(findObjAgain);

         RepositionGripperObject *reposGripObj = new RepositionGripperObject("reposGripObj");
         addNode(reposGripObj);

         PostMachineSuccess *postmachinesuccess8 = new PostMachineSuccess("postmachinesuccess8");
         addNode(postmachinesuccess8);

         SpeechNode *speechnode10 = new SpeechNode("speechnode10","error shape was not valid");
         addNode(speechnode10);

         PostMachineFailure *postmachinefailure13 = new PostMachineFailure("postmachinefailure13");
         addNode(postmachinefailure13);

         startnode = needToLook;

         needToLook->addTransition(new SignalTrans<bool>("signaltrans39",lookAround,true));
         lookAround->addTransition(new CompletionTrans("completiontrans43",reposBody));
         needToLook->addTransition(new SignalTrans<bool>("signaltrans40",reposBody,false));
         reposBody->addTransition(new CompletionTrans("completiontrans44",findObjAgain));
         findObjAgain->addTransition(new CompletionTrans("completiontrans45",reposGripObj));
         reposGripObj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans41",postmachinesuccess8,StateNode::successSignal));
         reposGripObj->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans42",speechnode10,StateNode::failureSignal));
         speechnode10->addTransition(new CompletionTrans("completiontrans46",postmachinefailure13));
        }
        Point objFutureEgoPos;   // from $provide
      };
  
      class CreateGripMonTask : public StateNode {
       public:
        CreateGripMonTask(const std::string &nodename="CreateGripMonTask") : StateNode(nodename) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          int &objectTagId = getAncestor<GrabActionRunner>()->objectTagId;
              std::cout << "[" << getName() << "]: creating perceptual task; ";
              Shape<AprilTagData> tag = find_if<AprilTagData>(localShS,
                                                              Kodu::HasAprilTagID(objectTagId));
              thisAgent->ptasks.push(
                                     new Kodu::VisualGripperMonitorTask(thisAgent->gripperObject, tag));
              std::cout << "centroid = " << tag->getCentroid() << "; id = "
                        << tag->getTagID();
              std::cout << "; angle = " << tag->getCentroid().atanYX() << std::endl;
              postStateCompletion();
        }
      };
  
      virtual void setup() {
       GrabObject *grabObj = new GrabObject("grabObj");
       addNode(grabObj);

       VerifyObjectWasGrabbed *verifyObjGrab = new VerifyObjectWasGrabbed("verifyObjGrab");
       addNode(verifyObjGrab);

       PrepareForAnotherGrasp *prepForRetry = new PrepareForAnotherGrasp("prepForRetry");
       addNode(prepForRetry);

       CreateGripMonTask *createTask = new CreateGripMonTask("createTask");
       addNode(createTask);

       PostMachineSuccess *postmachinesuccess9 = new PostMachineSuccess("postmachinesuccess9");
       addNode(postmachinesuccess9);

       SpeechNode *speechnode11 = new SpeechNode("speechnode11","unhandled error using grab action");
       addNode(speechnode11);

       PostMachineFailure *postmachinefailure14 = new PostMachineFailure("postmachinefailure14");
       addNode(postmachinefailure14);

       startnode = grabObj;

       grabObj->addTransition(new GrasperTrans("graspertrans2",verifyObjGrab));
       verifyObjGrab->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans43",createTask,StateNode::successSignal));
       createTask->addTransition(new CompletionTrans("completiontrans47",postmachinesuccess9));
       verifyObjGrab->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans44",prepForRetry,StateNode::failureSignal));
       prepForRetry->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans45",grabObj,StateNode::successSignal));
       prepForRetry->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans46",speechnode11,StateNode::failureSignal));
       speechnode11->addTransition(new CompletionTrans("completiontrans48",postmachinefailure14));
      }
  
    };

    class GrabActionEnd : public StateNode {
     public:
      GrabActionEnd(const std::string &nodename="GrabActionEnd") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          std::cout << "[" << getName() << "]: finishing up grab action\n";
    
          // reposition the gripper object (in the worldShS) outside the world bounds
          thisAgent->gripperObject->setPosition(Point(3000.0f, 3000.0f, 0.0f, allocentric));
          std::cout << "repositioning the grabbed object to "
                    << thisAgent->gripperObject->getCentroid() << std::endl;
    
          // the agent is not longer attempting to grab the object--the agent succeeded in grabbing it
          // the target object is (should be) in the gripper
          thisAgent->manipulationComplete();
          std::cout << "Grab action complete.\n";
          postStateCompletion();
      }
    };

    virtual void setup() {
     GrabActionStart *gStart = new GrabActionStart("gStart");
     addNode(gStart);

     PrepareBody *prep = new PrepareBody("prep");
     addNode(prep);

     ExecuteGrabAction *exec = new ExecuteGrabAction("exec");
     addNode(exec);

     GrabActionEnd *gEnd = new GrabActionEnd("gEnd");
     addNode(gEnd);

     SpeechNode *speechnode12 = new SpeechNode("speechnode12","failure during start node in grab action");
     addNode(speechnode12);

     PostMachineCompletion *postmachinecompletion5 = new PostMachineCompletion("postmachinecompletion5");
     addNode(postmachinecompletion5);

     SpeechNode *speechnode13 = new SpeechNode("speechnode13","failure during execution node in grab action");
     addNode(speechnode13);

     startnode = gStart;

     gStart->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans47",prep,StateNode::successSignal));
     gStart->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans48",speechnode12,StateNode::failureSignal));
     speechnode12->addTransition(new CompletionTrans("completiontrans49",gEnd));
     prep->addTransition(new CompletionTrans("completiontrans50",exec));
     exec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans49",gEnd,StateNode::successSignal));
     gEnd->addTransition(new CompletionTrans("completiontrans51",postmachinecompletion5));
     exec->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans50",speechnode13,StateNode::failureSignal));
     speechnode13->addTransition(new CompletionTrans("completiontrans52",gEnd));
    }
    int objectTagId;   // from $provide
  };

  // end of GrabActionRunner


  // GiveActionRunner

  class GiveActionRunner : public StateNode {
   public:
    GiveActionRunner(const std::string &nodename="GiveActionRunner") : StateNode(nodename) {}
    class PositionBody : public VisualRoutinesStateNode {
     public:
      PositionBody(const std::string &nodename="PositionBody") : VisualRoutinesStateNode(nodename), dx() {}
  
      class Rotate : public WalkNode {
       public:
        Rotate(const std::string &nodename="Rotate") : WalkNode(nodename) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          float &dx = getAncestor<PositionBody>()->dx;
      
              const float buffer = 550;
              Point target = thisAgent->giveTargetObject->getCentroid() - theAgent->getCentroid();
              target = target.unitVector()*(target.xyNorm() - buffer);
              req = WalkRequest::displacement(0, 0, target.atanYX());
              dx = target.xyNorm();
        }
      };
  
      class Walk : public WalkNode {
       public:
        Walk(const std::string &nodename="Walk") : WalkNode(nodename) {}
        virtual void doStart() {
          float &dx = getAncestor<PositionBody>()->dx;
      
              req = WalkRequest::displacement(dx, 0, 0);
        }
      };
  
      virtual void setup() {
       Rotate *rotate1 = new Rotate("rotate1");
       addNode(rotate1);

       Walk *walk1 = new Walk("walk1");
       addNode(walk1);

       PostMachineCompletion *postmachinecompletion6 = new PostMachineCompletion("postmachinecompletion6");
       addNode(postmachinecompletion6);

       startnode = rotate1;

       rotate1->addTransition(new CompletionTrans("completiontrans53",walk1));
       walk1->addTransition(new CompletionTrans("completiontrans54",postmachinecompletion6));
      }
      float dx;   // from $provide
    };

    class Drop : public VisualRoutinesStateNode {
     public:
      Drop(const std::string &nodename="Drop") : VisualRoutinesStateNode(nodename) {}
      virtual void doStart() {
        DropActionRunner* &dropActRef = getAncestor<KoduInterpreter>()->dropActRef;
          dropActRef->start();
          postStateCompletion();
      }
    };

    class Backup : public PilotNode {
     public:
      Backup(const std::string &nodename="Backup") : PilotNode(nodename,PilotTypes::walk) {}
      virtual void doStart() {
          pilotreq = PilotRequest(PilotTypes::walk);
          pilotreq.dx = -150;
      }
    };

    class GiveActionStart : public StateNode {
     public:
      GiveActionStart(const std::string &nodename="GiveActionStart") : StateNode(nodename) {}
              void doStart() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                  std::string name = thisAgent->giveTargetObject->getName();
                  std::cout << "Giving to " << thisAgent->giveTargetObject->getName() << std::endl;
                  bool found = false;
                  for (map<int,player_identity>::iterator it = KoduDiscover::players.begin();
                       it != KoduDiscover::players.end(); ++it) {
                      if (KoduType::strs[it->second.type] == name && it->first != 0) {
                          thisAgent->giveTargetObject->setHostAddr(it->first);
                          thisAgent->gotGiveReady = false;
                          KoduGiveEvent evt;
                          evt.setDestination(thisAgent->giveTargetObject->getHostAddr());
                          erouter->postEvent(evt);
                          erouter->addTimer(this, 0, 200, true);
                          found = true;
                          std::cout << "Found player at " << it->first << std::endl;
                          break;
                      }
                  }
  
                  if (!found) {
                      std::cout << "Couldn't find player " << name << " in map" << std::endl;
                  }
              }
  
        void doEvent() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
  
                  if (event->getGeneratorID() == EventBase::timerEGID) {
  
                      if (thisAgent->gotGiveReady) {
                          erouter->removeTimer(this);
                          postStateCompletion();
                      }
                  }
        }
    };

    class GiveActionSend : public StateNode {
     public:
      GiveActionSend(const std::string &nodename="GiveActionSend") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
    
          AngTwoPi theirheading = thisAgent->giveTargetObject->getOrientation();
          fmat::Column<3> theirpos = thisAgent->giveTargetObject->getCentroid().getCoords();
                fmat::Column<3> gpos;
    #if defined(TGT_IS_CALLIOPE2)
                gpos = kine->linkToBase(GripperFrameOffset).translation();
    #endif
          fmat::Column<3> ourpos = theAgent->getCentroid().getCoords();
    
                fmat::Column<3> poffset = ourpos + gpos - theirpos;
    
          AngSignPi target = atan2(poffset[1], poffset[0]);
          AngSignPi offset = target - (AngSignPi)theirheading;
    
          KoduGiveEvent evt;
          evt.setDestination(thisAgent->giveTargetObject->getHostAddr());
          evt.setTurnAndObject(offset, thisAgent->gripperObject->getName());
          erouter->postEvent(evt);
    
          thisAgent->manipulationComplete();
          thisAgent->setWantsToGiveObjectFlag(false);
          thisAgent->setIsExecutingMotionActionFlag(false);
          postStateCompletion();
      }
    };

    virtual void setup() {
     GiveActionStart *give = new GiveActionStart("give");
     addNode(give);

     GiveActionSend *send = new GiveActionSend("send");
     addNode(send);

     PositionBody *positionbody1 = new PositionBody("positionbody1");
     addNode(positionbody1);

     Drop *drop1 = new Drop("drop1");
     addNode(drop1);

     Backup *backup1 = new Backup("backup1");
     addNode(backup1);

     PostMachineCompletion *postmachinecompletion7 = new PostMachineCompletion("postmachinecompletion7");
     addNode(postmachinecompletion7);

     startnode = give;

     give->addTransition(new CompletionTrans("completiontrans55",positionbody1));
     positionbody1->addTransition(new CompletionTrans("completiontrans56",drop1));
     drop1->addTransition(new CompletionTrans("completiontrans57",backup1));
     backup1->addTransition(new CompletionTrans("completiontrans58",send));
     send->addTransition(new CompletionTrans("completiontrans59",postmachinecompletion7));
    }
  };

  // end of GiveActionRunner

  // ReceiveActionRunner

  class ReceiveActionRunner : public StateNode {
   public:
    ReceiveActionRunner(const std::string &nodename="ReceiveActionRunner") : StateNode(nodename) {}

    class ReceiveActionTurnAndGet : public VisualRoutinesStateNode {
     public:
      ReceiveActionTurnAndGet(const std::string &nodename="ReceiveActionTurnAndGet") : VisualRoutinesStateNode(nodename) {}
      class Rotate : public WalkNode {
       public:
        Rotate(const std::string &nodename="Rotate") : WalkNode(nodename) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              req = WalkRequest::displacement(0, 0, thisAgent->giveAngleToTurn);
        }
      };
  
      class LookDown : public HeadPointerNode {
       public:
        LookDown(const std::string &nodename="LookDown") : HeadPointerNode(nodename) {}
        virtual void doStart() {
              getMC()->lookAtPoint(400, 0, 0, 0);
        }
      };
  
      class FindObject : public MapBuilderNode {
       public:
        FindObject(const std::string &nodename="FindObject") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
        virtual void doStart() {
              mapreq.immediateRequest = true;
              mapreq.addObjectColor(cylinderDataType, "red");
              mapreq.setAprilTagFamily();
        }
      };
  
      class Grab : public VisualRoutinesStateNode {
       public:
        Grab(const std::string &nodename="Grab") : VisualRoutinesStateNode(nodename) {}
        virtual void doStart() {
          GrabActionRunner* &grabActRef = getAncestor<KoduInterpreter>()->grabActRef;
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
              Shape<CylinderData> object;
              NEW_SHAPEVEC(apples, CylinderData, select_type<CylinderData>(localShS));
              if(apples.size() == 0) {
                NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(localShS));
                if(tags.size() == 0) {
                  postStateFailure();
                  return;
                }
                Point center = tags[0]->getCentroid();
                          for (unsigned int i = 1; i < tags.size(); i++) {
                              if (tags[i]->getCentroid().xyNorm() < center.xyNorm()) {
                                  center = tags[i]->getCentroid();
                              }
                          }
                object = Shape<CylinderData>(new CylinderData(localShS, Point(center.coordX(), center.coordY(), 0)));
              } else {
                          int closest = 0;
                          Point center = apples[0]->getCentroid();
                          for (unsigned int i = 1; i < apples.size(); i++) {
                              if (apples[i]->getCentroid().xyNorm() < center.xyNorm()) {
                                  center = apples[i]->getCentroid();
                                  closest = i;
                              }
                          }
                object = apples[closest];
              }
                      std::cout << "CENTER: " << object->getCentroid() << std::endl;
              thisAgent->gripperObject = object;
              grabActRef->start();
              postStateCompletion();
        }
      };
  
      virtual void setup() {
       Rotate *rotate2 = new Rotate("rotate2");
       addNode(rotate2);

       LookDown *lookdown1 = new LookDown("lookdown1");
       addNode(lookdown1);

       StateNode *statenode3 = new StateNode("statenode3");
       addNode(statenode3);

       FindObject *findobject1 = new FindObject("findobject1");
       addNode(findobject1);

       PostMachineCompletion *postmachinecompletion8 = new PostMachineCompletion("postmachinecompletion8");
       addNode(postmachinecompletion8);

       Grab *grab = new Grab("grab");
       addNode(grab);

       SpeechNode *speechnode14 = new SpeechNode("speechnode14","failed to see given object");
       addNode(speechnode14);

       PostMachineFailure *postmachinefailure15 = new PostMachineFailure("postmachinefailure15");
       addNode(postmachinefailure15);

       startnode = rotate2;

       rotate2->addTransition(new CompletionTrans("completiontrans60",lookdown1));
       lookdown1->addTransition(new CompletionTrans("completiontrans61",statenode3));
       statenode3->addTransition(new TimeOutTrans("timeouttrans2",findobject1,500));
       findobject1->addTransition(new CompletionTrans("completiontrans62",grab));
       grab->addTransition(new CompletionTrans("completiontrans63",postmachinecompletion8));
       grab->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans51",speechnode14,StateNode::failureSignal));
       speechnode14->addTransition(new NullTrans("nulltrans1",postmachinefailure15));
      }
    };

    class ReceiveActionStart : public StateNode {
     public:
      ReceiveActionStart(const std::string &nodename="ReceiveActionStart") : StateNode(nodename) {}
        void doStart() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                  std::cout << "Sending give ping-back to " << thisAgent->agentReceivingFrom << std::endl;
                  erouter->addTimer(this, 0, 200, true);
          KoduGiveEvent evt;
          evt.setDestination(thisAgent->agentReceivingFrom);
          erouter->postEvent(evt);
        }
  
        void doEvent() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                  if (event->getGeneratorID() == EventBase::timerEGID && thisAgent->gotGiveInformation) {
                      erouter->removeTimer(this);
                      postStateCompletion();
                  }
        }
    };

    class FindObject : public MapBuilderNode {
     public:
      FindObject(const std::string &nodename="FindObject") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
      virtual void doStart() {
          std::cout << "[" << getName() << "]: looking for objects.\n";
          mapreq.immediateRequest = true;
          mapreq.addObjectColor(cylinderDataType, "red");
          mapreq.setAprilTagFamily();
      }
    };

    class ReceiveActionEnd : public StateNode {
     public:
      ReceiveActionEnd(const std::string &nodename="ReceiveActionEnd") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          thisAgent->setIsReceiving(false);
          postStateCompletion();
      }
    };

    virtual void setup() {
     ReceiveActionStart *recv = new ReceiveActionStart("recv");
     addNode(recv);

     ReceiveActionTurnAndGet *turnandget = new ReceiveActionTurnAndGet("turnandget");
     addNode(turnandget);

     ReceiveActionEnd *receiveactionend1 = new ReceiveActionEnd("receiveactionend1");
     addNode(receiveactionend1);

     PostMachineCompletion *postmachinecompletion9 = new PostMachineCompletion("postmachinecompletion9");
     addNode(postmachinecompletion9);

     ReceiveActionEnd *receiveactionend2 = new ReceiveActionEnd("receiveactionend2");
     addNode(receiveactionend2);

     PostMachineFailure *postmachinefailure16 = new PostMachineFailure("postmachinefailure16");
     addNode(postmachinefailure16);

     startnode = recv;

     recv->addTransition(new CompletionTrans("completiontrans64",turnandget));
     turnandget->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans52",receiveactionend1,StateNode::successSignal));
     receiveactionend1->addTransition(new CompletionTrans("completiontrans65",postmachinecompletion9));
     turnandget->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans53",receiveactionend2,StateNode::failureSignal));
     receiveactionend2->addTransition(new CompletionTrans("completiontrans66",postmachinefailure16));
    }

  };

  // end of ReceiveActionRunner


  // MotionActionRunner
    
  class MotionActionRunner : public StateNode {
   public:
    MotionActionRunner(const std::string &nodename="MotionActionRunner") : StateNode(nodename), cmd() {}
        
    virtual void stop() {
    Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
      // set is walking flag to false, and invalidate the current motion command
      thisAgent->motionComplete();
      cmd = Kodu::MotionCommand();
      // abort the pilot
      VRmixin::pilot->pilotAbort();
      StateNode::stop();
    }

    class MotionStart : public StateNode {
     public:
      MotionStart(const std::string &nodename="MotionStart") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
        Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
    
          //std::cout << "[" << getName() << "]: starting up motion action runner\n";
          //thisAgent->agentIsWalking = true;
          thisAgent->signalMotionActionStart();
          cmd = thisAgent->currMotionCmd;
          postStateCompletion();
      }
    };

    class ExecuteMotionAction : public StateNode {
     public:
      ExecuteMotionAction(const std::string &nodename="ExecuteMotionAction") : StateNode(nodename), kMinDistForGoToShape(250.0f), targetShapeTagId(-1) {}
  
        enum MotionTransType {
          MTT_SIMPLE_MOTION = 0,
          MTT_GO_TO_SHAPE,
          MTT_DRIVER_MOTION
        };
  
      class CheckMotionType : public StateNode {
       public:
        CheckMotionType(const std::string &nodename="CheckMotionType") : StateNode(nodename) {}
        virtual void doStart() {
          Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
          int &targetShapeTagId = getAncestor<ExecuteMotionAction>()->targetShapeTagId;
          
              // *********** temp fix
              //std::cout << "[" << getName() << "]: checking motion type\n";
          
          
              // check the type of motion the robot wants to perform
              // if the motion command's target shape is valid, then it is a move towards action
              
              if ( cmd.isDriven() )
                postStateSignal<MotionTransType>(MTT_DRIVER_MOTION);
              else if (cmd.targetObjectIsValid()) {
                //std::cout << "looks like a move towards action.\n";
          const float &kMinDistForGoToShape = getAncestor<ExecuteMotionAction>()->kMinDistForGoToShape;
                if ( Kodu::distanceInBetweenAgentAndObject(cmd.getTargetObject()) > kMinDistForGoToShape ) {
                  //std::cout << "agent is more than " << kMinDistForGoToShape
                  //    << "mm away from shape.\n";
                  std::cout << "shape was bumped before? ";
                  int shapeId = cmd.getTargetObject().getId();
                  if (theWorld->shapeTagPairExists(shapeId)) {
                    targetShapeTagId = theWorld->getTagIdForShape(shapeId);
                    std::cout << "yes! id = " << targetShapeTagId << std::endl;
                  } else {
                    std::cout << "no.\n";
                  }
                  postStateSignal<MotionTransType>(MTT_GO_TO_SHAPE);
                } else {
                  //std::cout << "agent is too close to execute a goToShape request.\n";
                  postStateSignal<MotionTransType>(MTT_SIMPLE_MOTION);
                }
                // *********** temp fix
                // this task should be created anytime the robot is executing a motion action
                // except when doing stationary turns...
                //std::cout << "creating walk progress perceptual task\n";
                thisAgent->ptasks.push(new Kodu::VisualNavErrMonTask(cmd.getTargetObject()));
                // ***********
              }
              // else it might be something else
              else {
                //std::cout << "looks like something that needs simple motion.\n";
                postStateSignal<MotionTransType>(MTT_SIMPLE_MOTION);
              }
              // ***********
        }
      };
  
      class ExecuteGoToShapeRequest : public PilotNode {
       public:
        ExecuteGoToShapeRequest(const std::string &nodename="ExecuteGoToShapeRequest") : PilotNode(nodename,PilotTypes::goToShape) {}
        virtual void doStart() {
          Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
          const float &kMinDistForGoToShape = getAncestor<ExecuteMotionAction>()->kMinDistForGoToShape;
              // construct the pilot request
              std::cout << "[" << getName() << "]: executing goToShape motion command\n";
              pilotreq = PilotRequest(PilotTypes::goToShape);
              ShapeRoot target = cmd.getTargetObject();
              target->setObstacle(false);
              pilotreq.targetShape = target;
              // prevent the agent from moving backwards if there is something in the gripper
              if (thisAgent->isHoldingAnObject()) {
                //std::cout << "agent is holding an object... disallowing backward motion\n";
                pilotreq.allowBackwardMotion = false;
              }
              // calculate the offset
              float centroidDist = Kodu::distanceFromAgentToObject(target);
              float spatialDist = Kodu::distanceInBetweenAgentAndObject(target);
              float xOffset = centroidDist - spatialDist + kMinDistForGoToShape;
              pilotreq.baseOffset = fmat::pack(xOffset, 0, 0);
              cout << "centroidDist=" << centroidDist << " spatialDist=" << spatialDist
                   << " baseOffset=" << pilotreq.baseOffset << endl;
              // ignore any collisions
              pilotreq.collisionAction = collisionIgnore;
        }
      };
  
      class ExecuteSimpleMotion : public StateNode {
       public:
        ExecuteSimpleMotion(const std::string &nodename="ExecuteSimpleMotion") : StateNode(nodename) {}
    
        class CheckForValidTarget : public StateNode {
         public:
          CheckForValidTarget(const std::string &nodename="CheckForValidTarget") : StateNode(nodename) {}
          virtual void doStart() {
            Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
                            
                  // check if the robot needs to do a final approach
                  std::cout << "[" << getName() << "]: robot needs to do final approach? ";
                  if (cmd.targetObjectIsValid())
                    std::cout << "yes!\n";
                  else
                    std::cout << "no!\n";
                  postStateSignal<bool>(cmd.targetObjectIsValid());
          }
        };
    
        class FindMotionTarget : public MapBuilderNode {
         public:
          FindMotionTarget(const std::string &nodename="FindMotionTarget") : MapBuilderNode(nodename,MapBuilderRequest::localMap) {}
          virtual void doStart() {
            Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
            int &targetShapeTagId = getAncestor<ExecuteMotionAction>()->targetShapeTagId;
                            
                  std::cout << "[" << getName() << "]: searching for target object\n";
                  // construct the mapbuilder request to search for the target
                  mapreq.clearLocal = true;
                  const ShapeRoot& kTarget = cmd.getTargetObject();
                  // search for a shape with:
                  // --- the same color and type
                  mapreq.addObjectColor(kTarget->getType(), kTarget->getColor());
                  // --- the approx. coordinates
                  mapreq.searchArea = kTarget->getCentroidPtShape();
                  // --- no centroid beyond this distance
                  float centroidDist = Kodu::distanceFromAgentToObject(kTarget);
                  float fudgeFactor = Kodu::objectRadius(kTarget) * 1.5f;
                  mapreq.maxDist = centroidDist + fudgeFactor;
                  // if the target was bumped before look for april tags as well
                  if (targetShapeTagId > 0) {
                    //std::cout << "searching for april tags too (shape was bumped before).\n";
                    mapreq.setAprilTagFamily();
                  }
                  mapreq.pursueShapes = false;
          }
        };
    
        class CreateFinalApproachCommand : public StateNode {
         public:
          CreateFinalApproachCommand(const std::string &nodename="CreateFinalApproachCommand") : StateNode(nodename) {}
          virtual void doStart() {
            Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
            Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
            int &targetShapeTagId = getAncestor<ExecuteMotionAction>()->targetShapeTagId;
        
                  // std::cout << "[" << getName() << "]: creating final approach motion command.\n";
                  // check if the object was bumped before, and if that tag was seen in the 
                  // recently performed mapbuilder request
                  ShapeRoot targetShape = find_if(localShS, Kodu::HasAprilTagID(targetShapeTagId));
                  // if the shaperoot reference is valid, then the tag was seen
                  if (targetShape.isValid()) {
                    //std::cout << "this object was bumped before! using the april tag as target\n";
                  }
                  // else, the object was either not bumped before or the tag was not seen
                  else {
                    //  std::cout << "either the tag was not seen, or the object was never bumped; ";
                    const ShapeRoot& kTarget = cmd.getTargetObject();
                    ShapeRoot lclTarget = VRmixin::mapBuilder->importWorldToLocal(kTarget);
                    targetShape = find_if(localShS, Kodu::IsMatchForTargetObject(lclTarget));
                    if (!targetShape.isValid()) {
                      std::cout << "could not find a match! THIS IS A SERIOUS ERROR!!!\n";
                      postStateFailure();
                    }
                  }
                  // get the bearing to the tag
                  float angleToTurn = Kodu::bearingFromAgentToObject(targetShape);
                  // get the distance to travel:
                  // --- get the radius of the object
                  float shapeRadius = Kodu::objectRadius(cmd.getTargetObject());
                  // --- create a fake cylinder in the world (the shape will be enclosed in this
                  //     cylinder)
                  NEW_SHAPE(fakeCyl, CylinderData, new CylinderData(localShS,
                                                                    targetShape->getCentroid(), 0.0f, shapeRadius));
                  // --- calculate the distance in between the agent and this enclosed shape
                  float distanceToTravel = Kodu::distanceInBetweenAgentAndObject(fakeCyl);
                  // ******* temp fix
                  if (thisAgent->isHoldingAnObject())
                    distanceToTravel += 45.0f;
                  // *******
                  // create a new motion command
                  cmd = Kodu::MotionCommand(distanceToTravel, angleToTurn, 0.0f, 0.0f);
                  //std::cout << "angle = " << angleToTurn << "rads (= "
                  //    << rad2deg(angleToTurn) << "degs); distance = "
                  //    << distanceToTravel << "mm\n";
                  // signal process was successful!
                  postStateSuccess();
          }
        };
    
        class ExecuteSimpleTurn : public PilotNode {
         public:
          ExecuteSimpleTurn(const std::string &nodename="ExecuteSimpleTurn") : PilotNode(nodename,PilotTypes::walk) {}
          virtual void doStart() {
            Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
                  // construct the pilot request                    $reference MotionActionRunner::cmd;
        
                  //std::cout << "[" << getName() << "]: executing simple turn command; da = ";
                  pilotreq = PilotRequest(PilotTypes::walk);
                  pilotreq.da = cmd.getTurningAngle();
                  //std::cout << pilotreq.da << "rads (= " << rad2deg(pilotreq.da) << "degs)\n";
                  pilotreq.collisionAction = collisionIgnore;
          }
        };
    
        class ExecuteSimpleWalk : public PilotNode {
         public:
          ExecuteSimpleWalk(const std::string &nodename="ExecuteSimpleWalk") : PilotNode(nodename,PilotTypes::walk) {}
          virtual void doStart() {
            Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
                  // construct the pilot request
                  //std::cout << "[" << getName() << "]: executing simple forward walk command; dx = ";
                  pilotreq = PilotRequest(PilotTypes::walk);
                  pilotreq.dx = cmd.getDistanceToTravel();
                  //std::cout << pilotreq.dx << "mm\n";
                  pilotreq.collisionAction = collisionIgnore;
          }
        };
    
        virtual void setup() {
         CheckForValidTarget *checkForValidTarget = new CheckForValidTarget("checkForValidTarget");
         addNode(checkForValidTarget);

         FindMotionTarget *findMotionTarget = new FindMotionTarget("findMotionTarget");
         addNode(findMotionTarget);

         CreateFinalApproachCommand *createFnlApprCmd = new CreateFinalApproachCommand("createFnlApprCmd");
         addNode(createFnlApprCmd);

         ExecuteSimpleTurn *execTurn = new ExecuteSimpleTurn("execTurn");
         addNode(execTurn);

         ExecuteSimpleWalk *execWalk = new ExecuteSimpleWalk("execWalk");
         addNode(execWalk);

         SpeechNode *finalApprFail = new SpeechNode("finalApprFail","error nothing available for final approach");
         addNode(finalApprFail);

         PostMachineFailure *postmachinefailure17 = new PostMachineFailure("postmachinefailure17");
         addNode(postmachinefailure17);

         PostMachineCompletion *postmachinecompletion10 = new PostMachineCompletion("postmachinecompletion10");
         addNode(postmachinecompletion10);

         startnode = checkForValidTarget;

         checkForValidTarget->addTransition(new SignalTrans<bool>("signaltrans54",findMotionTarget,true));
         findMotionTarget->addTransition(new CompletionTrans("completiontrans67",createFnlApprCmd));
         checkForValidTarget->addTransition(new SignalTrans<bool>("signaltrans55",execTurn,false));
         createFnlApprCmd->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans56",execTurn,StateNode::successSignal));
         createFnlApprCmd->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans57",finalApprFail,StateNode::failureSignal));
         finalApprFail->addTransition(new CompletionTrans("completiontrans68",postmachinefailure17));
         execTurn->addTransition(new CompletionTrans("completiontrans69",execWalk));
         execWalk->addTransition(new CompletionTrans("completiontrans70",postmachinecompletion10));
        }
      };
  
      class ExecuteDriverMotion : public PilotNode {
       public:
        ExecuteDriverMotion(const std::string &nodename="ExecuteDriverMotion") : PilotNode(nodename,PilotTypes::walk) {}
        virtual void doStart() {
              pilotreq = PilotRequest(PilotTypes::setVelocity);
        }
      };
  
      virtual void setup() {
       CheckMotionType *checkMotionType = new CheckMotionType("checkMotionType");
       addNode(checkMotionType);

       ExecuteGoToShapeRequest *execGoToShape = new ExecuteGoToShapeRequest("execGoToShape");
       addNode(execGoToShape);

       PostMachineSuccess *postmachinesuccess10 = new PostMachineSuccess("postmachinesuccess10");
       addNode(postmachinesuccess10);

       ExecuteSimpleMotion *execSimpleMotion = new ExecuteSimpleMotion("execSimpleMotion");
       addNode(execSimpleMotion);

       PostMachineCompletion *postmachinecompletion11 = new PostMachineCompletion("postmachinecompletion11");
       addNode(postmachinecompletion11);

       SpeechNode *speechnode15 = new SpeechNode("speechnode15","simple motion failed ");
       addNode(speechnode15);

       PostMachineFailure *postmachinefailure18 = new PostMachineFailure("postmachinefailure18");
       addNode(postmachinefailure18);

       ExecuteDriverMotion *execDriverMotion = new ExecuteDriverMotion("execDriverMotion");
       addNode(execDriverMotion);

       PostMachineFailure *postmachinefailure19 = new PostMachineFailure("postmachinefailure19");
       addNode(postmachinefailure19);

       PostMachineCompletion *postmachinecompletion12 = new PostMachineCompletion("postmachinecompletion12");
       addNode(postmachinecompletion12);

       startnode = checkMotionType;

       checkMotionType->addTransition(new SignalTrans<MotionTransType>("signaltrans58",execGoToShape,MTT_GO_TO_SHAPE));
       checkMotionType->addTransition(new SignalTrans<MotionTransType>("signaltrans59",execSimpleMotion,MTT_SIMPLE_MOTION));
       checkMotionType->addTransition(new SignalTrans<MotionTransType>("signaltrans60",execDriverMotion,MTT_DRIVER_MOTION));
       execGoToShape->addTransition(new PilotTrans("pilottrans7",postmachinesuccess10,PilotTypes::abort));
       execGoToShape->addTransition(new PilotTrans("pilottrans8",execSimpleMotion));
       execSimpleMotion->addTransition(new CompletionTrans("completiontrans71",postmachinecompletion11));
       execSimpleMotion->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans61",speechnode15,StateNode::failureSignal));
       speechnode15->addTransition(new CompletionTrans("completiontrans72",postmachinefailure18));
       execDriverMotion->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans62",postmachinefailure19,StateNode::failureSignal));
       execDriverMotion->addTransition(new CompletionTrans("completiontrans73",postmachinecompletion12));
      }
      const float kMinDistForGoToShape;   // from $provide
      int targetShapeTagId;   // from $provide
    };

    class MotionEnd : public StateNode {
     public:
      MotionEnd(const std::string &nodename="MotionEnd") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
        Kodu::MotionCommand &cmd = getAncestor<MotionActionRunner>()->cmd;
                
          //std::cout << "[" << getName() << "]: Motion Action Runner complete.\n";
          if (thisAgent->currMotionCmd.targetObjectIsValid()) {
            //std::cout << "making target shape an obstacle (again).\n";
            ShapeRoot targetShape = thisAgent->currMotionCmd.getTargetObject();
            targetShape->setObstacle();
          }
          cmd = Kodu::MotionCommand();
          thisAgent->motionComplete();
          postStateCompletion();
      }
    };
           

    virtual void setup() {
     MotionStart *mStart = new MotionStart("mStart");
     addNode(mStart);

     ExecuteMotionAction *execMotion = new ExecuteMotionAction("execMotion");
     addNode(execMotion);

     MotionEnd *mEnd = new MotionEnd("mEnd");
     addNode(mEnd);

     SpeechNode *speechnode16 = new SpeechNode("speechnode16","error during motion");
     addNode(speechnode16);

     PostMachineCompletion *postmachinecompletion13 = new PostMachineCompletion("postmachinecompletion13");
     addNode(postmachinecompletion13);

     startnode = mStart;

     mStart->addTransition(new CompletionTrans("completiontrans74",execMotion));
     execMotion->addTransition(new CompletionTrans("completiontrans75",mEnd));
     execMotion->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans63",speechnode16,StateNode::failureSignal));
     speechnode16->addTransition(new CompletionTrans("completiontrans76",mEnd));
     mEnd->addTransition(new CompletionTrans("completiontrans77",postmachinecompletion13));
    }
    Kodu::MotionCommand cmd;   // from $provide
  };

  // end of MotionActionRunner

  // PageSwitchActionRunner

  class PageSwitchActionRunner : public StateNode {
   public:
    PageSwitchActionRunner(const std::string &nodename="PageSwitchActionRunner") : StateNode(nodename), waitCount(0) {}

    class PageSwitchStart : public StateNode {
     public:
      PageSwitchStart(const std::string &nodename="PageSwitchStart") : StateNode(nodename) {}
      virtual void doStart() {
          //std::cout << "[" << getName() << "]\n";
                
        PerceptualMultiplexor* &multiplexorRef = getAncestor<KoduInterpreter>()->multiplexorRef;
        MotionActionRunner* &motionActRef = getAncestor<KoduInterpreter>()->motionActRef;
        KoduConditionEvaluator* &evaluatorRef = getAncestor<KoduInterpreter>()->evaluatorRef;
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
        bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
    
          //PRINT_ATTRS("Multiplexor is active?", multiplexorRef->isActive());
          //PRINT_ATTRS("Multiplexor is in recovery?", multiplexorRef->isInRecovery());
          //PRINT_ATTRS("Evaluator is active?", evaluatorRef->isActive());
          //PRINT_ATTRS("Agent is walking?", VRmixin::isWalkingFlag);
          //PRINT_ATTRS("Attempting manipulation?", thisAgent->isExecutingManipAction());
    
          if (multiplexorRef->isActive() && !multiplexorRef->isInRecovery()) {
            //std::cout << "stopping multiplexor (it is active, but not in recovery mode).\n";
            multiplexorRef->stop();
          }
    
          if (needToHaltExecution == false) {
            //std::cout << "requesting evaluator to stop... (needToHaltExecution = true)\n";
            needToHaltExecution = true;
          }
    
          if (evaluatorRef->isActive()) {
            //std::cout << "stopping evaluator.\n";
            evaluatorRef->stop();
          }
    
          if (thisAgent->isExecutingMotionAction()) {
            //std::cout << "aborting the current motion command.\n";
            motionActRef->stop();
          }
    
          if (!evaluatorRef->isActive() && !multiplexorRef->isActive()
              && VRmixin::isWalkingFlag == false && !thisAgent->isExecutingManipAction())
            {
        unsigned int &waitCount = getAncestor<PageSwitchActionRunner>()->waitCount;
              if ((++waitCount) == 2) {
                waitCount = 0;
                //std::cout << "proceeding...\n";
                postStateCompletion();
                return;
              }
            }
          //std::cout << "waiting...\n";
      }
    };

    class SwitchToNewPage : public StateNode {
     public:
      SwitchToNewPage(const std::string &nodename="SwitchToNewPage") : StateNode(nodename) {}
      virtual void doStart() {
          std::cout << "[" << getName() << "]: executing the page switch action.\n";
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
          int newPageIndex = thisAgent->newReqdPage - 1;          
          if (thisAgent->pages[newPageIndex] == NULL) {
            std::cerr << "!!! A NULL page (index #" << newPageIndex << ") was requested.\n"
                      << "Ignoring action.\n";
            postStateFailure();
            return;
          }
          // switch to the new page
          std::cout << "Switching to page " << thisAgent->newReqdPage
                    << " (index #" << newPageIndex << ")\n";
          Kodu::KoduPage* nextPage = thisAgent->pages[newPageIndex];
    
          // reinitialize all the primitives on the new page
          const int kRuleCount = nextPage->getRuleCount();
          for (int i = 0; i < kRuleCount; i++) {
            nextPage->getRuleInPos(i)->reinitializePrimitives();
          }
          thisAgent->currPageIndex = newPageIndex;
          nextPage = NULL;
      
          // if the ptasks queue is not empty, remove all tasks except gripper monitor and localization
          if (!thisAgent->ptasks.empty()) {
            std::size_t numbOfTasks = thisAgent->ptasks.size();
            //std::cout << "ptasks queue has " << numbOfTasks << " task(s)...\n";
            Kodu::PerceptualTaskType_t type;
            while (numbOfTasks > 0) {
              type = thisAgent->ptasks.front()->getType();
              if (type == Kodu::PT_GRIPPER_VIS_MONITOR || type == Kodu::PT_VIS_LOCALIZATION) {
                std::cout << "\tadding task #" << thisAgent->ptasks.front()->getTaskId()
                          << " to back of queue.\n";
                thisAgent->ptasks.push(thisAgent->ptasks.front());
              } else {
                std::cout << "\tdeleting task #" << thisAgent->ptasks.front()->getTaskId()
                          << " from the queue.\n";
                GeneralFncs::deletePtr(thisAgent->ptasks.front());
              }
              thisAgent->ptasks.pop();
              numbOfTasks--;
            }
          } else {
            //std::cout << "the perceptual tasks queue is empty... continuing.\n";
          }
          thisAgent->newReqdPage = 0;
          postStateCompletion();
      }
    };

    class PageSwitchEnd : public StateNode {
     public:
      PageSwitchEnd(const std::string &nodename="PageSwitchEnd") : StateNode(nodename) {}
      virtual void doStart() {
        KoduConditionEvaluator* &evaluatorRef = getAncestor<KoduInterpreter>()->evaluatorRef;
        bool &needToHaltExecution = getAncestor<KoduInterpreter>()->needToHaltExecution;
                
          //std::cout << "[" << getName() << "]: restarting evaluaStateNode";              
             
          needToHaltExecution = false;
          evaluatorRef->start();
    
          //std::cout << "Page Switch action completion.\n";
          postStateCompletion();
      }
    };

    virtual void setup() {
     PageSwitchStart *psStart = new PageSwitchStart("psStart");
     addNode(psStart);

     SwitchToNewPage *execSwitch = new SwitchToNewPage("execSwitch");
     addNode(execSwitch);

     PageSwitchEnd *psEnd = new PageSwitchEnd("psEnd");
     addNode(psEnd);

     PostMachineFailure *postmachinefailure20 = new PostMachineFailure("postmachinefailure20");
     addNode(postmachinefailure20);

     PostMachineCompletion *postmachinecompletion14 = new PostMachineCompletion("postmachinecompletion14");
     addNode(postmachinecompletion14);

     startnode = psStart;

     psStart->addTransition(new CompletionTrans("completiontrans78",execSwitch));
     psStart->addTransition(new TimeOutTrans("timeouttrans3",psStart,500));
     execSwitch->addTransition(new CompletionTrans("completiontrans79",psEnd));
     execSwitch->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans64",postmachinefailure20,StateNode::failureSignal));
     psEnd->addTransition(new CompletionTrans("completiontrans80",postmachinecompletion14));
    }
    unsigned int waitCount;   // from $provide
  };

  // end of PageSwitchActionRunner

  // PlayActionRunner

  class PlayActionRunner : public StateNode {
   public:
    PlayActionRunner(const std::string &nodename="PlayActionRunner") : StateNode(nodename) {}

    class PlayStart : public StateNode {
     public:
      PlayStart(const std::string &nodename="PlayStart") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                                        std::string wavfile = thisAgent->playQueue.front();
                                        thisAgent->playQueue.pop();
                                        postStateSignal<std::string>(wavfile);
      }
    };

    class PlayEnd : public StateNode {
     public:
      PlayEnd(const std::string &nodename="PlayEnd") : StateNode(nodename) {}
      virtual void doStart() {
        Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
                                        if (thisAgent->hasSoundsToPlay()) {
                                          postStateCompletion();
                                        } else {
                                          //std::cout << "All play actions complete.\n";
                                          postParentCompletion();
                                        }
      }
    };

    virtual void setup() {
     PlayStart *pStart = new PlayStart("pStart");
     addNode(pStart);

     PlayEnd *pEnd = new PlayEnd("pEnd");
     addNode(pEnd);

     SoundNode *soundnode1 = new SoundNode("soundnode1");
     addNode(soundnode1);

     startnode = pStart;

     pStart->addTransition(new SignalTrans<std::string>("signaltrans65",soundnode1));
     soundnode1->addTransition(new TimeOutTrans("timeouttrans4",pEnd,50));
     pEnd->addTransition(new CompletionTrans("completiontrans81",pStart));
    }
  };

  class PlayActuator : public StateNode {
   public:
    PlayActuator(const std::string &nodename="PlayActuator") : StateNode(nodename) {}
    virtual void doStart() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
      if (thisAgent->hasSoundsToPlay()) {
        //std::cout << "All Play actions complete.\n";
        postStateCompletion();
        return;
      }
      std::string wavfile = thisAgent->playQueue.front();
      thisAgent->playQueue.pop();
      postStateSignal<std::string>(wavfile);
    }
  };
    
  // end of Play Actuator

  // Say Actuator

  class SayActuator : public StateNode {
   public:
    SayActuator(const std::string &nodename="SayActuator") : StateNode(nodename) {}
    virtual void doStart() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
      postStateSignal<std::string>(thisAgent->stringToSpeak);
    }
  };

  class CompleteSayActuator : public StateNode {
   public:
    CompleteSayActuator(const std::string &nodename="CompleteSayActuator") : StateNode(nodename) {}
    virtual void doStart() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
  
      //Post event to all other robots
      erouter->postEvent(KoduSayEvent(thisAgent->stringToSpeak));
  
      thisAgent->stringToSpeak.clear();
      std::cout << "Say action complete.\n";
      postStateCompletion();
    }
  };

  // end of Speech Actuator

  // Score Actuator

  class ScoreActuator : public StateNode {
   public:
    ScoreActuator(const std::string &nodename="ScoreActuator") : StateNode(nodename) {}
    virtual void doStart() {
      Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
      Kodu::KoduWorld* &theWorld = getAncestor<KoduInterpreter>()->theWorld;
      theWorld->applyGlobalScoreChanges(thisAgent->scoreQueue);
      std::cout << "All Score actions complete.\n";
      postStateCompletion();
    }
  };

  class NotificationMonitor : public StateNode {
   public:
    NotificationMonitor(const std::string &nodename="NotificationMonitor") : StateNode(nodename) {}
    virtual void doStart(){
      erouter->addListener(this, EventBase::buttonEGID);
    }

    virtual void doEvent() {
    Kodu::KoduAgent* &thisAgent = getAncestor<KoduInterpreter>()->thisAgent;
    Kodu::KoduState* &kodustate = getAncestor<KoduInterpreter>()->kodustate;

      if ( event->getGeneratorID() == EventBase::buttonEGID
           && IS_GAMEPAD_SID(event->getSourceID()) ) {
        kodustate->update(event->getGeneratorID(), 
                          event->getSourceID(),
                          event->getMagnitude());

        if ( thisAgent->currMotionCmd.isDriven() ) {
          float xvel = kodustate->joysticky;
          float avel = kodustate->joystickx;
          float speed = 125.0;
          float angvel = M_PI/8.0;
          pilot->changeVelocity(speed*xvel, 0.0, angvel*avel);
        }

      }
    }
     
  };

  // end of Score ActuatorStateNode               

    
  // ==================================================================================== //
    
  virtual void setup() {
   InitializeAgent *initAgent = new InitializeAgent("initAgent");
   addNode(initAgent);

   WalkMonitor *walkMon = new WalkMonitor("walkMon");
   addNode(walkMon);

   PerceptualMultiplexor *mplex = new PerceptualMultiplexor("mplex");
   addNode(mplex);

   KoduConditionEvaluator *evaluator = new KoduConditionEvaluator("evaluator");
   addNode(evaluator);

   KoduActionRunner *runner = new KoduActionRunner("runner");
   addNode(runner);

   NotificationMonitor *notifier = new NotificationMonitor("notifier");
   addNode(notifier);

   StateNode *endState = new StateNode("endState");
   addNode(endState);

   DropActionRunner *dropAct = new DropActionRunner("dropAct");
   addNode(dropAct);

   KoduConditionEvaluator *koduconditionevaluator1 = new KoduConditionEvaluator("koduconditionevaluator1");
   addNode(koduconditionevaluator1);

   GrabActionRunner *grabAct = new GrabActionRunner("grabAct");
   addNode(grabAct);

   GiveActionRunner *giveAct = new GiveActionRunner("giveAct");
   addNode(giveAct);

   ReceiveActionRunner *recvAct = new ReceiveActionRunner("recvAct");
   addNode(recvAct);

   MotionActionRunner *motionAct = new MotionActionRunner("motionAct");
   addNode(motionAct);

   SayActuator *sayAct = new SayActuator("sayAct");
   addNode(sayAct);

   CompleteSayActuator *compSayAct = new CompleteSayActuator("compSayAct");
   addNode(compSayAct);

   SpeechNode *speechnode17 = new SpeechNode("speechnode17");
   addNode(speechnode17);

   PlayActuator *playAct = new PlayActuator("playAct");
   addNode(playAct);

   SoundNode *soundnode2 = new SoundNode("soundnode2");
   addNode(soundnode2);

   ScoreActuator *scoreAct = new ScoreActuator("scoreAct");
   addNode(scoreAct);

   PageSwitchActionRunner *pageSwitchAct = new PageSwitchActionRunner("pageSwitchAct");
   addNode(pageSwitchAct);

   SpeechNode *speechnode18 = new SpeechNode("speechnode18","unhandled switch to a null page");
   addNode(speechnode18);

   startnode = initAgent;

   CompletionTrans *completiontrans82 = new CompletionTrans("completiontrans82",evaluator);
   completiontrans82->addDestination(walkMon);
   completiontrans82->addDestination(notifier);
   initAgent->addTransition(completiontrans82);

   walkMon->addTransition(new TimeOutTrans("timeouttrans5",walkMon,500));
   evaluator->addTransition(new CompletionTrans("completiontrans83",runner));
   evaluator->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans66",endState,StateNode::failureSignal));
   runner->addTransition(new TimeOutTrans("timeouttrans6",evaluator,150));
   mplex->addTransition(new CompletionTrans("completiontrans84",endState));
   dropAct->addTransition(new CompletionTrans("completiontrans85",endState));
   grabAct->addTransition(new CompletionTrans("completiontrans86",endState));
   giveAct->addTransition(new CompletionTrans("completiontrans87",endState));
   recvAct->addTransition(new CompletionTrans("completiontrans88",endState));
   motionAct->addTransition(new CompletionTrans("completiontrans89",endState));
   sayAct->addTransition(new SignalTrans<std::string>("signaltrans67",speechnode17));
   speechnode17->addTransition(new CompletionTrans("completiontrans90",compSayAct));
   compSayAct->addTransition(new CompletionTrans("completiontrans91",endState));
   playAct->addTransition(new SignalTrans<std::string>("signaltrans68",soundnode2));
   soundnode2->addTransition(new TimeOutTrans("timeouttrans7",playAct,25));
   playAct->addTransition(new CompletionTrans("completiontrans92",endState));
   scoreAct->addTransition(new CompletionTrans("completiontrans93",endState));
   pageSwitchAct->addTransition(new CompletionTrans("completiontrans94",endState));
   pageSwitchAct->addTransition(new SignalTrans<StateNode::SuccessOrFailure>("signaltrans69",speechnode18,StateNode::failureSignal));
   speechnode18->addTransition(new CompletionTrans("completiontrans95",endState));
    // 
    multiplexorRef = mplex;
    evaluatorRef = evaluator;
    runnerRef = runner;
        
    // action runner references
    dropActRef = dropAct;
    grabActRef = grabAct;
    giveActRef = giveAct;
    recvActRef = recvAct;
    motionActRef = motionAct;


    pageSwitchActRef = pageSwitchAct;
    playActRef = playAct;
    sayActRef = sayAct;
    scoreActRef = scoreAct;
  }
    
 private:
  DISALLOW_COPY_ASSIGN(KoduInterpreter);
  Kodu::KoduWorld* theWorld;   // from $provide
  Kodu::KoduAgent* thisAgent;   // from $provide
  KoduDiscover* discover;   // from $provide
  DropActionRunner* dropActRef;   // from $provide
  GrabActionRunner* grabActRef;   // from $provide
  GiveActionRunner* giveActRef;   // from $provide
  ReceiveActionRunner* recvActRef;   // from $provide
  MotionActionRunner* motionActRef;   // from $provide
  PageSwitchActionRunner* pageSwitchActRef;   // from $provide
  PlayActuator* playActRef;   // from $provide
  SayActuator* sayActRef;   // from $provide
  ScoreActuator* scoreActRef;   // from $provide
  PerceptualMultiplexor* multiplexorRef;   // from $provide
  bool needToHaltExecution;   // from $provide
  KoduConditionEvaluator* evaluatorRef;   // from $provide
  KoduActionRunner* runnerRef;   // from $provide
  KoduConfig* koduconfig;   // from $provide
  KoduEventListener* elistener;   // from $provide
  Kodu::KoduState* kodustate;   // from $provide
};