#include "Behaviors/StateMachine.h"

$nodeclass Lab4Q2 : VisualRoutinesStateNode {

  $nodeclass EggFind : MapBuilderNode : doStart {
    mapreq.addObjectColor(ellipseDataType, "green");
    mapreq.addObjectColor(lineDataType, "green");
    mapreq.addObjectColor(ellipseDataType, "blue");
    mapreq.addObjectColor(ellipseDataType, "red");
  }
  
  $nodeclass Counter : VisualRoutinesStateNode : doStart{
    NEW_SHAPEVEC(egg_shapes, EllipseData, select_type<EllipseData>(camShS));
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
    int left = 0, right = 0;
    if ( lines.size() > 0) {
      lines[0]->setInfinite();
      SHAPEVEC_ITERATE(egg_shapes, EllipseData, myegg){
        left += lines[0]->pointIsLeftOf(myegg->getCentroid());
      }END_ITERATE;
      right = egg_shapes.size() - left;
    }
    else{
      cout << "No line found" << endl;
    }
    cout << "There are " << left << " ellipses on the left of the line and " << right << " ellipses on the right of the line." << endl;
  }
  $setupmachine{
    EggFind =C=> Counter =C=> SpeechNode("done!")
  }

}

REGISTER_BEHAVIOR(Lab4Q2);