#include "Behaviors/StateMachine.h"

$nodeclass Part3 : VisualRoutinesStateNode {

  // look for the lines
  $nodeclass Scan : MapBuilderNode : doStart {
    mapreq.addObjectColor(lineDataType, "red");
  }

  // find the wedge and find the eggs in the wedge
  $nodeclass GetEggs : VisualRoutinesStateNode : doStart {

    // make sure nothing in the way
    camSkS.clear();
  
    // pull out the lines
    NEW_SHAPEVEC(lines,LineData,select_type<LineData>(camShS));
    
    // make the planes
    NEW_SKETCH(rl_a,bool,visops::topHalfPlane(lines[0]));
    NEW_SKETCH(rl_b,bool,visops::topHalfPlane(lines[1]));

    // make the wedge
    NEW_SKETCH(wedge,bool,rl_a | rl_b);

    // get the camera frame 
    NEW_SKETCH(frame,uchar,sketchFromSeg());
    
    // get the eggs
    NEW_SKETCH(blue_stuff,bool,visops::colormask(frame,"blue"));
    NEW_SKETCH(eggs,uint,visops::labelcc(blue_stuff));

    // filter the eggs
    NEW_SKETCH(some_eggs,bool,(eggs > 0) & !wedge);
  }

  $setupmachine {
    Scan =C=> GetEggs
  }

}

REGISTER_BEHAVIOR(Part3);