#include "Behaviors/StateMachine.h"

$nodeclass Lab5Part2 : VisualRoutinesStateNode {
    $nodeclass FindShapes : MapBuilderNode : doStart {
        mapreq.addObjectColor(lineDataType, "blue");
	mapreq.addObjectColor(ellipseDataType, "red");
    }
    $nodeclass ProcessAndReport : VisualRoutinesStateNode : doStart {
        NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
	if (lines.size() < 2) {
	    cout << "did not find enough lines!" << endl;
	    postStateFailure();
	    return;
	}
        NEW_SKETCH(s1, bool, visops::topHalfPlane(lines[0]));
	NEW_SKETCH(s2, bool, visops::topHalfPlane(lines[1]));
	NEW_SKETCH(intersect, bool, s1 & s2);
	NEW_SKETCH(camFrame, uchar, sketchFromSeg());
	NEW_SKETCH(red, bool, visops::colormask(camFrame,"red"));
	NEW_SKETCH(insideRed, bool, intersect & red);
    }
    $setupmachine {
        FindShapes =C=> ProcessAndReport
    }
}

REGISTER_BEHAVIOR(Lab5Part2);