#ifndef _finalProj_h_
#define _finalProj_h_

#include <istream>

#include "Sound/SoundManager.h"
#include "Behaviors/Nodes/MotionSequenceNode.h"
#include "Behaviors/Nodes/WalkNode.h"
#include "Behaviors/Transitions/CompletionTrans.h"
#include "Behaviors/Transitions/NullTrans.h"
#include "Behaviors/Transitions/SignalTrans.h"
#include "Behaviors/Transitions/TimeOutTrans.h"
#include "Behaviors/Transitions/EventTrans.h"
#include "Events/EventRouter.h"
#include "Events/DataEvent.h"	
#include "Events/LocomotionEvent.h"
#include "Motion/HeadPointerMC.h"
#include "Motion/WalkMC.h"
#include "Motion/MMAccessor.h"
#include "Shared/WorldState.h"
#include "DualCoding/DualCoding.h"
#include "DualCoding/VisualRoutinesStateNode.h"
#include "Motion/PostureMC.h"
#include "DualCoding/Pilot.h"
#include "DualCoding/PilotRequest.h"
#include "Shared/string_util.h"

using namespace DualCoding;
using namespace std;


enum TargetAction { approach, identify, notfound };

inline std::istream& operator>>(std::istream &s, TargetAction &x) {
  unsigned int v;
  s >> v;
  x = (TargetAction)v;
  return s;
}

enum Result { notFound, found };

inline std::istream& operator>>(std::istream &s, Result &x) {
  unsigned int v;
  s >> v;
  x = (Result)v;
  return s;
}

#ifndef _Types_
#define _Types_

enum CardType {rank, face};

inline std::istream& operator>>(std::istream &s, CardType &x) {
  unsigned int v;
  s >> v;
  x = (CardType)v;
  return s;
}
#endif
  
//****************LookAtCard****************
class LookAtCard : public VisualRoutinesStateNode {
  // head pointer
  MotionManager::MC_ID headpointer_id;
  
 public:
  LookAtCard() : VisualRoutinesStateNode("LookAtCard"), headpointer_id(MotionManager::invalid_MC_ID) {}
  
  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    const int pink_index = ProjectInterface::getColorIndex("pink");

    
    // Build a local map from what we can see
    localShS.clear();  // needed because mapreq.clearShapes will be false
    vector<Point> gazePts;
    int ground_frame_offset = -200;
    gazePts.push_back(Point(200, 0, ground_frame_offset, egocentric));
    gazePts.push_back(Point(400, 0, ground_frame_offset, egocentric));
    gazePts.push_back(Point(400, 100,ground_frame_offset, egocentric));
    gazePts.push_back(Point(400, -100, ground_frame_offset, egocentric));
    NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, gazePts, false));
    
    MapBuilderRequest mapreq(MapBuilderRequest::localMap);
    mapreq.searchArea = gazePoly;
    mapreq.doScan = true;
    mapreq.pursueShapes = true;
    mapreq.maxDist = 2000;
    mapreq.clearShapes = false;   // preserve the scan PolygonShape
    mapreq.objectColors[blobDataType].insert(pink_index);
    mapreq.minBlobAreas[pink_index] = 50;
    mapreq.rawY = true;
    
    unsigned int mapreq_id = mapBuilder.executeRequest(mapreq);
    erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::deactivateETID);
  }
  
  void processEvent(const EventBase &event) {
    if(event.getGeneratorID() == EventBase::mapbuilderEGID){
      //Map builder finished, now look at the object
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS)); 
      if(blobs.size() == 0){
	cout << "Card not Found " << endl;
	erouter->postEvent(DataEvent<Result>(notFound,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
	return;
      }
      Point target = blobs[0]->getCentroid();
      cout << "Found card" << endl;
      //Actually look at it
      SharedObject<HeadPointerMC> head_mc;
      head_mc->lookAtPoint(target.coordX(), target.coordY(), target.coordZ());
      headpointer_id = motman->addPersistentMotion(head_mc);
      erouter->addListener(this, EventBase::motmanEGID, headpointer_id, EventBase::statusETID);
    }
    else if(event.getGeneratorID() == EventBase::motmanEGID && event.getSourceID() == headpointer_id){
      cout << "Now looking at card" << endl;
      erouter->postEvent(DataEvent<Result>(found,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
      return;
    }
  }
  void DoStop() {
    motman->removeMotion(headpointer_id);
    headpointer_id = MotionManager::invalid_MC_ID;
    cout << "LookAtCard stopped." << endl;
    VisualRoutinesStateNode::DoStop();
  }
};

//**************IdentifyCard******************

class IdentifyCard : public VisualRoutinesStateNode {
  protected:
    int ip;

public:
  IdentifyCard() : VisualRoutinesStateNode("IdentifyCard"), ip(string_util::stringToIntIP("128.2.177.223")) {}
  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    cout << "IdentifyCard starting up." << endl;
    

    //Get image as an array of bytes
    /*NEW_SKETCH(camFrame, uchar, sketchFromRawY());
    cout << "creating pic array" << endl;
    byteArray pic;
    memcpy(&pic.buf, camFrame->getRawPixels(), 33280);

    
    erouter->addRemoteListener(this, ip, EventBase::stateSignalEGID);
    erouter->postEvent(DataEvent<byteArray>(pic, EventBase::stateSignalEGID,(unsigned int)this, EventBase::statusETID)); 
    */
    erouter->postEvent(DataEvent<CardType>(rank,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
  }
  
  void DoStop() {
    cout << "IdentifyCard stopped." << endl;
    VisualRoutinesStateNode::DoStop();
  }

  void processEvent(const EventBase &event) {
    if(event.getHostID() == ip && event.getGeneratorID() == EventBase::stateSignalEGID){
      cout << "Received card type event" << endl;
      const DataEvent<CardType> &cEvent = dynamic_cast<const DataEvent<CardType>& >(event);
      if(cEvent.getData() == face){
	cout << "Card is Face card" << endl;
	erouter->postEvent(DataEvent<CardType>(face,EventBase::stateSignalEGID,(unsigned int)this, EventBase::statusETID));
      }
      else{
	cout << "Card is Rank card" << endl;
	erouter->postEvent(DataEvent<CardType>(rank,EventBase::stateSignalEGID,(unsigned int)this, EventBase::statusETID));
      }

    }
  }
};

//****************LookForBlock****************
class LookForBlock : public VisualRoutinesStateNode {
  // head pointer
  MotionManager::MC_ID headpointer_id;
  
 public:
 LookForBlock() : VisualRoutinesStateNode("LookForBlock"), headpointer_id(MotionManager::invalid_MC_ID) {}
  
  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    const int pink_index = ProjectInterface::getColorIndex("pink");
    
    // Build a local map from what we can see
    localShS.clear();  // needed because mapreq.clearShapes will be false
    NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, Lookout::groundSearchPoints(), false));
    
    MapBuilderRequest mapreq(MapBuilderRequest::localMap);
    mapreq.searchArea = gazePoly;
    mapreq.doScan = true;
    mapreq.pursueShapes = true;
    mapreq.maxDist = 2000;
    mapreq.clearShapes = false;   // preserve the scan PolygonShape
    mapreq.objectColors[blobDataType].insert(pink_index);
    mapreq.minBlobAreas[pink_index] = 50;
    mapreq.rawY = true;
    
    unsigned int mapreq_id = mapBuilder.executeRequest(mapreq);
    erouter->addListener(this, EventBase::mapbuilderEGID, mapreq_id, EventBase::deactivateETID);
  }
  
  void processEvent(const EventBase &event) {
    if(event.getGeneratorID() == EventBase::mapbuilderEGID){
      //Map builder finished, now look at the object
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS)); 
      if(blobs.size() == 0){
	cout << "No more Cards found " << endl;
	erouter->postEvent(DataEvent<Result>(notFound,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
	return;
      }
      Point target = blobs[0]->getCentroid();
      cout << "Found target" << endl;
      //Actually look at it
      SharedObject<HeadPointerMC> head_mc;
      head_mc->lookAtPoint(target.coordX(), target.coordY(), target.coordZ());
      headpointer_id = motman->addPersistentMotion(head_mc);
      erouter->addListener(this, EventBase::motmanEGID, headpointer_id, EventBase::statusETID);
    }
    else if(event.getGeneratorID() == EventBase::motmanEGID && event.getSourceID() == headpointer_id){
      cout << "Now looking at pink blob" << endl;
      erouter->postEvent(DataEvent<Result>(found,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
      return;
    }
  }
  void DoStop() {
    motman->removeMotion(headpointer_id);
    headpointer_id = MotionManager::invalid_MC_ID;
    cout << "LookForBlock stopped." << endl;
    VisualRoutinesStateNode::DoStop();
  }
};

//**************** TrackBlock ****************

//! Keep the camera pointed at the bone
class TrackBlock : public VisualRoutinesStateNode {
 public:
 TrackBlock() : VisualRoutinesStateNode("TrackBlock"),
    head_mc(), head_id(MotionManager::invalid_MC_ID) {}
  
  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    cout << "TrackBlock starting up." << endl;
    head_id = motman->addPersistentMotion(head_mc);
    erouter->addTimer(this,1,500,true);
  }
  
  void DoStop() {
    motman->removeMotion(head_id);
    head_id = MotionManager::invalid_MC_ID;
    cout << "TrackBlock stopped." << endl;
    VisualRoutinesStateNode::DoStop();
  }
  
  void processEvent(const EventBase &) {
    camShS.clear();
    const color_index pink_index = ProjectInterface::getColorIndex("pink");
    NEW_SHAPEVEC(pinkblobs, BlobData, 
		 getBlobsFromRegionGenerator(pink_index,30,BlobData::groundplane,1));
    projectToGround();
    //cout << "Finding pink blobs" << endl;
    if ( groundShS.allShapes().size() == 0 ) {
      cout << "TrackBlock found no pink blob" << endl;
      return;
    }
    Shape<BlobData> groundblock = ShapeRootType(groundShS.allShapes()[0],BlobData);
    Point target = groundblock->getCentroid();
    //cout << "TrackBlock target = " << target << endl;
    MMAccessor<HeadPointerMC>(head_id)->lookAtPoint(target.coordX(),target.coordY(),target.coordZ());
  }
  
 private:
  SharedObject<HeadPointerMC> head_mc;
  MotionManager::MC_ID head_id;
};

#define CALC_ORIENT		     \
  GET_SHAPE(pt1,PointData,localShS); \
  GET_SHAPE(pt2,PointData,localShS);				   \
  Point const midpt = (pt1->getCentroid() + pt2->getCentroid())/2;   \
  AngSignPi const midbearing = atan2(midpt.coordY(),midpt.coordX()); \
  Point const diff = (pt2->getCentroid() - pt1->getCentroid());	     \
  AngPi const local_orient = atan2(diff.coordY(),diff.coordX());      \
  AngSignPi const perp_heading = float(local_orient) - AngPi(M_PI/2); \
  AngSignPi const adist = midbearing + perp_heading;		      \
  float const dist = 150;					      \
  
/*
  cout << "midbearing = " << float(midbearing)*180/M_PI			\
  << "    local_orient = " << float(local_orient)*180/M_PI		\
  << "    perp_heading = " << float(perp_heading)*180/M_PI		\
  << "    adist = " << float(adist)*180/M_PI << endl;
*/

//**************** CalcDest ****************
//! Calculate destination point for approach
class CalcDest : public VisualRoutinesStateNode {
 public:
 CalcDest() : VisualRoutinesStateNode("CalcDest") {}
  
  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    cout << "CalcDest is starting up" << endl;
    camSkS.clear();
    camShS.clear();
    const color_index pink_index = ProjectInterface::getColorIndex("pink");
    NEW_SHAPEVEC(pinkblobs, BlobData, 
		 getBlobsFromRegionGenerator(pink_index,30,BlobData::groundplane,1));
    if ( pinkblobs.size() == 0 ) {
      cout << "CalcDest: no pink blobs" << endl;
      erouter->postEvent(DataEvent<TargetAction>(notfound,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
      return;
    }
    Shape<BlobData> camblock = pinkblobs[0];;
    NEW_SKETCH(pinkrend,bool,camblock->getRendering());
    Region block_reg(camSkS);
    block_reg.addIndices(pinkrend);
    const AngPi cam_orient = block_reg.findPrincipalAxisOrientation();
    cout << "CalcDest: block orientation in cam space = " << float(cam_orient)*180/M_PI << " deg" << endl;
    if ( cam_orient > M_PI/2 ) {
      NEW_SHAPE(pt1, PointData, new PointData(camShS, camblock->bottomLeft));
      NEW_SHAPE(pt2, PointData, new PointData(camShS, camblock->topRight));
    } else {
      NEW_SHAPE(pt1, PointData, new PointData(camShS, camblock->topLeft));
      NEW_SHAPE(pt2, PointData, new PointData(camShS, camblock->bottomRight));
    }

    groundShS.clear();
    projectToGround();
    localShS.clear();
    localShS.importShapes(groundShS);
    CALC_ORIENT
    float const dx = dist * cos(perp_heading);
    float const dy = dist * sin(perp_heading);
    Point destpoint = midpt - Point(dx,dy,0);
    NEW_SHAPE(dest,PointData,new PointData(localShS, destpoint));
    const TargetAction a = 
      ( fabs(destpoint.coordX()-dist) > 30 || fabs(destpoint.coordY()) > 60 || fabs(adist) > 0.75 ) ?
      approach : identify;
    erouter->postEvent(DataEvent<TargetAction>(a,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
  }

};


//**************** ApproachTarget ****************
class ApproachTarget : public WalkEngineNode<WalkMC>, public VRmixin {
 public:
 ApproachTarget() : WalkEngineNode<WalkMC>("ApproachTarget",0,0,0) {}
  
  void DoStart() {
    WalkEngineNode<WalkMC>::DoStart();
    
    GET_SHAPE(dest,PointData,localShS);
    if ( ! dest.isValid() ) {
      cout << "ApproachTarget couldn't find 'dest' in localShS" << endl;
      postCompletionEvent();
      return;
    }
    Point const destpoint = dest->getCentroid();
    CALC_ORIENT
      
      float dx=0, dy=0, da=0;
    if ( destpoint.coordX() > dist )
      dx = min(50.f,destpoint.coordX()-dist);
    else if ( destpoint.coordX() < dist-50.f )
      dx = max(-50.f,dist-destpoint.coordX());
    if ( destpoint.coordX() < 400 )
      if ( destpoint.coordY() > 30.f)
	dy = min(destpoint.coordY()/2,30.f);
      else if ( destpoint.coordY() < -5)
	dy = max(destpoint.coordY()/2,-30.f);
    if ( fabs(destpoint.coordX()-dist) < 100 )
      da = max(-0.2f, min(0.2f, float(adist)/4));
    cout << "Approach:  dx = " << dx << "  dy = " << dy << "  da = " << float(da)*180/M_PI << endl;
    getMC()->setSlowMo(0.5);
    getMC()->setTargetDisplacement(dx,dy,da,2);
  }
  
};


//**************** FindFaceG ****************
class FindFaceG : public VisualRoutinesStateNode {
 public:
 FindFaceG() : VisualRoutinesStateNode("FindFaceG"),
    head_mc(), head_id(MotionManager::invalid_MC_ID), gaze_index(0) {}
  
  void DoStart() {
    cout << "FindFaceG starting." << endl;
    VisualRoutinesStateNode::DoStart();
    head_id = motman->addPersistentMotion(head_mc);
    erouter->addListener(this,EventBase::motmanEGID,head_id,EventBase::statusETID);
    moveToNextGazePoint();
  }
  
  void DoStop() {
    motman->removeMotion(head_id);
    head_id = MotionManager::invalid_MC_ID;
    VisualRoutinesStateNode::DoStop();
  }

  void processEvent(const EventBase &event) {
    if ( event.getGeneratorID() == EventBase::motmanEGID ) {
      erouter->addTimer(this,1,1000);
      return;
    }
    // event was a timer expiration: ready to take a picture now
    localShS.clear();
    int const orange_index = ProjectInterface::getColorIndex("orange");
    NEW_SHAPEVEC(oblobs,BlobData,getBlobsFromRegionGenerator(orange_index,200,BlobData::groundplane,1));
    if ( oblobs.size() == 0 )
      moveToNextGazePoint();
    else
      postCompletionEvent();
  }
  
  void moveToNextGazePoint() {
    MMAccessor<HeadPointerMC>(head_id)->setJoints(0,gaze_points[gaze_index],0);
    // cout << "FindCone gazing at " << gaze_points[gaze_index]*180/M_PI << endl;
    if ( gaze_points[++gaze_index] == 999 )
      gaze_index = 0;
  }

 private:
  SharedObject<HeadPointerMC> head_mc;
  MotionManager::MC_ID head_id;
  int gaze_index;
  static float const gaze_points[];
};

//**************** PushToFaceG ****************
class PushToFaceG  : public VisualRoutinesStateNode {
 public:
 PushToFaceG() : VisualRoutinesStateNode("PushToFaceG"),
    head_mc(), head_id(MotionManager::invalid_MC_ID),
    walk_mc(), walk_id(MotionManager::invalid_MC_ID) {}
  
  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    head_mc->setJoints(0,state->outputs[HeadOffset+PanOffset],0);
    head_id = motman->addPersistentMotion(head_mc);
    walk_mc->LoadFile("bone/grasp.prm");
    walk_mc->setSlowMo(0.5);
    walk_id = motman->addPersistentMotion(walk_mc);
    erouter->addTimer(this,1,500,true);
  }
  
  void DoStop() {
    motman->removeMotion(walk_id);
    walk_id = MotionManager::invalid_MC_ID;
    motman->removeMotion(head_id);
    head_id = MotionManager::invalid_MC_ID;
    VisualRoutinesStateNode::DoStop();
  }
  
  void processEvent(const EventBase&) {
    camShS.clear();
    int const orange_index = ProjectInterface::getColorIndex("orange");
    NEW_SHAPEVEC(oblobs,BlobData,getBlobsFromRegionGenerator(orange_index,200,BlobData::groundplane,1));
    if ( oblobs.size() == 0 )
      return;
    
    //Checks if it's close enough to the cone
    if (oblobs[0]->getArea() > 20000){
      postCompletionEvent();
      return;
    }	
    
    Point opoint = oblobs[0]->getCentroid();
    // cout << "Orange blob at " << opoint <<endl;
    double const head_pan_angle = state->outputs[HeadOffset+PanOffset];
    double const head_move = (camSkS.getWidth()/2 - opoint.coordX()) / camSkS.getWidth() * CameraFOV;
    // cout << "head_pan_angle = " << head_pan_angle*180/M_PI 
    // << "   head_move = " << head_move*180/M_PI << endl;
    double new_pan = head_pan_angle + head_move;
    MMAccessor<HeadPointerMC>(head_id)->setJoints(0,new_pan,0);
    MMAccessor<WalkMC>(walk_id)->setTargetVelocity(80,0,new_pan/2);
  }
  
 private:
  SharedObject<HeadPointerMC> head_mc;
  MotionManager::MC_ID head_id;
  SharedObject<WalkMC> walk_mc;
  MotionManager::MC_ID walk_id;
};

//**************** FindRankG ****************
class FindRankG : public VisualRoutinesStateNode {
 public:
 FindRankG() : VisualRoutinesStateNode("FindRankG"),
	       head_mc(), head_id(MotionManager::invalid_MC_ID), gaze_index(0), count(0) {}
  
  void DoStart() {
    cout << "FindRankG starting." << endl;
    VisualRoutinesStateNode::DoStart();
    head_id = motman->addPersistentMotion(head_mc);
    erouter->addListener(this,EventBase::motmanEGID,head_id,EventBase::statusETID);
    moveToNextGazePoint();
  }
  
  void DoStop() {
    motman->removeMotion(head_id);
    head_id = MotionManager::invalid_MC_ID;
    cout << "Find Rank G stopped" << endl;
    VisualRoutinesStateNode::DoStop();
  }

  void processEvent(const EventBase &event) {
    if ( event.getGeneratorID() == EventBase::motmanEGID ) {
      erouter->addTimer(this,1,1000);
      return;
    }
    // event was a timer expiration: ready to take a picture now
    localShS.clear();
    int const yellow_index = ProjectInterface::getColorIndex("yellow");
    NEW_SHAPEVEC(yblobs,BlobData,getBlobsFromRegionGenerator(yellow_index,200,BlobData::groundplane,1));
    if ( yblobs.size() == 0 )
      moveToNextGazePoint();
    else
      erouter->postEvent(DataEvent<Result>(found,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
  }
  
  void moveToNextGazePoint() {
    MMAccessor<HeadPointerMC>(head_id)->setJoints(0,gaze_points[gaze_index],0);
    // cout << "FindCone gazing at " << gaze_points[gaze_index]*180/M_PI << endl;
    count++;
    if ( gaze_points[++gaze_index] == 999 ){
      gaze_index = 0;
    }
    if(count > 10){
      count = 0;
      cout << "Yellow cone not found" << endl;
      erouter->postEvent(DataEvent<Result>(notFound,EventBase::stateSignalEGID,(unsigned int)this,EventBase::statusETID));
    }
  }
  
 private:
  SharedObject<HeadPointerMC> head_mc;
  MotionManager::MC_ID head_id;
  int gaze_index;
  static float const gaze_points[];
  int count;
};

//**************** PushToRankG ****************
class PushToRankG  : public VisualRoutinesStateNode {
 public:
 PushToRankG() : VisualRoutinesStateNode("PushToRankG"),
    head_mc(), head_id(MotionManager::invalid_MC_ID),
    walk_mc(), walk_id(MotionManager::invalid_MC_ID) {}
  
  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    head_mc->setJoints(0,state->outputs[HeadOffset+PanOffset],0);
    head_id = motman->addPersistentMotion(head_mc);
    walk_mc->LoadFile("bone/grasp.prm");
    walk_mc->setSlowMo(0.5);
    walk_id = motman->addPersistentMotion(walk_mc);
    erouter->addTimer(this,1,500,true);
  }
  
  void DoStop() {
    motman->removeMotion(walk_id);
    walk_id = MotionManager::invalid_MC_ID;
    motman->removeMotion(head_id);
    head_id = MotionManager::invalid_MC_ID;
    VisualRoutinesStateNode::DoStop();
  }
  
  void processEvent(const EventBase&) {
    camShS.clear();
    int const yellow_index = ProjectInterface::getColorIndex("yellow");
    NEW_SHAPEVEC(yblobs,BlobData,getBlobsFromRegionGenerator(yellow_index,200,BlobData::groundplane,1));
    if ( yblobs.size() == 0 )
      return;
    
    //Checks if it's close enough to the cone
    if (yblobs[0]->getArea() > 20000){
      postCompletionEvent();
      return;
    }	
    
    Point ypoint = yblobs[0]->getCentroid();
    // cout << "Orange blob at " << opoint <<endl;
    double const head_pan_angle = state->outputs[HeadOffset+PanOffset];
    double const head_move = (camSkS.getWidth()/2 - ypoint.coordX()) / camSkS.getWidth() * CameraFOV;
    // cout << "head_pan_angle = " << head_pan_angle*180/M_PI 
    // << "   head_move = " << head_move*180/M_PI << endl;
    double new_pan = head_pan_angle + head_move;
    MMAccessor<HeadPointerMC>(head_id)->setJoints(0,new_pan,0);
    MMAccessor<WalkMC>(walk_id)->setTargetVelocity(80,0,new_pan/2);
  }
  
 private:
  SharedObject<HeadPointerMC> head_mc;
  MotionManager::MC_ID head_id;
  SharedObject<WalkMC> walk_mc;
  MotionManager::MC_ID walk_id;
};


//**************** TurnArnoud ****************
class TurnAround  : public VisualRoutinesStateNode {
 protected:
 unsigned int pilot_id;


 public:
 TurnAround() : VisualRoutinesStateNode("TurnAround"), pilot_id(Pilot::invalid_Pilot_ID) {}
  
  void DoStart() {
    VisualRoutinesStateNode::DoStart();
    cout << "Turn Around is starting up" << endl;

    //Determines the location to walk to
    NEW_SHAPEVEC(agents, AgentData, select_type<AgentData>(worldShS));
    //we want to go to the spot 100 behind us
    Point dogLoc = agents[0]->getCentroid();
    Point target = Point(dogLoc.coordX()-100, dogLoc.coordY(), dogLoc.coordZ());
    
    PilotRequest preq(PilotRequest::walk);
    preq.da = M_PI*0.75;
    cout<< "Sending off request to turn around" <<endl;
    erouter->addListener(this,EventBase::pilotEGID);
    pilot_id = pilot.executeRequest(preq);
    cout << "Pilot turning around, id " << pilot_id << endl;
  }
  
  void DoStop() {
    pilot.abort();
    pilot_id = MotionManager::invalid_MC_ID;
    VisualRoutinesStateNode::DoStop();
    cout << "Turn around is stopping" << endl;
  }
  
  void processEvent(const EventBase& event) {
    if(event.getGeneratorID() == EventBase::pilotEGID){
      cout << "Finished Turning" << endl;
      postCompletionEvent();
    }
  }
};


//**************** finalProj ****************
class finalProj : public VisualRoutinesStateNode {
 public:
 finalProj() : VisualRoutinesStateNode("finalProj") {}
  
  void setup() {
    
#statemachine
  start: StateNode == EventTrans($,
				EventBase::buttonEGID,
				RobotInfo::HeadFrButOffset,
				EventBase::activateETID) ==>
      lookAround: LookForBlock() == SignalTrans<Result>($, found) ==>
      nullNode: StateNode == multistart: NullTrans ==>
      trackBlock: TrackBlock() == ftransF:CompletionTrans($,1) ==> 
      FindFaceG() == CompletionTrans ==>
      PushToFaceG()  == CompletionTrans  ==>
      setDown: TinyMotionSequenceNode("$", "bone/acquire.mot") 
      	       == CompletionTrans ==>
      walkAwayG: WalkEngineNode<WalkMC>()
      [getMC()->LoadFile("bone/acquire.prm");
       getMC()->setSlowMo(0.5);
       setDisplacement(-175, 0, 0, 5);] == CompletionTrans ==>
      turnAround: TurnAround() == CompletionTrans ==>
      lookAround
      
      lookAround == SignalTrans<Result>($, notFound) ==>
      lookAround

      trackBlock == ftransR:CompletionTrans($,1) ==> 
      findRG: FindRankG() == SignalTrans<Result>($, found) ==>
      PushToRankG()  == CompletionTrans  ==>
      setDown
	   
      findRG == SignalTrans<Result>($, notFound) ==>
      turn90: WalkEngineNode<WalkMC>()
      [getMC()->LoadFile("bone/grasp.prm");
       getMC()->setSlowMo(0.5);
       setDisplacement(0,0,M_PI/2,5);] == CompletionTrans ==>
      findRG
      
      multistart ==>
      calcdest: CalcDest() == SignalTrans<TargetAction>($,approach) ==>
      ApproachTarget() == CompletionTrans ==>
      StateNode == TimeOutTrans($,500) ==> calcdest
      
      calcdest == SignalTrans<TargetAction>($, identify) ==>	
      TinyMotionSequenceNode("$","bone/acquire.mot") == CompletionTrans ==>
      walkToCard: WalkEngineNode<WalkMC>()
      [getMC()->LoadFile("bone/acquire.prm");
       getMC()->setSlowMo(0.5);
       setDisplacement(175, 0, 0, 5); ] == CompletionTrans ==>
      TinyMotionSequenceNode("$", "situpL.mot") == CompletionTrans ==>
      lookCard: LookAtCard() == SignalTrans<Result>($, found) ==>
      identifyCard: IdentifyCard() == SignalTrans<CardType>($, face) ==>
      SmallMotionSequenceNode("$","bone/acquire2.mot") == ftransF
      
      lookCard == SignalTrans<Result>($, notFound) ==> lookAround

      calcdest == SignalTrans<TargetAction>($, notfound) ==>
      lookAround

      identifyCard == SignalTrans<CardType>($, rank) ==>
      SmallMotionSequenceNode("$","bone/acquire2.mot") == ftransR
#endstatemachine

      startnode = start;
  }
  
  void DoStart() {
    cout << "finalProj starting up" << endl;
    VisualRoutinesStateNode::DoStart();
    erouter->addListener(this,EventBase::textmsgEGID);
  }
  
  
  finalProj(const finalProj&); //!< do not call
  finalProj& operator=(const finalProj&); //!< do not call
  
};

#endif