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


$nodeclass Explore {
	$provide std::vector<DualCoding::Shape<LineData> > walls;

	$nodeclass FindStuff : MapBuilderNode(MapBuilderRequest::worldMap) : doStart {
		mapreq.setAprilTagFamily();
		mapreq.searchArea = Lookout::groundSearchPoints();
		mapreq.pursueShapes = true;
	}

	$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);
			Point p2 = Point(originX - dx, originY - dy, 0);
			// probably not necessary, just confirming that reference
			// is with respect to world
			p1.setRefFrameType(allocentric);
			p2.setRefFrameType(allocentric);

			NEW_SHAPE(line, LineData, new LineData(worldShS, p1, p2));
			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;

		//Loop this
		Point robot = theAgent->getCentroid();
		float orient = 3.14159 / 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 = length * sin(orient);
			float y = length * cos(orient) * (-1); 
			Point sight = Point(x, y, 0, egocentric);
			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;
	
		//TODO move to init function
		//const float scale = 0.01f;
		//const float worldwidth = VRmixin::worldSkS.getWidth();
		//const float worldheight = VRmixin::worldSkS.getHeight();
		//worldSkS.setTmat(scale, worldwidth/2, worldheight/2);
		//NEW_SKETCH(seen, bool, visops::zeros(worldSkS));
	
		//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));


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

		//TODO Currently, I have a collection of polygons. They are unfilled, 
		//     and all unique. I CAN keep a vector of them, sample a point, and 
		//     ask if it was "seen", but I would like a more efficient way to 
		//     "combine" all the polygons, by filling them in, or making one 
		//	large, computationally cheaper polygon.
	}

	//$nodeclass MoveToPoint : PilotNode($, PilotTypes::goToShape) : doStart {
	//}
	
	$setupmachine{
		find: FindStuff
		determine: DetermineBoxes
		rays: DrawRays 
		find =C=> determine =N=> rays =N=> SpeechNode("Done.")
		
		//TODO -- MOVE SOMEWHERE ARBITRARY, find + determine + rays + done
	}
}

REGISTER_BEHAVIOR(Explore);