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

$nodeclass Lab8Q2 : PilotDemo {
	
	virtual void buildMap(){
		vector<Point> boxpts;
		float boxSide = 750;
		float offset = 500 + boxSide/2;
		boxpts.push_back(Point(offset-boxSide/2, boxSide/2,0,allocentric));
		boxpts.push_back(Point(offset+boxSide/2, boxSide/2,0,allocentric));
		boxpts.push_back(Point(offset+boxSide/2,-boxSide/2,0,allocentric));
		boxpts.push_back(Point(offset-boxSide/2,-boxSide/2,0,allocentric));
		NEW_SHAPE(box, PolygonData, new PolygonData(worldShS, boxpts, true));
		box->setColor("blue");
		box->setObstacle(false);
		//objects
		NEW_SHAPE(obj1, EllipseData, new EllipseData(worldShS, Point(350, 250, 0, allocentric), 50, 50));
		NEW_SHAPE(obj2, EllipseData, new EllipseData(worldShS, Point(700, 800, 0, allocentric), 50, 50));
		NEW_SHAPE(obj3, EllipseData, new EllipseData(worldShS, Point(1300, 500, 0, allocentric), 50, 50));
		NEW_SHAPE(obj4, EllipseData, new EllipseData(worldShS, Point(1500, -200, 0, allocentric), 50, 50));
		NEW_SHAPE(obj5, EllipseData, new EllipseData(worldShS, Point(800, -850, 0, allocentric), 50, 50));
		//obstacles
		NEW_SHAPE(obs1, EllipseData, new EllipseData(worldShS, Point(1300, 1200, 0, allocentric), 50, 50));
		NEW_SHAPE(obs2, EllipseData, new EllipseData(worldShS, Point(650, -300, 0, allocentric), 50, 50));
		NEW_SHAPE(obs3, EllipseData, new EllipseData(worldShS, Point(400, -600, 0, allocentric), 50, 50));
		//set color		
		obj1->setColor("red");
		obj2->setColor("red");
		obj3->setColor("red");
		obj4->setColor("red");
		obj5->setColor("red");
		obs1->setColor("green");
		obs2->setColor("green");
		obs2->setObstacle(false);
		obs3->setColor("green");
		
		//obj1->setMobile(true);

		//set targets
		NEW_SHAPE(target1, PointData, new PointData(worldShS, Point(875, 0, 0, allocentric)));
		NEW_SHAPE(target2, PointData, new PointData(worldShS, Point(630, 250, 0, allocentric)));
		NEW_SHAPE(target3, PointData, new PointData(worldShS, Point(1150, 250, 0, allocentric)));
		NEW_SHAPE(target4, PointData, new PointData(worldShS, Point(1150, -75, 0, allocentric)));
		NEW_SHAPE(target5, PointData, new PointData(worldShS, Point(900, -300, 0, allocentric)));
		target1->setObstacle(false);
		target2->setObstacle(false);
		target3->setObstacle(false);
		target4->setObstacle(false);
		target5->setObstacle(false);
		
	}
	
	$nodeclass Push1 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(obj1, EllipseData, worldShS);
		GET_SHAPE(target1, PointData, worldShS);
		obj1->setObstacle(false);
		pilotreq.objectShape = obj1;
		pilotreq.targetShape = target1;
	}
	$nodeclass Push2 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(obj2, EllipseData, worldShS);
		GET_SHAPE(target2, PointData, worldShS);
		obj2->setObstacle(false);
		pilotreq.objectShape = obj2;
		pilotreq.targetShape = target2;
	}
	$nodeclass Push3 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(obj3, EllipseData, worldShS);
		GET_SHAPE(target3, PointData, worldShS);
		obj3->setObstacle(false);
		pilotreq.objectShape = obj3;
		pilotreq.targetShape = target3;
	}
	$nodeclass Push4 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(obj1, EllipseData, worldShS);
		obj1->setObstacle(false);
		GET_SHAPE(obj4, EllipseData, worldShS);
		GET_SHAPE(target4, PointData, worldShS);
		obj4->setObstacle(false);
		pilotreq.objectShape = obj4;
		pilotreq.targetShape = target4;
	}
	$nodeclass Push5 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(obj1, EllipseData, worldShS);
		obj1->setObstacle(false);
		//GET_SHAPE(obj4, EllipseData, worldShS);
		//obj4->setObstacle(false);
		GET_SHAPE(obj5, EllipseData, worldShS);
		GET_SHAPE(target5, PointData, worldShS);
		obj5->setObstacle(false);
		pilotreq.objectShape = obj5;
		pilotreq.targetShape = target5;
	}
	$setupmachine{
		  rundemo: Push1 =PILOT=> Push2 =PILOT=> Push3 =PILOT=> Push4 =PILOT=> Push5 =PILOT=> SpeechNode("done")
	}	

}
REGISTER_BEHAVIOR(Lab8Q2);