#include "Behaviors/StateMachine.h"


Shape<PolygonData> poly;

$nodeclass Lab4Part4 : VisualRoutinesStateNode {

    $nodeclass FindRedShapes : MapBuilderNode : doStart {
        mapreq.addObjectColor(ellipseDataType, "red");
    }
    $nodeclass FindBlueShapes : MapBuilderNode : doStart {
        mapreq.addObjectColor(ellipseDataType, "blue");
    }
    $nodeclass BuildPolygon : VisualRoutinesStateNode : doStart{
        NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));
	if (ellipse_shapes.size() < 3) {
	    cout << "not enough ellipses!";
	    postStateFailure();
	    return;
	}
	Shape<EllipseData> e1 = ellipse_shapes[0];
	Shape<EllipseData> e2 = ellipse_shapes[1];
	Shape<EllipseData> e3 = ellipse_shapes[2];
	cout << e1 << endl;
	cout << e2 << endl;
	cout << e3 << endl;
	std::vector<Point> pts;
	pts.push_back(e1->getCentroid());
	pts.push_back(e2->getCentroid());
	pts.push_back(e3->getCentroid());
	NEW_SHAPE(newpolygon, PolygonData, new PolygonData(camShS, pts, true));
	poly = newpolygon;
	postStateSuccess();
    }
    $nodeclass Report : VisualRoutinesStateNode : doStart {
        NEW_SHAPEVEC(ellipse_shapes, EllipseData, select_type<EllipseData>(camShS));
	if (ellipse_shapes.size() == 0) {
	    cout << "no blue polygon found!" << endl;
	    return;
	}
	Shape<EllipseData> inside = ellipse_shapes[ellipse_shapes.size() - 1];
	cout << poly << endl;
	cout << inside << endl;
	if (poly->isInside(inside->getCentroid())) {
	    cout << "the polygon contains the fourth ellipse" << endl;
	}
	else {
	    cout << "the polygon does not contain the fourth ellipse" << endl;
	}
    }
    $setupmachine {
        FindRedShapes =C=> BuildPolygon =S=> FindBlueShapes =C=> Report
    }
}

REGISTER_BEHAVIOR(Lab4Part4);