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

// Demo program for the 2012 ARTSI Student Robotics Competition.
//
// Locates a red canister and pushes it.

$nodeclass MoveCanister : PilotDemo {

  $nodeclass TakeImage : MapBuilderNode(MapBuilderRequest::localMap) : doStart{
    mapreq.addObjectColor(lineDataType,"blue");
	mapreq.addOccluderColor(lineDataType,"red");
	mapreq.addObjectColor(blobDataType,"red");
	mapreq.addBlobOrientation("red", BlobData::pillar);
  }

  $nodeclass AcquireTarget : PilotNode(PilotTypes::goToShape) : doStart {
    NEW_SHAPE(canister, BlobData, find_if<BlobData>(localShS));
    if ( canister.isValid() )
      pilotreq.targetShape = canister;
    else
      cancelThisRequest();
  }

  $nodeclass Turn30 : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.da = M_PI/3;
	pilotreq.turnSpeed = 0.2;
    pilotreq.collisionAction = collisionIgnore;
  }

  $nodeclass Fwd500 : PilotNode(PilotTypes::walk) : doStart {
    pilotreq.dx = 500;
    pilotreq.collisionAction = collisionIgnore;
  }

  $setupmachine{
  	startdemo: TakeImage =C=> acquire

    acquire: AcquireTarget
    acquire =C=> move
	acquire =F=> SpeechNode("saw nothing") =T(5000)=> startdemo

  	move: Turn30 =C=> Fwd500 =C=> SpeechNode("done")
  }
}

REGISTER_BEHAVIOR(MoveCanister);