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

$nodeclass PushCanister1 : PilotDemo {

	virtual void buildMap() {
		std::vector<DualCoding::Point> boxpts;

		// Tell robot about box
		float boxSide = 750;  // millimeters
		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);

		// Tell robot about obstacles
		NEW_SHAPE(obstacle1, EllipseData,
			new EllipseData(worldShS, Point(1300,1200,0,allocentric), 125, 100));
		obstacle1->setColor("green");

		NEW_SHAPE(obstacle2, EllipseData,
			new EllipseData(worldShS, Point(650,-350,0,allocentric), 125, 100));
		obstacle2->setColor("green");

		NEW_SHAPE(obstacle3, EllipseData,
			new EllipseData(worldShS, Point(400,-600,0,allocentric), 125, 100));
		obstacle3->setColor("green");
 
		// Tell robot about canisters
		NEW_SHAPE(object1, PointData,
			new PointData(worldShS, Point(350,250,0,allocentric)));
		object1->setObstacle(false);

		NEW_SHAPE(object2, PointData,
			new PointData(worldShS, Point(1500,-200,0,allocentric)));
		object2->setObstacle(false);

		NEW_SHAPE(object3, PointData,
			new PointData(worldShS, Point(700,800,0,allocentric)));
		object3->setObstacle(false);
	
		NEW_SHAPE(object4, PointData,
			new PointData(worldShS, Point(800,-850,0,allocentric)));
		object4->setObstacle(false);

		NEW_SHAPE(object5, PointData,
			new PointData(worldShS, Point(1300,500,0,allocentric)));
		object5->setObstacle(false);
	}	

	$nodeclass PushIt1 : PilotNode(PilotTypes::pushObject) {

		virtual void doStart() {
			GET_SHAPE(object1, PointData, worldShS);
			GET_SHAPE(box, PolygonData, worldShS);
			pilotreq.objectShape = object1;
			pilotreq.targetShape = box;
			worldShS.deleteShape(object1);
		}

		virtual void doStop() {
			NEW_SHAPE(newLoc1, PointData,
					new PointData(worldShS, theAgent->getCentroid()));
		}

	}

	$nodeclass PushIt2 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object2, PointData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
		pilotreq.objectShape = object2;
		pilotreq.targetShape = box;
		worldShS.deleteShape(object2);
		//NEW_SHAPE(newLoc2, PointData, new PointData(worldShS,theAgent->getCentroid()));
	}

	$nodeclass PushIt3 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object3, PointData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
		pilotreq.objectShape = object3;
		pilotreq.targetShape = box;
		worldShS.deleteShape(object3);
		//NEW_SHAPE(newLoc3, PointData, new PointData(worldShS,theAgent->getCentroid()));
	}

	$nodeclass PushIt4 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object4, PointData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
		pilotreq.objectShape = object4;
		pilotreq.targetShape = box;
		worldShS.deleteShape(object4);
		//NEW_SHAPE(newLoc4, PointData, new PointData(worldShS,theAgent->getCentroid()));
	}

	$nodeclass PushIt5 : PilotNode(PilotTypes::pushObject) : doStart {
		GET_SHAPE(object5, PointData, worldShS);
		GET_SHAPE(box, PolygonData, worldShS);
		pilotreq.objectShape = object5;
		pilotreq.targetShape = box;
		worldShS.deleteShape(object5);
		//NEW_SHAPE(newLoc5, PointData, new PointData(worldShS,theAgent->getCentroid()));
	}

	$setupmachine {
  		startdemo: PushIt1 =C=> PushIt2 =C=> PushIt3 =C=> PushIt4 =C=> PushIt5 
			=C=> SpeechNode("done")
 	}
}

REGISTER_BEHAVIOR(PushCanister1);