#include "Behaviors/Demos/Navigation/PilotDemo.h"
#define SIDE_SIZE 400


$nodeclass Explore {
	$provide std::vector<DualCoding::Shape<LineData> > walls;
	$provide int numChoices;
	$provide int targetX;
	$provide int targetY;
	$provide float targetHeading;
	enum choice {moveNow, stopMotion};

	$nodeclass InitWorld : VisualRoutinesStateNode : doStart {
		const float scale = 0.01f;
		const float worldwidth = VRmixin::worldSkS.getWidth();
		const float worldheight = VRmixin::worldSkS.getHeight();
		$reference Explore::numChoices;
		numChoices = 0;
		worldSkS.setTmat(scale, worldwidth/2, worldheight/2);
		NEW_SKETCH(seen, bool, visops::zeros(worldSkS));
	}

	$nodeclass FindStuff : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		mapreq.setAprilTagFamily();
		mapreq.searchArea = Lookout::groundSearchPoints();
		mapreq.pursueShapes = true;
		mapreq.setVerbosity = MapBuilder::MBVdeleteShape;
		//mapreq.clearVerbosity = MapBuilder::MBVshapeMatch;
	}

	$nodeclass DetermineBoxes : VisualRoutinesStateNode : doStart {
		$reference Explore::walls;
		NEW_SHAPEVEC(tags, AprilTagData, select_type<AprilTagData>(worldShS));
		if (tags.empty()){
			return;
		}
		SHAPEVEC_ITERATE(tags, AprilTagData, tag) {
			// TODO IF we happen to repeat seeing apriltags, 
			//	create a hashmap of previously seen apriltags, 
			//	and only iterate over new tags.

			Point tagCenter = tag->getCentroid();
			AngSignPi angle = tag->getQuaternion().ypr()[0];
			float originX = tagCenter.coordX();
			float originY = tagCenter.coordY();

			cout << "Angle: " << angle << " X: " << originX << " Y: " << originY << endl;

			// line segment that corresponds to box side
			float d = SIDE_SIZE/2; //distance
			float dx = sin(angle)*d;
			float dy = (-1)*cos(angle)*d; //y is flipped

			cout << "dx: " << dx << " dy: " << dy << endl;
	
			Point p1 = Point(originX + dx, originY + dy, 0, allocentric);
			Point p2 = Point(originX - dx, originY - dy, 0, allocentric);

			NEW_SHAPE(line, LineData, new LineData(worldShS, p1, p2));
			line->setObstacle(true);
			walls.push_back(line);
		
		} END_ITERATE;
	}	
	
	$nodeclass DrawRays : VisualRoutinesStateNode : doStart {
		$reference Explore::walls;

		cout << "Draw rays started" << endl;
		//This is like an empty SHAPEVEC	
		std::vector<DualCoding::Shape<LineData> > lines;

		Point robot = theAgent->getCentroid();
		float orient = theAgent->getOrientation();
		orient += (M_PI / 2);
				//TODO - Figure out frame of reference properly, pi
				//theAgent->getOrientation();
	
		int length = 1500;	
		float boundary = 1.0;
		float step = 0.02;

		int numSteps = floor(boundary * 2 / step);
		orient -= boundary; 
		
		for (int i = 0; i < numSteps; i++){
			float x = robot.coordX() + length * sin(orient);
			float y = robot.coordY() + length * cos(orient) * (-1); 
			Point sight = Point(x, y, 0, allocentric);
			NEW_SHAPE(newL, LineData, new LineData(worldShS, robot, sight));
			lines.push_back(newL);
			orient += step;
		}


		//Iterate over all lines, find intersections with walls, 
		//and trim down the lines that end before "length".
		SHAPEVEC_ITERATE(lines, LineData, l) {
			for(unsigned int i = 0; i < walls.size(); i++){
				if(walls[i]->intersectsLine(l)){
					l->setEndPts(robot, walls[i]->intersectionWithLine(l));
				}
			}
		} END_ITERATE;
	
		//Iterate over all lines AGAIN, and now make some appropriate polygons.
		std::vector< Point > points;
		points.push_back(robot);
		for(unsigned int i = 0; i < lines.size(); i++){
			//Skip the last line... there is no "next" line
			if(i != lines.size() - 1){
				LineData l1 = *lines[i];
				LineData l2 = *lines[i+1];

				if(l1.end1Pt() == robot)
					points.push_back(l1.end2Pt());
				else {
					points.push_back(l1.end1Pt());
				}
				if(l2.end1Pt() == robot)
					points.push_back(l2.end2Pt());
				else {
					points.push_back(l2.end1Pt());
				}
			}
		}
		bool closed = true;
		NEW_SHAPE(poly, PolygonData, new PolygonData(worldShS, points, closed));
		poly->setObstacle(false);

		NEW_SKETCH_N(bounds, bool, poly->getRendering());
		NEW_SKETCH_N(fov, bool, visops::fillInterior(bounds));
		GET_SKETCH(seen, bool, worldSkS);
		seen |= fov;
		
		poly.deleteShape();
		
		//Remove vision rays -- we have the vision "areas" of polygons.
		SHAPEVEC_ITERATE(lines, LineData, l) {
			l.deleteShape();
		} END_ITERATE;
		
	}


	$nodeclass MoveToPoint : PilotNode(PilotTypes::goToShape) : doStart {
		$reference Explore::targetX;
		$reference Explore::targetY;
		$reference Explore::targetHeading;

		NEW_SHAPE(destination, EllipseData, 
			  new EllipseData(worldShS, 
					  Point(targetX, targetY, 0, allocentric), 25, 25));
		destination->setColor(rgb(255,0,0));
		destination->setObstacle(false);
		pilotreq.targetShape = destination;
		pilotreq.targetHeading = targetHeading; 
		// 0 == north, M_PI == south, M_PI/2 == west
	}
	
	$nodeclass EvaluateMotion : StateNode : doStart {
		$reference Explore::targetX;
		$reference Explore::targetY;
		$reference Explore::targetHeading;
		$reference Explore::numChoices;
		Point robot = theAgent->getCentroid();
		cout << "EVALUATE -- Num choices: " << numChoices << endl;
		const float worldSkSwidth = VRmixin::worldSkS.getWidth();
		const float worldSkSheight = VRmixin::worldSkS.getHeight();
		cout << "SKS WIDTH: " << worldSkSwidth << endl;
		cout << "SKS HEIGHT: " << worldSkSheight << endl;
		
		//Information about robot's search history
		GET_SKETCH(seen, bool, worldSkS);
		// x,y are coordinates in seen
 		//fmat::Column<3> worldPt = worldSkS.applyTmatInv(fmat::pack(x,y,0));

 		fmat::Column<3> worldPt = fmat::pack(robot.coordX(), robot.coordY(), 0);
		cout << "Robot coordinates in world space: " << worldPt << endl;
		worldSkS.applyTmat(worldPt);
		cout << "Robot coordinates in sketch space: " << worldPt << endl;
		int sketchRobotX = worldPt[0];
		int sketchRobotY = worldPt[1];
		int seenNorth = 0;
		int seenSouth = 0;
		int seenEast = 0;
		int seenWest = 0;
		
		for (int x = 0; x < seen.width ; x++){
			for (int y = 0; y < seen.height ; y++){
				if (seen(x, y)){
					if (x < sketchRobotX) {
						seenSouth++;
					} else {
						seenNorth++;
					}
					if (y < sketchRobotY){
						seenEast++;
					} else {
						seenWest++;
					}
				}
			}
		}
		cout << "NSEW" << endl;
		cout << seenNorth << endl;
		cout << seenSouth << endl;
		cout << seenEast << endl;
		cout << seenWest << endl;

		const float north = 0;	
		const float south = M_PI;
		const float west  = M_PI / 2;
		const float east  = -M_PI / 2;

		numChoices++;
		if(numChoices >= 3) {
			postStateSignal<choice>(stopMotion);
			return;
		}
		
		if (seenNorth > seenSouth){
			targetX = robot.coordX() - 200;
			targetHeading = south;
		} else {
			targetX = robot.coordX() + 200;
			targetHeading = north;	
		}

		targetY = robot.coordY();
		cout << targetX << ", " << targetY << endl;
		postStateSignal<choice>(moveNow);
		
	//	if (numChoices == 0){
	//		targetX = 250;
	//		targetY = 250;
	//		targetHeading = east;
	//		numChoices++;	
	//		postStateSignal<choice>(moveNow);
	//	} else if (numChoices == 1) {
	//		targetX = -250;
	//		targetY = 250;
	//		targetHeading = west;
	//		numChoices++;	
	//		postStateSignal<choice>(moveNow);
	//	} else {
	//		postStateSignal<choice>(stopMotion);
	//	}


	}

	$setupmachine{
		InitWorld =N=> find 
		find : FindStuff
		evalMotion : EvaluateMotion
		find =C=> DetermineBoxes =N=> DrawRays =N=> evalMotion
		//TODO Break MoveToPoint =C=> into "=S=>" and "=F=>" cases
		evalMotion =S<choice>(moveNow)=> MoveToPoint =C=> find 
		evalMotion =S<choice>(stopMotion)=> SpeechNode("Done.")
		
	}
}

REGISTER_BEHAVIOR(Explore);