#include "Motion/WalkMC.h"
#ifdef TGT_HAS_WALK

#include "Behaviors/Demos/Navigation/PilotDemo.h"

$nodeclass ServoGrabTest : PilotDemo {
  $provide bool endLoop(false);

  $nodeclass PushIt : PilotNode(PilotTypes::pushObject) {
    static ShapeRoot matchBox(const ShapeRoot &) {
      std::cout<<"::matchBox start::"<<endl;
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(localShS));
      if ( !blobs.empty() )
	return blobs[0];
      else
	return ShapeRoot();
    }
    
    virtual void doStart() {
      std::cout<<"::PushIt started::"<<endl;
      pilotreq.collisionAction = collisionIgnore;
      NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS));
      NEW_SHAPEVEC(redBlobs, BlobData, subset(blobs, IsColor(ProjectInterface::getColorIndex("red"))));
      for(unsigned int i = 0; i < redBlobs.size(); i++){
	std::cout<<"::blob["<<i<<"] was targeted::"<<redBlobs[i]<<endl;
	redBlobs[i]->setObstacle(false);
	pilotreq.objectShape = redBlobs[i];
	NEW_SHAPE(target, EllipseData, new EllipseData(worldShS, (redBlobs[i]->getCentroid()+redBlobs[i]->getCentroid()/4), 10, 10));
	target->setObstacle(false);
	pilotreq.targetShape = target; //Sets the target to push an object to.
	break;
      }
      MapBuilderRequest *mapreq = new MapBuilderRequest(MapBuilderRequest::localMap);
      std::vector<Point> ptVec;
      Point p1 = Point(-100, -100, 0, allocentric);
      Point p2 = Point(100, -100, 0, allocentric);
      Point p3 = Point(-100, 100, 0, allocentric);
      Point p4 = Point(100, 100, 0, allocentric);
      ptVec.push_back(pilotreq.objectShape->getCentroid());
      ptVec.push_back(pilotreq.objectShape->getCentroid()+p1);
      ptVec.push_back(pilotreq.objectShape->getCentroid()+p2);
      ptVec.push_back(pilotreq.objectShape->getCentroid()+p3);
      ptVec.push_back(pilotreq.objectShape->getCentroid()+p4);
      ptVec.push_back(pilotreq.objectShape->getCentroid());
      NEW_SHAPE_N(polygon, PolygonData, new PolygonData(worldShS, ptVec, false));
      polygon->setObstacle(false);
      mapreq->searchArea = polygon;
      mapreq->addObjectColor(blobDataType, "red");
      mapreq->addBlobOrientation("red", BlobData::pillar);
      pilotreq.objectExtractor = mapreq;
      pilotreq.objectMatcher = &matchBox;
      //pilotreq.displayObstacles = true;
      pilotreq.forwardSpeed = 200;
      pilotreq.turnSpeed = .25;
      pilot->setVerbosity(pilot->getVerbosity()|Pilot::PVnavStep);
      $reference ServoGrabTest::endLoop;
      endLoop = false;
    }
  }
  
  $nodeclass TakeAPictureLocal : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    std::cout<<"::TakeAPictureLocal start::"<<endl;
    NEW_SHAPE(homePoint, PointData, new PointData(worldShS, theAgent->getCentroid()));
    homePoint->setObstacle(false);
    vector<Point> gazePts = Lookout::groundSearchPoints();
    NEW_SHAPE(gazePoly, PolygonData, new PolygonData(localShS, gazePts, true));
    mapreq.searchArea = gazePoly;
    mapreq.addObjectColor(blobDataType,"red");
    mapreq.addBlobOrientation("red", BlobData::pillar);
  }
  
  $nodeclass ClearWorld : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
    std::cout<<"::ClearWorld start::"<<endl;
    mapreq.clearLocal = true;
    mapreq.clearWorld = true;
  }

  $nodeclass GoHome : PilotNode(PilotTypes::goToShape) : doStart {
    std::cout<<"GoHome start"<<endl;
    NEW_SHAPEVEC(blobs, BlobData, select_type<BlobData>(worldShS));
    NEW_SHAPEVEC(redBlobs, BlobData, subset(blobs, IsColor(ProjectInterface::getColorIndex("red"))));
    for(unsigned int i = 0; i < redBlobs.size(); i++){
      std::cout<<"::blob["<<i<<"] is not an obstacle::"<<redBlobs[i]<<endl;
      redBlobs[i]->setObstacle(false);
    }
    GET_SHAPE(homePoint, PointData, worldShS);
    pilotreq.targetShape = homePoint;			
    pilotreq.collisionAction = collisionIgnore;
    pilotreq.forwardSpeed = 200.f;
    pilotreq.turnSpeed = .25;
  }

  $nodeclass Backward : PilotNode(PilotTypes::walk) :doStart{
    std::cout<<"::Backward start::"<<endl;
    pilotreq.dx = -50;
    pilotreq.forwardSpeed = 200;
    pilotreq.collisionAction = collisionIgnore;
  } 

  $nodeclass TurnAround : PilotNode(PilotTypes::walk) :doStart{
    std::cout<<"::TurnAround start::"<<endl;
    pilotreq.da = M_PI;
    pilotreq.collisionAction = collisionIgnore;
    pilotreq.turnSpeed = .25;
  }
  
  $nodeclass CheckLoop : StateNode() : doStart {
    $reference ServoGrabTest::endLoop;
    if(endLoop) postStateSuccess();
    else postStateFailure();
  }
  
  $nodeclass EndLoopState : StateNode() : doStart {
    $reference ServoGrabTest::endLoop;
    endLoop = true;
    postStateCompletion();
  }

  $setupmachine{
  startdemo:  PostureNode("armBeforeGrab.pos") =C=> TakeAPictureLocal =C=> PushIt =PILOT(noError)=> PostureNode("armGrabClosed.pos") =C=> {holdloop, gohome}
        
  holdloop: PIDNode(GripperOffset, GripperOffset+1, 0.0f) =T(100)=> PIDNode(GripperOffset, GripperOffset+1, 1.0f) =C=> PostureNode("armGrabLift.pos") =T(4000)=> checkloop
      checkloop: CheckLoop
      checkloop =S=> SpeechNode("end loop")
      checkloop =F=> holdloop

      gohome: GoHome =C=> EndLoopState =T(5000)=> PostureNode("armGrabLower.pos") =C=> PostureNode("armGrabDrop.pos") =C=> Backward =C=>  SpeechNode("Throw it again") =T(5000)=> TurnAround =C=> ClearWorld =C=> startdemo
      }
}

REGISTER_BEHAVIOR(ServoGrabTest);

#endif