#include "Behaviors/StateMachine.h"
#include <vector>
#include <string>

$nodeclass Lab4Q3 : VisualRoutinesStateNode {

  $nodeclass EggFind : MapBuilderNode : doStart {
    mapreq.addObjectColor(ellipseDataType, "green");
    mapreq.addObjectColor(ellipseDataType, "blue");
    mapreq.addObjectColor(ellipseDataType, "red");
  }
  
  $nodeclass Counter : VisualRoutinesStateNode : doStart{
    NEW_SHAPEVEC(egg_shapes, EllipseData, select_type<EllipseData>(camShS));
    vector < Point > tri;
    Point target;
    rgb colors[3];
    int counters[] = {0,0,0};
    rgb polycolor;
    SHAPEVEC_ITERATE(egg_shapes, EllipseData, myegg){
      for (int i = 0; i < 3; i++)
      {
        if (counters[i] > 0 && colors[i] == myegg->getColor()) {
          counters[i]++;
          break;
        } else if (counters[i] == 0){
          counters[i]++;
          colors[i] = myegg->getColor();
          break;
        }
      }
    }END_ITERATE;
    
    for (int i = 0; i < 3; i++) {
      if (counters[i] >= 3) {
        polycolor = colors[i];
        break;
      }
    }
    
     
    SHAPEVEC_ITERATE(egg_shapes, EllipseData, myegg){
      if(myegg->getColor() == polycolor) {
        tri.push_back(myegg->getCentroid());
      } else
        target = myegg->getCentroid();
    }END_ITERATE;
    NEW_SHAPE(poly, PolygonData, new PolygonData(camShS, tri, true));
    
    cout << "The point " << target << " is ";
    if (! poly->isInside(target))
      cout << " not ";
    cout << " inside." << endl;
  }
  $setupmachine{
    EggFind =C=> Counter =C=> SpeechNode("done!")
  }

}

REGISTER_BEHAVIOR(Lab4Q3);