#include "Behaviors/StateMachine.h"

// Sample code for 2014 ARTSI Robotics Competition at Tapia.
// Pick up a cylinder and drop it in the center of the arena.

$nodeclass SortCylindersBehavior {
	
	$provide int cylindersMoved(3);
	$provide int numberExecuted(0);
	
  $nodeclass BuildWorld : VisualRoutinesStateNode : doStart {
		std::vector<Point> wallPts;
		wallPts.push_back(Point(-750, 1500,0,allocentric));
		wallPts.push_back(Point(-750,-1500,0,allocentric));
		wallPts.push_back(Point( 750,-1500,0,allocentric));
		wallPts.push_back(Point( 750, 1500,0,allocentric));
		NEW_SHAPE(worldBounds, PolygonData,
							new PolygonData(worldShS, wallPts, true));
		worldBounds->setObstacle(true);

		int const tagHeight = 8 * 25.4;
		MapBuilderRequest::defaultMarkerHeight = tagHeight;
		NEW_SHAPE(tagNorth, AprilTagData,
							new AprilTagData(worldShS, AprilTags::TagDetection(1),
															 Point( 750, 0,tagHeight,allocentric),
															 fmat::Quaternion::aboutZ(0)));
		NEW_SHAPE(tagSouth, AprilTagData,
							new AprilTagData(worldShS, AprilTags::TagDetection(3),
															 Point(-750, 0,tagHeight,allocentric),
															 fmat::Quaternion::aboutZ(0)));
		NEW_SHAPE(tagEast, AprilTagData,
							new AprilTagData(worldShS, AprilTags::TagDetection(2),
															 Point(0,-1500,tagHeight,allocentric),
															 fmat::Quaternion::aboutZ(M_PI)));
		NEW_SHAPE(tagWest, AprilTagData,
							new AprilTagData(worldShS, AprilTags::TagDetection(4),
															 Point(0, 1500,tagHeight,allocentric),
															 fmat::Quaternion::aboutZ(0)));
		tagNorth->setLandmark(true);
		tagSouth->setLandmark(true);
		tagEast->setLandmark(true);
		tagWest->setLandmark(true);
	}

  $nodeclass FindStuff : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		mapreq.addObjectColor(cylinderDataType, "red");
		mapreq.addObjectColor(polygonDataType, "blue");
		mapreq.addOccluderColor(polygonDataType, "red");
		mapreq.addMinBlobArea("red",1000);
		mapreq.setAprilTagFamily();
		mapreq.searchArea = Lookout::groundSearchPoints();
		mapreq.pursueShapes = true;
		mapreq.clearVerbosity = MapBuilder::MBVshapeMatch;
  }

  $nodeclass MoveOneCylinder : GrasperNode(GrasperRequest::moveTo) : doStart {
	$reference SortCylindersBehavior::cylindersMoved;
	$reference SortCylindersBehavior::numberExecuted;
	
  NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));
	NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(worldShS));
	NEW_SHAPEVEC(blueLines, PolygonData, select_type<PolygonData>(worldShS));
		
	if ( cyls.empty() || tags.empty() || blueLines.empty()) {
		cancelThisRequest();
		return;
	}

	if (cyls.size() < numberExecuted)
	{
		cancelThisRequest();
		return;
	}

	SHAPEVEC_ITERATE(blueLines, PolygonData, currentPoly){
		currentPoly->setObstacle(false);
	} END_ITERATE;


	//Find the tags whose ID's represent the tops of cylinders that need to be moved
		
			//If the tag represents a cylinder to be moved, find which cylinder:
			int i = 0;
			int closest = i;
			float closestdist = FLT_MAX;
			SHAPEVEC_ITERATE(tags, AprilTagData, currentTag) {
				if(currentTag->getTagID() == 10 || currentTag->getTagID() == 11)
				{
					cout << "T" << currentTag->getCentroid() << "\tC" << cyls[cylindersMoved]->getCentroid() << endl;
					float currentdist = cyls[cylindersMoved]->getCentroid().xyDistanceFrom(currentTag->getCentroid());;
					if (currentdist < closestdist)
					{
						closest = i;
						closestdist = currentdist;
					}
					cout << currentdist << endl;
				}
				i++;
			} END_ITERATE;
			graspreq.object = cyls[cylindersMoved];
			cout << closest << endl;
			cout << closestdist << endl;
					
			if(tags[closest]->getTagID() == 10 && cylindersMoved != 3){ //Needed due to robot not picking up #3's AprilTag
				NEW_SHAPE(destinationW, PointData, new PointData(worldShS, Point(100-(cylindersMoved % 2)*200,1200,0,allocentric)));
				cout << "east" << endl;
				destinationW->setObstacle(false);
				graspreq.targetLocation = destinationW;
			} else {
				NEW_SHAPE(destinationE, PointData, new PointData(worldShS, Point(100-(cylindersMoved % 2)*200,-1200,0,allocentric)));
				cout << "west" << endl;
				destinationE->setObstacle(false);
				graspreq.targetLocation = destinationE;
			}
					
			//"Mark" that we've used this cylinder/tag combo:
			//currentTag = null;
			cylindersMoved++;
			cylindersMoved %=3;
			numberExecuted++;
			
			return;
			
		
	//If no matching cylinders were found, we did not complete correctly
	if(! (graspreq.object.isValid() ) ){
		cancelThisRequest();
		return;
	}
	

//BEGIN HARDCODE
//	graspreq.object = cyls[cylindersMoved];
//	NEW_SHAPE(destination, PointData, new PointData(worldShS,Point(100-(cylindersMoved % 2)*200,((cylindersMoved == 0 || cylindersMoved == 3)? -1200 : 1200),0,allocentric) )); // define where to place the cylinder
//	destination->setObstacle(false);
	
//NEW_SHAPE(linePath, LineData, new LineData(worldShS, Point(100-(cylindersMoved % 2)*200,((cylindersMoved == 0 || cylindersMoved == 3)? -1200 : 1200),0,allocentric) , Point(0,0,0, allocentric))); // traces a line from the origin to the place where we want to place the cylidner
//	linePath->setObstacle(false);
		
//	graspreq.targetLocation = destination;
//	cylindersMoved++;
//	cylindersMoved %=3;
//END HARDCODE
  }

  $setupmachine{
    BuildWorld =N=> ParkArm =C=> find

  	find: FindStuff =C=> MoveOneCylinder =C=> FindStuff =C=> MoveOneCylinder =C=> MoveOneCylinder =C=> MoveOneCylinder =C=> SpeechNode("Done!")
	//move: MoveOneCylinder
  }

}

REGISTER_BEHAVIOR(SortCylindersBehavior);