#include "Behaviors/StateMachine.h"

$nodeclass Lab5Part2 : VisualRoutinesStateNode {

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

  $nodeclass ExamineResults : VisualRoutinesStateNode : doStart {
    camSkS.clear();

    // Get the current frame.
    NEW_SKETCH(camFrame, uchar, sketchFromSeg());

    // Get lines.
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
    SHAPEVEC_ITERATE(lines, LineData, l) {
        l->setInfinite();
    } END_ITERATE;
    
    // Intersect the top half planes of the two lines to get the vee.
    NEW_SKETCH(insideVee, bool, visops::topHalfPlane(lines[0]) & visops::topHalfPlane(lines[1]));

    // Find all ellipses.
    NEW_SKETCH(ellipses, bool, visops::colormask(camFrame, "red") | visops::colormask(camFrame, "green"));
    NEW_SKETCH(insideEllipses, bool, insideVee & ellipses);

    // Label the ellipses
    NEW_SKETCH(labeledInsideEllipses, uint, visops::labelcc(insideEllipses, 10));
  }

  $setupmachine{
    BuildMap =MAP=> ExamineResults
  }

}

REGISTER_BEHAVIOR(Lab5Part2);