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

$nodeclass FunWithParticles : PilotDemo {

	virtual void buildMap() {
		NEW_SHAPE(blue1, EllipseData,
							new EllipseData(worldShS, Point(1500,300,0,allocentric), 100, 100));
		blue1->setColor("blue");
		NEW_SHAPE(red1, EllipseData,
							new EllipseData(worldShS, Point(1500,-300,0,allocentric), 100, 100));
		red1->setColor("red");

		NEW_SHAPE(blue2, EllipseData,
							new EllipseData(worldShS, Point(-300,2500,0,allocentric), 100, 100));
		blue2->setColor("blue");
		NEW_SHAPE(red2, EllipseData,
							new EllipseData(worldShS, Point(300,2500,0,allocentric), 100, 100));
		red2->setColor("red");
	}

	$nodeclass Loc : PilotNode(PilotTypes::localize) : doStart {
		GET_SHAPE(blue1, EllipseData, worldShS);
		GET_SHAPE(red1, EllipseData, worldShS);
		GET_SHAPE(blue2, EllipseData, worldShS);
		GET_SHAPE(red2, EllipseData, worldShS);

		pilotreq.landmarks.push_back(blue1);
		pilotreq.landmarks.push_back(red1);
		pilotreq.landmarks.push_back(blue2);
		pilotreq.landmarks.push_back(red2);

		MapBuilderRequest* req = new MapBuilderRequest(MapBuilderRequest::localMap);
		req->addObjectColor(ellipseDataType, "blue");
		req->addObjectColor(ellipseDataType, "red");

		pilotreq.landmarkExtractor = req;
	}

	$nodeclass Loc2 : PilotNode(PilotTypes::localize) : doStart {
		GET_SHAPE(blue2, EllipseData, worldShS);
		GET_SHAPE(red2, EllipseData, worldShS);

		pilotreq.landmarks.push_back(blue2);
		pilotreq.landmarks.push_back(red2);

		MapBuilderRequest* req = new MapBuilderRequest(MapBuilderRequest::localMap);
		req->addObjectColor(ellipseDataType, "blue");
		req->addObjectColor(ellipseDataType, "red");

		pilotreq.landmarkExtractor = req;
	}

	$nodeclass Turn : PilotNode(PilotTypes::walk) : doStart {
		pilotreq.da = M_PI / 2;
	}

	$nodeclass Walk : PilotNode(PilotTypes::walk) : doStart {
		pilotreq.dx = 1000;
	}

	$nodeclass Rand : PilotNode(PilotTypes::noRequest) : doStart {
		particleFilter->resetFilter();
	}

	$setupmachine{
  	startdemo: Rand =C=> find 	
	find: Loc 
	find =TM("go")=> turn
	turn: Turn =C=> walk
	walk: Walk =TM("go")=> find2 
	find2: Loc2 
	}
}

REGISTER_BEHAVIOR(FunWithParticles);