#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));
		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->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 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) ));
		 //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(16, 5);
	      	pilotreq.landmarkExtractor = lex;
	}		
	

		$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;
	}
	//***********************************************************************
	// 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;
	}
	
	
	//****
	// LocalWalker
	// Walk using local map --incomplete
	//****
	$nodeclass LocalWalker : StateNode{
		$nodeclass LocalBuilder : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
			
		}
		virtual void setup(){
		}	
	}
	
	//****
	// Localizer
	// A test class for localization with color markers; using blob; using global map
	//****
	$nodeclass Localizer : PilotNode( PilotTypes::localize) : doStart{
		pilotreq.landmarkExtractor = new MapBuilderRequest(MapBuilderRequest::localMap);

		pilotreq.landmarkExtractor->addObjectColor(blobDataType, "red");
		pilotreq.landmarkExtractor->addObjectColor(blobDataType, "blue");
		NEW_SHAPE(redBlob1, BlobData, new BlobData(worldShS, Point(2345, 400, 0, allocentric), Point(2345, 350, 0, allocentric), Point(2295, 400, 0, allocentric), Point(2295, 350, 0, allocentric)));
		redBlob1->setColor("red");
		NEW_SHAPE(redBlob2, BlobData, new BlobData(worldShS, Point(2345, -350, 0, allocentric), Point(2345, -400, 0, allocentric), Point(2295, -350, 0, allocentric), Point(2295, -400, 0, allocentric)));
		redBlob2->setColor("red");
		NEW_SHAPE(blueBlob1, BlobData, new BlobData(worldShS, Point(1315, 375, 0, allocentric), Point(1315, 325, 0, allocentric), Point(1265, 375, 0, allocentric), Point(1265, 325, 0, allocentric)));
		blueBlob1->setColor("blue");
		NEW_SHAPE(blueBlob2, BlobData, new BlobData(worldShS, Point(1315, -325, 0, allocentric), Point(1315, -375, 0, allocentric), Point(1265, -325, 0, allocentric), Point(1265, -375, 0, allocentric)));
		blueBlob2->setColor("blue");
		NEW_SHAPE(greenBlob1, BlobData, new BlobData(worldShS, Point(1265, 1275, 0, allocentric), Point(1265, 1225, 0, allocentric), Point(1215, 1275, 0, allocentric), Point(1215, 1225, 0, allocentric)));
		greenBlob1->setColor("red");//dummy green marker1
		NEW_SHAPE(greenBlob2, BlobData, new BlobData(worldShS, Point(765, 1275, 0, allocentric), Point(765, 1225, 0, allocentric), Point(715, 1275, 0, allocentric), Point(715, 1225, 0, allocentric)));
		greenBlob2->setColor("blue");//dummy green marker2
		pilotreq.landmarks.push_back(redBlob1);
		pilotreq.landmarks.push_back(redBlob2);
		pilotreq.landmarks.push_back(blueBlob1);
		pilotreq.landmarks.push_back(blueBlob2);		
		pilotreq.landmarks.push_back(greenBlob1);		
		pilotreq.landmarks.push_back(greenBlob2);		
		
		//try to add some aprilTags
		GET_SHAPE(tag18, AprilTagData, worldShS);
		GET_SHAPE(tag7, AprilTagData, worldShS);
		pilotreq.landmarks.push_back(tag18);
		pilotreq.landmarks.push_back(tag7);				
		//pilotreq.landmarkExtractor->addObjectColor(ellipseDataType, "red");
		//pilotreq.landmarkExtractor->addObjectColor(ellipseDataType, "blue");
		//NEW_SHAPE(redSph1, EllipseData, new EllipseData(worldShS, Point(2320, 375, 0, allocentric), 30, 30));
		//redSph1->setColor("red");
		//NEW_SHAPE(redSph2, EllipseData, new EllipseData(worldShS, Point(2320, -375, 0, allocentric), 30, 30));
		//redSph1->setColor("red");
		//NEW_SHAPE(blueSph1, EllipseData, new EllipseData(worldShS, Point(1290, 350, 0, allocentric), 30, 30));
		//blueSph1->setColor("blue");
		//NEW_SHAPE(blueSph2, EllipseData, new EllipseData(worldShS, Point(1290, -350, 0, allocentric), 20, 20));
		//blueSph1->setColor("blue");
		//pilotreq.landmarks.push_back(redSph1);
		//pilotreq.landmarks.push_back(redSph2);
		//pilotreq.landmarks.push_back(blueSph1);
		//pilotreq.landmarks.push_back(blueSph2);
	}
	//****
	// TestMapper
	// A test map builder
	//****
	$nodeclass TestMapper : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
      		mapreq.addObjectColor(blobDataType, "red");
        	//mapreq.addBlobOrientation("red",BlobData::pillar);
        	mapreq.addObjectColor(blobDataType, "blue");
        	//mapreq.addBlobOrientation("blue",BlobData::pillar);
    	}
	
	//****
	// SiftTrainer
	// To train the sift snack finder
	//****
	$nodeclass SiftTrainer : VisualRoutinesStateNode {
		virtual void doStart(){
			mapBuilder->newSiftMatcher("snack.dat");
			mapBuilder->trainSiftObject("snack.dat", "biscuit");
    			mapBuilder->saveSiftDatabase("snack.dat");
		}
	}

	
	//****
	// SiftFinder
	// To find snacks with trained Sift model
	//****
	$nodeclass SiftFinder : VisualRoutinesStateNode {
		$nodeclass Looker : MapBuilderNode : doStart {
    			mapreq.siftDatabasePath = "snack.dat";
  		}

  		$nodeclass Display : VisualRoutinesStateNode : doStart {
    			NEW_SHAPEVEC(siftobjects, SiftData, select_type<SiftData>(camShS));
    			if ( siftobjects.size() > 0 )
      				siftobjects[0]->displayMatchedFeatures();
  		}
		
		virtual void setup(){
    			$statemachine{
      				Looker =MAP=> Display =C=> SpeechNode("Finding done!")
    			}
		}
 
	}
	//****
	// doStart
	// starting method called before setupmachine
	//****
	virtual void doStart(){
		//pilot->computeParticleBounds(100,100);
		//particleFilter->resetFilter();
		//particleFilter->displayIndividualParticles(100);
	}
	
	//***********************************************************************
	// Top level state machine for executing all lower level tasks
	//***********************************************************************
	$setupmachine {
		startdemo: SpeechNode("start") =C=> wait
		wait: StateNode
		wait =TM("self")=> Localizer =PILOT=> wait
		wait =TM("test")=> TestMapper =PILOT=> wait
		wait =TM("train")=> SiftTrainer =C=> wait
		wait =TM("april")=> Loc =C=> wait
		wait =TM("find")=> SiftFinder =C=> wait
		//startdemo: ParkArm =C=> Turn(M_PI/4) =C=> SnackLoc 
		//=C=> CobotToSnack =C=> StateNode =T(1000)=> SnackToCobot =C=> SnackLoc
	}
}
REGISTER_BEHAVIOR(SnackRobot);