#include "Behaviors/StateMachine.h"

$nodeclass SortLab {
	$provide int numWest(0);
	$provide int numEast(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;
  }


  /*
  returns whether a cylinder is bounded by a goal.
  The goal points are hardcoded. Not the prettiest thing, but I used a functor!
  */
  struct CylBounded : public UnaryShapeRootPred {
	public: 
		CylBounded() {
			eastBR = Point(-250,-1500,0, allocentric);
                 	eastTL = Point(250,-1000,0, allocentric);
                 	westTL = Point(250,1500,0, allocentric);
                 	westBR = Point(-250,1000,0, allocentric);
		}

		bool operator() (const ShapeRoot &shape) const {
			Point center = shape->getCentroid();
			center.setRefFrameType(allocentric);
			//Contained by east?
			if(center.isLeftOf(eastBR) && center.isRightOf(eastTL)) {
				if(center.isAbove(eastBR) && center.isBelow(eastTL)) {
					return false;
				}
			}
			//Contained by west?
			if(center.isLeftOf(westBR) && center.isRightOf(westTL)) {
				if(center.isAbove(westBR) && center.isBelow(westTL)) {
					return false;
				}
			}
			return true;
		}


	private:
                Point eastBR; 
                Point eastTL; 
                Point westTL; 
                Point westBR;

  };



  //given a cylinder and tag list, return tag closest to cylinder
  static DualCoding::Shape<AprilTagData> tagPair(const ShapeRoot &shape, std::vector< DualCoding::Shape <AprilTagData> >& tags) {
	Point center = shape->getCentroid();
	Shape<AprilTagData> curTag = tags[0];
	float curDistance = curTag->getCentroid().xyDistanceFrom(center);
	SHAPEVEC_ITERATE(tags, AprilTagData, mytag)
		Point myCenter = mytag->getCentroid();
		float myDistance = myCenter.xyDistanceFrom(center);
		if (myDistance < curDistance) {
			curTag = mytag;
			curDistance = myDistance;
		}
	END_ITERATE;
	return curTag;
 }

  // Requires: Assumes any cylinder in a goal belongs in that goal
  $nodeclass MoveWest : GrasperNode(GrasperRequest::moveTo) : doStart {
    		NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));
		NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(worldShS));	
		$reference SortLab::numWest;

		std::cout << "^^^In west: " << numWest << std::endl;

		NEW_SHAPE(westDestination, PointData, new PointData(worldShS, Point(0,1300 - (140)*numWest,0,allocentric)));  
		westDestination->setObstacle(false);

		//no cylinders on map
		if ( cyls.empty() ) {
			cancelThisRequest();
			return;
		}

		//make list of cylinders not already in goal
		NEW_SHAPEVEC(remainingCyls, CylinderData, subset(cyls, CylBounded()));

		//remove cylinders not with west target
		std::vector<DualCoding::Shape <CylinderData> > westCyls;
		SHAPEVEC_ITERATE(remainingCyls, CylinderData, curCyl) 
			DualCoding::Shape<AprilTagData> tag = tagPair(curCyl, tags);
			int tagDistance = tag->getCentroid().xyDistanceFrom(curCyl->getCentroid());
			if ( (tag->getTagID()==10) && (tagDistance < 150) ) {
				westCyls.push_back(curCyl);
			}
		END_ITERATE;


		//all target  in goal, nothing left to do
		if (westCyls.empty()) {
			std::cout << "^^^No remaining target Cylinders." << std::endl;
			cancelThisRequest();
			return;
		}

		std::cout << westCyls.size() << "^^^  " << "cyls remaining for west" << std::endl;

		//Select west cylinder nearest its destination
		ShapeRoot targetCyl = westCyls[0];
		Point destination = westDestination->getCentroid();
		SHAPEVEC_ITERATE(westCyls, CylinderData, curCyl) 
			int curBestDistance = targetCyl->getCentroid().xyDistanceFrom(destination);
			int curDistance = curCyl->getCentroid().xyDistanceFrom(destination);
			if (curDistance < curBestDistance) {
				targetCyl = curCyl;
			}			
		END_ITERATE;

		numWest++;
		graspreq.object = targetCyl;
		graspreq.targetLocation = westDestination;
  }

  // Requires: Assumes any cylinder in a goal belongs in that goal
  $nodeclass MoveEast : GrasperNode(GrasperRequest::moveTo) : doStart {
    		NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));
		NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(worldShS));	
		$reference SortLab::numEast;

		std::cout << "^^^In East: " << numEast << std::endl;

		NEW_SHAPE(eastDestination, PointData, new PointData(worldShS, Point(0,-1300 + (140)*numEast,0,allocentric)));   
		eastDestination->setObstacle(false);

		//no cylinders on map
		if ( cyls.empty() ) {
			cancelThisRequest();
			return;
		}

		//make list of cylinders not already in goal
		NEW_SHAPEVEC(remainingCyls, CylinderData, subset(cyls, CylBounded()));

		//remove cylinders not with west target
		std::vector<DualCoding::Shape <CylinderData> > eastCyls;
		SHAPEVEC_ITERATE(remainingCyls, CylinderData, curCyl) 
			DualCoding::Shape<AprilTagData> tag = tagPair(curCyl, tags);
			int tagDistance = tag->getCentroid().xyDistanceFrom(curCyl->getCentroid());
			if ( (tag->getTagID()==11) || (tagDistance >= 150) ) {
				eastCyls.push_back(curCyl);
			}
		END_ITERATE;


		//all target  in goal, nothing left to do
		if (eastCyls.empty()) {
			std::cout << "^^^No remaining target Cylinders." << std::endl;
			cancelThisRequest();
			return;
		}

		std::cout << "^^^  " << eastCyls.size() << " cyls remaining for east" << std::endl;

		//Select west cylinder nearest its destination
		ShapeRoot targetCyl = eastCyls[0];
		Point destination = eastDestination->getCentroid();
		SHAPEVEC_ITERATE(eastCyls, CylinderData, curCyl) 
			int curBestDistance = targetCyl->getCentroid().xyDistanceFrom(destination);
			int curDistance = curCyl->getCentroid().xyDistanceFrom(destination);
			if (curDistance < curBestDistance) {
				targetCyl = curCyl;
			}			
		END_ITERATE;

		numEast++;
		graspreq.object = targetCyl;
		graspreq.targetLocation = eastDestination;
  }

  $setupmachine{
        BuildWorld =N=> ParkArm =C=> find
  	find: WalkForward(-180) =C=> FindStuff =C=>  MoveWest =C=> MoveEast =C=> MoveWest =C=> MoveEast
  }
}

REGISTER_BEHAVIOR(SortLab);