#include "Behaviors/StateMachine.h"

$nodeclass SortLab {
  $provide Point westTL;
  $provide Point westBR;
  $provide Point eastTL;
  $provide Point eastBR;


  $nodeclass BuildWorld : VisualRoutinesStateNode : doStart {
                //Declare the goal bounds
		$reference SortLab::westTL;
		$reference SortLab::westBR;
		$reference SortLab::eastTL;
	        $reference SortLab::eastBR;

                eastBR = Point(-250,1500);
                eastTL = Point(250,1000);
                westTL = Point(250,-1500);
                westBR = Point(-250,-1000);
		//
		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;
  }


  //returns whether a cylinder is bounded by a goal
  struct CylBounded : public UnaryShapeRootPred {
	westTL = Point(0,0);

	bool operator() (const ShapeRoot &shape) const {
		return true;
	}
  };

  // Requires: Assumes any cylinder in a goal belongs in that goal
  $nodeclass MoveOneCylinder : GrasperNode(GrasperRequest::moveTo) : doStart {
    		NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));
		//no cylinders on map
		if ( cyls.empty() ) {
			cancelThisRequest();
			return;
		}
		//tag = 10 -> west
		//tag = 11 -> east
		ShapeRoot nearest = cyls[0]; // the nearest cylinder
		//make list of cylinders not already in goal
		NEW_SHAPEVEC(remainingCyls, CylinderData, subset(cyls, CylBounded()));
		//all cylinders in goal
		if ( remainingCyls.empty() ) {
			cancelThisRequest();
			return;
		}
	

		graspreq.object = cyls[0];
		NEW_SHAPE(destination, PointData, 
							new PointData(worldShS, Point(0,0,0,allocentric))); // center of arena
		destination->setObstacle(false);
		graspreq.targetLocation = destination;
  }


  virtual void setup() { 
	  $setupmachine{
	    BuildWorld =N=> ParkArm =C=> find

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

REGISTER_BEHAVIOR(SortLab);