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

$nodeclass SnackBotC : PilotDemo {

	virtual void buildMap() {
		//April Tags
		NEW_SHAPE(tag1, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750-294-15,0-400+30/2+1,127.0+200/2), AprilTags::TagDetection(1) ));
		NEW_SHAPE(tag3, AprilTagData,new AprilTagData(worldShS, Point(0-390+147+15,0-400+30/2+1,127.0+200/2), AprilTags::TagDetection(3) ));
		NEW_SHAPE(tag2, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750/2,0-400+30/2+1,127.0+200/2), AprilTags::TagDetection(2) ));
		NEW_SHAPE(tag0, AprilTagData,new AprilTagData(worldShS, Point(0-390+2750-15-0.5,0-400+367,127.0+200/2), AprilTags::TagDetection(0) ));
		NEW_SHAPE(tag4, AprilTagData,new AprilTagData(worldShS, Point(0-390+15+0.5,0-400+367,127.0+200/2), AprilTags::TagDetection(4) ));
		NEW_SHAPE(tag12, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300/2,800-400-15,127.0+200/2), AprilTags::TagDetection(12) ));
		NEW_SHAPE(tag17, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300+600+390/2,800-400-15,127.0+200/2), AprilTags::TagDetection(17) ));
		NEW_SHAPE(tag18, AprilTagData,new AprilTagData(worldShS, Point(0-390+780+300+600/2,1700-400-15+80,127.0+200/2), AprilTags::TagDetection(18) ));

		//Maze
		std::vector<Point> E;		
		E.push_back(Point(2750-390, 0-400, 0));
		E.push_back(Point(2750-390, 800-400, 0));
		E.push_back(Point(780+300+600-390, 800-400, 0));
		E.push_back(Point(780+300+600-390, 800+980-400, 0));
		E.push_back(Point(780+300-390, 800+980-400, 0));
		E.push_back(Point(780+300-390, 800-400, 0));
		E.push_back(Point(780-390, 800-400, 0));
		E.push_back(Point(780-390, 1700-400, 0));
		E.push_back(Point(0-390, 1700-400, 0));
		E.push_back(Point(0-390, 0-400, 0));
		NEW_SHAPE(worldBounds, PolygonData, new PolygonData(worldShS, E, true));
		worldBounds->setColor("blue");

		// Virtual points
		NEW_SHAPE(snacks, PointData, new PointData(worldShS, Point(0-390+780+300+600/2,-100,0)));
		snacks->setObstacle(false);
		NEW_SHAPE(cobot, PointData, new PointData(worldShS, Point(0,-100,0)));
		cobot->setObstacle(false);
	}

	$nodeclass Loc : PilotNode(PilotTypes::localize) : doStart {
		if (pilotreq.landmarks.size() == 0) {
			cout << "Adding Landmarks" << endl;
			GET_SHAPE(tag0, AprilTagData, worldShS);
			GET_SHAPE(tag1, AprilTagData, worldShS);
			GET_SHAPE(tag2, AprilTagData, worldShS);
			GET_SHAPE(tag3, AprilTagData, worldShS);
			GET_SHAPE(tag4, AprilTagData, worldShS);
			GET_SHAPE(tag12, AprilTagData, worldShS);
			GET_SHAPE(tag17, AprilTagData, worldShS);
			GET_SHAPE(tag18, AprilTagData, worldShS);

			pilotreq.landmarks.push_back(tag0);
			pilotreq.landmarks.push_back(tag1);
			pilotreq.landmarks.push_back(tag2);
			pilotreq.landmarks.push_back(tag3);
			pilotreq.landmarks.push_back(tag4);
			pilotreq.landmarks.push_back(tag12);
			pilotreq.landmarks.push_back(tag17);
			pilotreq.landmarks.push_back(tag18);
		}

		MapBuilderRequest* req = new MapBuilderRequest(MapBuilderRequest::localMap);
		req->setAprilTagFamily(16,5);

		pilotreq.landmarkExtractor = req;
	}

	$nodeclass GoToCobot : PilotNode(PilotTypes::goToShape) : doStart {
		GET_SHAPE(cobot, PointData, worldShS);
		pilotreq.targetShape = cobot;
		pilotreq.collisionAction = collisionIgnore;
	}

	$nodeclass TurnToCobot : PilotNode(PilotTypes::walk) : doStart {
		pilotreq.da = (AngTwoPi(M_PI/2) - theAgent->getOrientation());
	}

	$nodeclass GoToSnacks : PilotNode(PilotTypes::goToShape) : doStart {
		GET_SHAPE(snacks, PointData, worldShS);
		pilotreq.targetShape = snacks;
		pilotreq.collisionAction = collisionIgnore;
	}

	$nodeclass TurnToSnacks : PilotNode(PilotTypes::walk) : doStart {
		pilotreq.da = (AngTwoPi(M_PI/2) - theAgent->getOrientation());
	}

	$nodeclass FindSnack : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
		mapreq.pursueShapes = false;
		mapreq.addObjectColor(blobDataType, "red"); 
		mapreq.addBlobOrientation("red", BlobData::poster, 225); 

		NEW_SHAPE(snack, PointData, new PointData(worldShS, Point(780+300+150-390, 800-350, 225, allocentric)));
		mapreq.searchArea = snack;
	}

	$setupmachine{
  		startdemo: StateNode =N=> wait
		wait: StateNode
		wait =TM("loc")=>Loc =PILOT=>wait
		
		find: Loc =TM("go to cobot")=> goToCobot
		find =TM("go to snacks")=> goToSnacks
		find =TM("find snacks")=> findSnack
		findSnack: FindSnack 
		goToSnacks: GoToSnacks =C=> TurnToSnacks =C=> find
		goToCobot: GoToCobot =C=> TurnToCobot =C=> find
	}
}

REGISTER_BEHAVIOR(SnackBotC);