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

$nodeclass TestLocalize : PilotDemo {
	
	/** Build the map used for localization **/
	virtual void buildMap() {
		NEW_SHAPE(landmark1, EllipseData,
							new EllipseData(worldShS, Point(1500,-300,0,allocentric), 200, 150));
		landmark1->setColor("blue");

		NEW_SHAPE(landmark2, EllipseData,
							new EllipseData(worldShS, Point(1500,300,0,allocentric), 200, 150));
		landmark2->setColor("red");

		NEW_SHAPE(landmark3, EllipseData,
							new EllipseData(worldShS, Point(-300,2500,0,allocentric), 150, 200));
		landmark3->setColor("blue");

		NEW_SHAPE(landmark4, EllipseData,
							new EllipseData(worldShS, Point(300,2500,0,allocentric), 150, 200));
		landmark4->setColor("red");
	}

	/** Performs startup operations **/
	$nodeclass StartNode : StateNode : doStart {
		// Randomize the particle filter		
		particleFilter->resetFilter();
	}

	/** Localize the robot
	**/
	$nodeclass Localize : PilotNode(localize) : doStart {
		GET_SHAPE(landmark1, EllipseData, worldShS);
		GET_SHAPE(landmark2, EllipseData, worldShS);
		GET_SHAPE(landmark3, EllipseData, worldShS);
		GET_SHAPE(landmark4, EllipseData, worldShS);		
		// Tell the pilot node which landmarks to localize against
		pilotreq.landmarks.clear();
		pilotreq.landmarks.push_back(landmark1);
		pilotreq.landmarks.push_back(landmark2);
		pilotreq.landmarks.push_back(landmark3);
		pilotreq.landmarks.push_back(landmark4);
		MapBuilderRequest* extractor = new MapBuilderRequest(MapBuilderRequest::localMap);
		extractor->addObjectColor(ellipseDataType, "blue");
		extractor->addObjectColor(ellipseDataType, "red");
		pilotreq.landmarkExtractor = extractor;
	}

	$nodeclass TurnLeft : PilotNode(walk) : doStart {
		pilotreq.da = deg2rad(90.0);
	}

	$nodeclass WalkForwards : PilotNode(walk) : doStart {
		pilotreq.dx = 500;
	}	

	$setupmachine{
  	rundemo : StateNode

		rundemo =N=> StartNode =N=> Localize =TM("go")=>
			TurnLeft =C=> WalkForwards =C=> Localize =TM("go")=> Localize
  }

}

REGISTER_BEHAVIOR(TestLocalize);