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

$nodeclass Lab8Part2 : PilotDemo {
    virtual void buildMap() {
        NEW_SHAPE(r1, EllipseData, new EllipseData(worldShS, Point(1000,300,0,allocentric), 125, 125));
	NEW_SHAPE(l1, EllipseData, new EllipseData(worldShS, Point(1000,-300,0,allocentric), 125, 125));
	NEW_SHAPE(r2, EllipseData, new EllipseData(worldShS, Point(-300,1500,0,allocentric),125,125));
	NEW_SHAPE(l2, EllipseData, new EllipseData(worldShS, Point(300,1500,0,allocentric),125,125));
	particleFilter->resetFilter();
    }
    $nodeclass Go : PilotNode : doStart {
        MapBuilderRequest *mapreq = new MapBuilderRequest(MapBuilderRequest::localMap);
	mapreq->addAllObjectColors(ellipseDataType);
	pilotreq.landmarkExtractor = mapreq;
	NEW_SHAPEROOTVEC(landmarks, worldShS);
	pilotreq.landmarks = landmarks;
	pilotreq.requestType = localize;
	cout << "Here" << endl;
    }

    $setupmachine {
        startdemo: Go =C=> SpeechNode("done") =TM("go")=> Turn(1.57) =C=> Go =C=> SpeechNode("done")
    }
}

REGISTER_BEHAVIOR(Lab8Part2);