#include "Behaviors/StateMachine.h"

$nodeclass Lab5Q3 : VisualRoutinesStateNode {

  $nodeclass ProjectIt : MapBuilderNode(MapBuilderRequest::localMap) : doStart {
    mapreq.addObjectColor(lineDataType, "green");
    mapreq.addObjectColor(ellipseDataType, "red");
  }
  
  $nodeclass GetPlanes : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
    NEW_SHAPEVEC(egg_shapes, EllipseData, select_type<EllipseData>(camShS));
    camSkS.clear();
    if (lines.size() == 2) {
      NEW_SKETCH(plane1, bool, visops::topHalfPlane(lines[0]));
      NEW_SKETCH(plane2, bool, visops::topHalfPlane(lines[1]));
      
      NEW_SKETCH(camFrame, uchar, sketchFromSeg());
      
      NEW_SKETCH(red_eggs, bool, visops::colormask(camFrame,"red"));
      NEW_SKETCH(r_cc, uint, visops::labelcc(red_eggs));
      NEW_SKETCH(foo, bool, plane1 & plane2 & red_eggs);
    } else {
      cout << "Error! Wrong number of lines" << endl;
    }
  }
  
  $setupmachine{
  
    ProjectIt =C=> GetPlanes =C=> SpeechNode("done");
  }

}

REGISTER_BEHAVIOR(Lab5Q3);