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


$nodeclass ParticleFun : PilotDemo {

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

		NEW_SHAPE(object2, EllipseData, new EllipseData(worldShS, Point(1000, -300, 0, allocentric) , 125, 125));
        object2->setColor("red");

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

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

	particleFilter->resetFilter();
	
	}

	$nodeclass Localizer : PilotNode(PilotTypes::localize) : doStart {

	//particleFilter->resetFilter();
	
 	NEW_SHAPEVEC(spheres, EllipseData, select_type<EllipseData>(worldShS));

	pilotreq.landmarks.clear();
      	pilotreq.landmarks.push_back(spheres[0]);
	pilotreq.landmarks.push_back(spheres[1]);
	pilotreq.landmarks.push_back(spheres[2]);
	pilotreq.landmarks.push_back(spheres[3]);

	pilotreq.landmarkExtractor = new MapBuilderRequest(MapBuilderRequest::worldMap);
	pilotreq.landmarkExtractor->addAllObjectColors(ellipseDataType);


}


	$setupmachine {
		 rundemo: Localizer =TM("go")=> Turn(.6981) =C=> Localizer
	}
}

REGISTER_BEHAVIOR(ParticleFun);