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

$nodeclass BuildWorldMapOne : PilotDemo {

	virtual void buildMap() {
		NEW_SHAPE(red_ellipse_one, EllipseData,
							new EllipseData(worldShS, Point(1000,-300,0,allocentric), 100, 100));
		red_ellipse_one->setColor("red");

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

		NEW_SHAPE(blue_ellipse_one, EllipseData,
							new EllipseData(worldShS, Point(1000,300,0,allocentric), 100, 100));
		blue_ellipse_one->setColor("blue");

		NEW_SHAPE(blue_ellipse_two, EllipseData,
							new EllipseData(worldShS, Point(-300,1000,0,allocentric), 100, 100));
		blue_ellipse_two->setColor("blue");


		particleFilter->resetFilter();
		particleFilter->resizeParticles(400);
		
	}

	$nodeclass LocalizeOne : PilotNode(PilotTypes::localize) : doStart
	{
		GET_SHAPE(blue_ellipse_one, EllipseData, worldShS);
		pilotreq.landmarks.push_back(blue_ellipse_one);
		GET_SHAPE(red_ellipse_one, EllipseData, worldShS);
		pilotreq.landmarks.push_back(red_ellipse_one);


		MapBuilderRequest *mbr = new MapBuilderRequest(MapBuilderRequest::localMap);
		mbr->addObjectColor(ellipseDataType, "red");
		mbr->addObjectColor(ellipseDataType, "blue");
		pilotreq.landmarkExtractor = mbr;
	}

    $nodeclass TurnCCW(float x) : PilotNode(PilotTypes::walk) : doStart {
         pilotreq.da = x;
    }

	$setupmachine{
  	startdemo: LocalizeOne =C=> SpeechNode("done")
  }

}

REGISTER_BEHAVIOR(BuildWorldMapOne);