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

$nodeclass Lab8Q1 : PilotDemo {
	$provide vector<ShapeRoot> rMarkers;
	virtual void buildMap() {
		//marker set 1
		NEW_SHAPE(redSph1, EllipseData, 
			new EllipseData(worldShS, Point(1000, -300, 0, allocentric), 
		30, 30));
	redSph1->setColor("red");
		
		NEW_SHAPE(blueSph1, EllipseData, 
			new EllipseData(worldShS, Point(1000, 300, 0, allocentric), 
		30, 30));
	blueSph1->setColor("blue");
		
		//marker set 2
		NEW_SHAPE(redSph2, EllipseData, 
			new EllipseData(worldShS, Point(-300, 2000, 0, allocentric), 
		30, 30));
	redSph2->setColor("red");
		
		NEW_SHAPE(blueSph2, EllipseData, 
			new EllipseData(worldShS, Point(300, 2000, 0, allocentric), 
		30, 30));
	blueSph2->setColor("blue");
		rMarkers.push_back(redSph1);
		rMarkers.push_back(blueSph1);
		rMarkers.push_back(redSph2);
		rMarkers.push_back(blueSph2);
	}
	
	virtual void doStart(){
		pilot->computeParticleBounds(2000, 2000);
		particleFilter->resetFilter();
		//particleFilter->displayIndividualParticles(100);
	}

	$nodeclass Localizer : PilotNode( PilotTypes::localize) : doStart{
		pilotreq.landmarkExtractor = new MapBuilderRequest(MapBuilderRequest::localMap);
		pilotreq.landmarkExtractor->addObjectColor(ellipseDataType, "red");
		pilotreq.landmarkExtractor->addObjectColor(ellipseDataType, "blue");
		//vector<ShapeRoot> markers;
		$reference Lab8Q1::rMarkers;
		pilotreq.landmarks = rMarkers;
	}
	
	
	$setupmachine{
		wait: StateNode
		startdemo: Localizer =PILOT=>wait
		wait =TM("local")=>Localizer=PILOT=>SpeechNode("localization done")=C=>wait
		wait =TM("go")=>Turn(mathutils::deg2rad(90.0))=PILOT=>WalkForward(100)=C=>wait
	}

}

REGISTER_BEHAVIOR(Lab8Q1);