#include "Behaviors/StateMachine.h"
#include "DualCoding/ShapeTypes.h"

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

	$setupmachine {
		BuildMap =MAP=> CountMap
	}

	$nodeclass CountMap : VisualRoutinesStateNode : doStart {
		cout << "There were " << camShS.allShapes().size() << " ellipses." << endl;

		NEW_SKETCH(camFrame, uchar, sketchFromSeg());
		NEW_SKETCH(red_stuff, bool, visops::colormask(camFrame, "red"));
		NEW_SHAPEVEC(red_blobs, EllipseData, EllipseData::extractEllipses(red_stuff));

		int ellipses = 0;
		vector<Point> centroidArray;
		SHAPEVEC_ITERATE(red_blobs, EllipseData, myblob)
			if (myblob->getSemimajor() > 20 && ellipses <3) {
				centroidArray.push_back(myblob->getCentroid());
				ellipses++;
			} else
				myblob.deleteShape();
		END_ITERATE;
		
		NEW_SHAPE(polygon, PolygonData, new PolygonData(camShS, centroidArray, true));


		NEW_SKETCH(blue_stuff, bool, visops::colormask(camFrame, "blue"));
		NEW_SHAPEVEC(blue_blobs, EllipseData, EllipseData::extractEllipses(blue_stuff));

		SHAPEVEC_ITERATE(blue_blobs, EllipseData, myblob)
			if (myblob->getSemimajor() > 20) {
				if (polygon->isInside(myblob->getCentroid())) 
					cout << "The blue egg is inside the polygon." << endl;
				else
					cout << "The blue egg is outside the polygon." << endl;
				return;
			}
		END_ITERATE;
	}
	
}

REGISTER_BEHAVIOR(Lab3part3part3);