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

$nodeclass SnackRobot : PilotDemo {

	//***********************************************************************
	// buildMap
	// Create world representation for robot when behavior starts up
	//***********************************************************************
	virtual void buildMap() {

		 // The points which form the playpen polygon
		    std::vector<Point> penPoints;		
		    penPoints.push_back(Point(-375, -375, 0, allocentric));
		    penPoints.push_back(Point(2250, -375, 0, allocentric));
		    penPoints.push_back(Point(2250, 375, 0, allocentric));
		    penPoints.push_back(Point(1250, 375, 0, allocentric));				
		    penPoints.push_back(Point(1250, 1250, 0, allocentric));
		    penPoints.push_back(Point(750, 1250, 0, allocentric));
		    penPoints.push_back(Point(750, 375, 0, allocentric));
		    penPoints.push_back(Point(375, 375, 0, allocentric));
		    penPoints.push_back(Point(375, 1250, 0, allocentric));
		    penPoints.push_back(Point(-375, 1250, 0, allocentric));
		    NEW_SHAPE(worldBounds, PolygonData, new PolygonData(worldShS, penPoints, true));
	    	    worldBounds->setObstacle(true);

		//Cobot object
		NEW_SHAPE(cobot, EllipseData, new EllipseData(worldShS, 
			Point(780/2-390, 800+305/2-400, 500/2,allocentric), 60, 60));
		cobot->setColor("black");

		 //April tag locations
		    NEW_SHAPE(tag18, AprilTagData,
			      new AprilTagData(worldShS, Point(1000, 1280, 325, allocentric),
					       AprilTags::TagDetection(18)));
		    NEW_SHAPE(tag2, AprilTagData,
			      new AprilTagData(worldShS, Point(1000, -390, 325, allocentric),
					       AprilTags::TagDetection(2)));
		    NEW_SHAPE(tag12, AprilTagData,
			      new AprilTagData(worldShS, Point(550,  375, 375, allocentric),
					       AprilTags::TagDetection(12)));
		    NEW_SHAPE(tag3, AprilTagData,
			      new AprilTagData(worldShS, Point(-250, -390, 325, allocentric),
					       AprilTags::TagDetection(3)));
		    NEW_SHAPE(tag7, AprilTagData,
			      new AprilTagData(worldShS, Point(2320, 0, 325, allocentric),
					       AprilTags::TagDetection(7)));
		    NEW_SHAPE(tag4, AprilTagData,
			      new AprilTagData(worldShS, Point(-375, 0, 325, allocentric),
					       AprilTags::TagDetection(4)));
		    NEW_SHAPE(tag17, AprilTagData,
			      new AprilTagData(worldShS, Point(1450, 375, 325, allocentric),
					       AprilTags::TagDetection(17)));
		    NEW_SHAPE(tag1, AprilTagData,
			      new AprilTagData(worldShS, Point(2000, -390, 325, allocentric),
					       AprilTags::TagDetection(1)));

		// Point in front of the Cobot and point in front of the snacks
		NEW_SHAPE(snackPoint, PointData,
			new PointData(worldShS, Point(900, 0, 0, allocentric)));
		snackPoint->setObstacle(false);

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


	//***********************************************************************
	// ParkArm
	// Put robot's arm in correct position for travelling
	//***********************************************************************
	$nodeclass ParkArm : StateNode {
		$nodeclass RelaxArm : StateNode : doStart {	
			SharedObject<PIDMC> pid;
			MC_ID pidMCID = motman->addPersistentMotion(pid,MotionManager::kHighPriority);

			MMAccessor<PIDMC> relaxer(pidMCID);
			for (unsigned int joint = 0; joint < NumArmJoints; joint++)
				relaxer->setJointPowerLevel(ArmOffset + joint, 0);
			processEvent(EventBase(EventBase::timerEGID,1,EventBase::statusETID,0));
			erouter->addTimer(this,0,500);
			postParentCompletion();
		}

		$setupmachine {
			DynamicMotionSequenceNode("parkArm.mot") =C=> RelaxArm
		}
	}

	//***********************************************************************
	// SnackLoc
	// Use AprilTags to localize the robot on world map
	//***********************************************************************
	$nodeclass SnackLoc : PilotNode(PilotTypes::localize) : doStart {
		GET_SHAPE(tag18, AprilTagData, worldShS);	
		GET_SHAPE(tag2, AprilTagData, worldShS);	
		GET_SHAPE(tag12, AprilTagData, worldShS);	
		GET_SHAPE(tag3, AprilTagData, worldShS);	
		GET_SHAPE(tag7, AprilTagData, worldShS);	
		GET_SHAPE(tag4, AprilTagData, worldShS);	
		GET_SHAPE(tag1, AprilTagData, worldShS);
		GET_SHAPE(tag17, AprilTagData, worldShS);

		pilotreq.landmarks.clear();
      		pilotreq.landmarks.push_back(tag18);
      		pilotreq.landmarks.push_back(tag2);
      		pilotreq.landmarks.push_back(tag12);
      		pilotreq.landmarks.push_back(tag3);
      		pilotreq.landmarks.push_back(tag7);
     		pilotreq.landmarks.push_back(tag4);
     		pilotreq.landmarks.push_back(tag17);
      		pilotreq.landmarks.push_back(tag1);

	      	MapBuilderRequest* lex = new MapBuilderRequest(MapBuilderRequest::localMap);
	      	lex->setAprilTagFamily();
	      	pilotreq.landmarkExtractor = lex;
	}		
	
	//***********************************************************************
	// CobotToSnack
	// Use AprilTags to localize the robot on world map
	//***********************************************************************
	$nodeclass CobotToSnack : PilotNode(PilotTypes::goToShape) : doStart {
		GET_SHAPE(snackPoint, PointData, worldShS);
		pilotreq.targetShape = snackPoint;
		pilotreq.targetHeading = -M_PI/2;
	}

	//***********************************************************************
	// SnackToCobot
	// Use AprilTags to localize the robot on world map
	//***********************************************************************
	$nodeclass SnackToCobot : PilotNode(PilotTypes::goToShape) : doStart {
		GET_SHAPE(cobotPoint, PointData, worldShS);
		pilotreq.targetShape = cobotPoint;
		pilotreq.targetHeading = M_PI/2;
	}

	//***********************************************************************
	// Top level state machine for executing all lower level tasks
	//***********************************************************************
	$setupmachine {
		startdemo: ParkArm =C=> Turn(M_PI/4) =C=> SnackLoc 
		//=C=> CobotToSnack =C=> StateNode =T(1000)=> SnackToCobot =C=> SnackLoc
	}
}
REGISTER_BEHAVIOR(SnackRobot);