#include "Behaviors/StateMachine.h"

$nodeclass Lab8 {

	static int boxY[];
	enum DirectionToGo { LEFT, RIGHT };

  $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");
		// We set the cylinder destination manually, so we don't need to detect the boxes
		// 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 {
  	cout << "MoveOneCylinder start" << endl;
  	NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(worldShS));
    NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));

		if ( cyls.empty() ) {
			cancelThisRequest();
			return;
		}

		DualCoding::Shape<CylinderData> closestCylinder;
		DirectionToGo minDirection;
		double minDistanceToGoal = std::numeric_limits<double>::infinity();

		int numCylindersInGoal = 0;

		SHAPEVEC_ITERATE(cyls, CylinderData, c) {
			// If it's already in the goal
			if (fabs(c->getCentroid().coordY()) > 1000) {
				numCylindersInGoal++;
				continue;
			}

			double minDistance = std::numeric_limits<double>::infinity();
			DualCoding::Shape<AprilTagData> minTag;
			SHAPEVEC_ITERATE(tags, AprilTagData, tag) {
				double curDistance = tag->getCentroid().distanceFrom(c->getCentroid());
				if (curDistance < minDistance) {
					minDistance = curDistance;
					minTag = tag;
				}
			} END_ITERATE;

			// Probably isn't the tag of this cylinder.
			if (minDistance > 400)
				continue;

			DirectionToGo curDirection;
			if (minTag->getTagID() == 10)
				curDirection = LEFT;
			else if (minTag->getTagID() == 11)
				curDirection = RIGHT;
			else
				continue;

			double curDistance = c->getCentroid().distanceFrom(Point(0, Lab8::boxY[curDirection], 0, allocentric));

			if (curDistance < minDistanceToGoal) {
				minDistanceToGoal = curDistance;
				minDirection = curDirection;
				closestCylinder = c;
			}

		} END_ITERATE;

		// Couldn't find a valid cylinder...either they're all in the goal OR
		// couldn't find an AprilTag.
		if (!closestCylinder.isValid()) {
			cancelThisRequest();
			return;
		}

		graspreq.object = closestCylinder;
		NEW_SHAPE(destination, PointData, 
							new PointData(worldShS, Point(0,Lab8::boxY[minDirection],0,allocentric))); // center of arena
		if (minDirection == LEFT) {
			Lab8::boxY[minDirection] -= 250;
		} else if (minDirection == RIGHT) {
			Lab8::boxY[minDirection] += 250;
		}
		destination->setObstacle(false);
		graspreq.targetLocation = destination;
		cout << "MoveOneCylinder end" << endl;
	}

	$nodeclass MoveToCenter : PilotNode(PilotTypes::goToShape) : doStart {
		NEW_SHAPE(destination, PointData, 
												new PointData(worldShS, Point(0,0,0,allocentric))); // center of arena
		destination->setObstacle(false);
		pilotreq.targetShape = destination;
	}

  $setupmachine{
    startnode: BuildWorld =N=> find

  	find: ParkArm =C=> FindStuff =C=> findAndMove
  	findAndMove: MoveOneCylinder =C=> MoveToCenter =C=> find
  	findAndMove =F=> SpeechNode("All detected cylinders are in the goal.")
  }

}

int Lab8::boxY[] = { 1300, -1300 };

REGISTER_BEHAVIOR(Lab8);


//box right center 0 -1250
//box left center 0 1250