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

$nodeclass Lab8Q1: PilotDemo {
    virtual void buildMap() {
		NEW_SHAPE(nav1, EllipseData, new EllipseData(worldShS, Point(1000, -300, 0, allocentric), 50, 50));
		nav1->setColor("red");

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

        NEW_SHAPE(nav3, EllipseData, new EllipseData(worldShS, Point(-400, 0, 0, allocentric), 50, 50));
        nav3->setColor("blue");

		NEW_SHAPE(nav4, EllipseData, new EllipseData(worldShS, Point(-1000, 0, 0, allocentric), 50, 50));
		nav4->setColor("red");
		
		//NEW_SHAPE(dummy, EllipseData, new EllipseData(worldShS, Point(0, 0, 0, allocentric), 5000, 5000));
		
	}
	
	$nodeclass LocalizeIt : PilotNode(PilotTypes::localize) : doStart {	
   	    particleFilter->resetFilter();
	    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(worldShS));
	    
	    std::vector<DualCoding::ShapeRoot> landmarks;
	    SHAPEVEC_ITERATE(ellipses, EllipseData, landmark){
    	    landmarks.push_back(landmark);
    	} END_ITERATE;
	    
	    pilotreq.landmarks = landmarks;
        
	    MapBuilderRequest * mapreq = new MapBuilderRequest(MapBuilderRequest::localMap);
	    mapreq->addObjectColor(ellipseDataType, "red");
	    mapreq->addObjectColor(ellipseDataType, "blue");
	    pilotreq.landmarkExtractor = mapreq;
	}
	
	$nodeclass LocalizeIt2 : PilotNode(PilotTypes::localize) : doStart {	
	    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(worldShS));
	    
	    std::vector<DualCoding::ShapeRoot> landmarks;
	    SHAPEVEC_ITERATE(ellipses, EllipseData, landmark){
    	    landmarks.push_back(landmark);
    	} END_ITERATE;
	    
	    pilotreq.landmarks = landmarks;
        
	    MapBuilderRequest * mapreq = new MapBuilderRequest(MapBuilderRequest::localMap);
	    mapreq->addObjectColor(ellipseDataType, "red");
	    mapreq->addObjectColor(ellipseDataType, "blue");
	    pilotreq.landmarkExtractor = mapreq;
	}
	
    $nodeclass GoToLandmark1 : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.collisionAction = collisionIgnore;
        pilotreq.da = 3.1416*3.0/4.0;
        pilotreq.dx = 1000;
    }
    
    $nodeclass Turner : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.collisionAction = collisionIgnore;
        pilotreq.da = 3.1416*3.0/4.0;
    }
    
    $nodeclass Turner2 : PilotNode(PilotTypes::walk) : doStart {
        pilotreq.collisionAction = collisionIgnore;
        pilotreq.da = -3.1416/4.0;
    }
    
    $nodeclass GoToLandmark2 : PilotNode(PilotTypes::walk) : doStart {
    	pilotreq.collisionAction = collisionIgnore;
        pilotreq.da = 3.1416/2.0;
        pilotreq.dx = 900;
    }

	$setupmachine{
      	rundemo: PostureNode("lookdown.pos") =C=> LocalizeIt =TM("go")=> GoToLandmark1 =PILOT=> Turner =PILOT=> PostureNode("lookdown.pos") =C=> LocalizeIt2 =TM("go")=> GoToLandmark2 =PILOT=> Turner2 =PILOT=> PostureNode("lookdown.pos") =C=> LocalizeIt2 =PILOT=> SpeechNode("done")
    }
}
REGISTER_BEHAVIOR(Lab8Q1);