#include "Behaviors/StateMachine.h"

///////////////////
// Part I - The MapBuilder
///////////////////

$nodeclass Lab4Part1 : VisualRoutinesStateNode {
	$nodeclass FindStuff : MapBuilderNode : doStart {
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "green");
		mapreq.addObjectColor(ellipseDataType, "blue");
		mapreq.addObjectColor(ellipseDataType, "black");
	}

	$nodeclass ReportEllipses : VisualRoutinesStateNode : doStart {
		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));

		if (ellipse_shapes.size() > 0) {
			NEW_SKETCH(ellipse, bool, ellipse_shapes[0]->getRendering());
		}

		SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myellipse)
			cout << "Id: " << myellipse->getId()
				 << "  Color: " << myellipse->getColor()
				 << "  Area: " << myellipse->getArea()
				 << endl;

			NEW_SHAPE(myellipse2, EllipseData, new EllipseData(
				camShS, 
				myellipse->getCentroid(), 
				myellipse->getSemimajor() * 1.5, 
				myellipse->getSemiminor() * 1.5, 
				myellipse->getOrientation()));
			myellipse2->setColor(myellipse->getColor());
		END_ITERATE;
	}

	$setupmachine {
		FindStuff=C=>ReportEllipses
	}
}

REGISTER_BEHAVIOR(Lab4Part1);

///////////////////
// Part II - Lines
///////////////////

$nodeclass Lab4Part2 : VisualRoutinesStateNode {
	$nodeclass FindStuff : MapBuilderNode : doStart {
		mapreq.addObjectColor(lineDataType, "red");
		mapreq.addObjectColor(lineDataType, "green");
		mapreq.addObjectColor(lineDataType, "blue");
		mapreq.addObjectColor(lineDataType, "black");
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "green");
		mapreq.addObjectColor(ellipseDataType, "blue");
		mapreq.addObjectColor(ellipseDataType, "black");
	}

	$nodeclass Report : VisualRoutinesStateNode : doStart {
		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));
		NEW_SHAPEVEC(line_shapes, LineData, select_type<LineData>(camShS));

		cout << "Found " << line_shapes.size() << " lines." << endl;

		int leftCount = 0, rightCount = 0;

		if (line_shapes.size() == 0)
			cout << "Couldn't find any lines" << endl;
		else {
			SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myellipse);
				if (myellipse->getArea() < 500) {
					if (line_shapes[0]->pointIsLeftOf(myellipse->getCentroid()))
						leftCount++;
					else
						rightCount++;
				} 	
			END_ITERATE;

			cout << leftCount << " circles to the left" << endl
				 << rightCount << " circles to the right" << endl;

			line_shapes[0]->setInfinite();
		}
	}

	$setupmachine {
		FindStuff=C=>Report
	}
}

REGISTER_BEHAVIOR(Lab4Part2);

///////////////////
// Part III - AprilTags
///////////////////

$nodeclass Lab4Part3 : VisualRoutinesStateNode {
	$nodeclass Look : MapBuilderNode($,MapBuilderRequest::worldMap) : constructor {
		cout << "Point the camera at an April Tag before running this demo." << endl;
		cout << "Type \"msg <Enter>\" to take another image and run again." << endl;
		cout << "Assumed marker height is " << MapBuilderRequest::defaultMarkerHeight
			<< " mm " << endl;
		mapreq.setAprilTagFamily();  // Use the default tag family
		mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "green");
		mapreq.addObjectColor(ellipseDataType, "blue");
	}

	$nodeclass Report : SpeechNode : doStart {
		NEW_SHAPEVEC(cameraTags, AprilTagData, select_type<AprilTagData>(camShS));
		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));

		int leftCount = 0, rightCount = 0;

		if (ellipse_shapes.size() == 0)
			cout << "Couldn't find any ellipses" << endl;
		else {
			IsLeftOfThis isLeftOfApril(cameraTags[0]);
			SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myellipse);
				if (isLeftOfApril(myellipse))
					leftCount++;
				else
					rightCount++;
			END_ITERATE;

			if (leftCount > 0 && rightCount == 0)
				cout << "All ellipses to the left" << endl;
			else if (leftCount == 0 && rightCount > 0)
				cout << "All ellipses to the right" << endl;
			else if (leftCount > 0 && rightCount > 0)
				cout << "Ellipses are mixed" << endl;
		}
	}

	$setupmachine{
		look: Look =MAP=> Report =TM=> look
	}
}

REGISTER_BEHAVIOR(Lab4Part3);

///////////////////
// Part IV - Polygons
///////////////////

$nodeclass Lab4Part4 : VisualRoutinesStateNode {
	$nodeclass Look : MapBuilderNode($,MapBuilderRequest::worldMap) : constructor {
		mapreq.addObjectColor(ellipseDataType, "green");
		mapreq.addObjectColor(ellipseDataType, "blue");
	}

	$nodeclass Report : SpeechNode : doStart {
		NEW_SHAPEVEC(cameraTags, AprilTagData, select_type<AprilTagData>(camShS));
		NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));

		vector<Point> polyCentroids;
		Point otherCentroid;

		IsColor greenTest("green");
		IsColor blueTest("blue");

		if (ellipse_shapes.size() == 0)
			cout << "Couldn't find any ellipses" << endl;
		else {
			SHAPEVEC_ITERATE(ellipse_shapes, EllipseData, myellipse);
				if (blueTest(myellipse) && myellipse->getArea() > 150)
					polyCentroids.push_back(myellipse->getCentroid());
				else if (greenTest(myellipse))
					otherCentroid = myellipse->getCentroid();
			END_ITERATE;

			PolygonData poly(camShS, polyCentroids, true);
			poly.setColor("blue");
			NEW_SKETCH(polySketch, bool, poly.getRendering());
			if (poly.isInside(otherCentroid))
				cout << "Green ellipse is inside the blue polygon" << endl;
			else
				cout << "Green ellipse is not inside the blue polygon" << endl;
		}
	}

	$setupmachine{
		look: Look =MAP=> Report =TM=> look
	}
}

REGISTER_BEHAVIOR(Lab4Part4);