#include "Behaviors/StateMachine.h"


$nodeclass MakePolygon : VisualRoutinesStateNode {

        $setupmachine{

                FindEllipses  =C=> ConnectTheDots =C=> SpeechNode("done")
        }

        $nodeclass FindEllipses : MapBuilderNode : doStart {
                mapreq.addObjectColor(ellipseDataType, "red");
		mapreq.addObjectColor(ellipseDataType, "blue");
        }

        $nodeclass ConnectTheDots : VisualRoutinesStateNode : doStart {

                NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
		int i=0;
		std::vector<Point> points(3);
                SHAPEVEC_ITERATE(ellipses, EllipseData, currellipse)
			if(i==3)
				break;
			if(currellipse->getColor() != IsColor("red").color)
				continue;
			points[i] = currellipse->getCentroid();
			
			i++;
	
                END_ITERATE;

		NEW_SHAPE(polygon, PolygonData, new PolygonData(camShS, points, true, true, true));
		
		Point testPoint;
		SHAPEVEC_ITERATE(ellipses, EllipseData, currEllipse)
		
			if(currEllipse->getColor() != IsColor("blue").color)
				continue;
			testPoint = currEllipse->getCentroid();
			break;

		END_ITERATE;

		if(polygon->isInside(testPoint))
		cout<<"is inside"<< endl;

		else cout<<"not inside"<< endl;
        }

}

REGISTER_BEHAVIOR(MakePolygon);