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

$nodeclass PushBox1 : PilotDemo {

  virtual void buildMap() {
    NEW_SHAPE(object, EllipseData,
	      new EllipseData(worldShS, Point(600,0,0,allocentric), 125, 100));
    object->setColor("red");

    NEW_SHAPE(target, PointData,
	      new PointData(worldShS, Point(1200,600,0,allocentric)));
    target->setObstacle(false);

    NEW_SHAPE(obstacle, EllipseData,
	      new EllipseData(worldShS, Point(900,300,0,allocentric), 125, 125, -M_PI/4));
  }

  $nodeclass Randomize : StateNode : doStart {
    particleFilter->resetFilter(); 
  }

  $nodeclass Localize : PilotNode(PilotTypes::localize) : doStart {
    GET_SHAPE(object, EllipseData, worldShS);
    GET_SHAPE(obstacle, EllipseData, worldShS);
    pilotreq.landmarks.push_back(object);
    pilotreq.landmarks.push_back(obstacle);
    pilotreq.landmarkExtractor = new MapBuilderRequest(MapBuilderRequest::worldMap);
    pilotreq.landmarkExtractor->addAllObjectColors(ellipseDataType);
  }

  $setupmachine{
    //startdemo: PushIt =C=> SpeechNode("done")
    startdemo: Randomize =N=> Localize =C=> SpeechNode("done");
  }

}

REGISTER_BEHAVIOR(PushBox1);