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

$nodeclass MyParticleFilter : PilotDemo {

  virtual void buildMap() {
    NEW_SHAPE(navMarker1a, EllipseData,
	      new EllipseData(worldShS, Point(2000,-300,0,allocentric), 125, 125));
    navMarker1a->setColor("blue");

    NEW_SHAPE(navMarker1b, EllipseData,
	      new EllipseData(worldShS, Point(2000,300,0,allocentric), 125, 125));
	navMarker1b->setColor("green");

	NEW_SHAPE(navMarker2a, EllipseData,
		new EllipseData(worldShS, Point(-300, -2000, 0, allocentric), 125, 125));
	navMarker2a->setColor("blue");

	NEW_SHAPE(navMarker2b, EllipseData,
		new EllipseData(worldShS, Point(300, -2000, 0, allocentric), 125, 125));
	navMarker2b->setColor("green");
  }

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

  $nodeclass Localize : PilotNode(PilotTypes::localize) : doStart {
	MapBuilderRequest* mapreq = new MapBuilderRequest(MapBuilderRequest::localMap);
	mapreq->addObjectColor(ellipseDataType, "blue");
	mapreq->addObjectColor(ellipseDataType, "green");
	
	pilotreq.landmarkExtractor = mapreq;

    GET_SHAPE(navMarker1a, EllipseData, worldShS);
    GET_SHAPE(navMarker1b, EllipseData, worldShS);
    GET_SHAPE(navMarker2a, EllipseData, worldShS);
    GET_SHAPE(navMarker2b, EllipseData, worldShS);

    pilotreq.landmarks.push_back(navMarker1a);
	pilotreq.landmarks.push_back(navMarker1b);
	pilotreq.landmarks.push_back(navMarker2a);
	pilotreq.landmarks.push_back(navMarker2b);
  }


  $setupmachine{
    rundemo: ResetFilter =N=> Localize
				=TM("go")=> Turn(-Pi/2) =C=> Localize
				=TM("go")=> Localize =N=> SpeechNode("done")
  }

}

REGISTER_BEHAVIOR(MyParticleFilter);