#include "Behaviors/StateMachine.h"

$nodeclass Lab4Q3 : VisualRoutinesStateNode {

  // Looks at scene and extracts ellipses
  $nodeclass Eye : MapBuilderNode : doStart {
    mapreq.addObjectColor(ellipseDataType, "red");
    mapreq.addObjectColor(ellipseDataType, "blue");
  }

  // Examines the scene and reports number of ellipses
  $nodeclass Report : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));

    int bluefound = 0;
    vector<Point> points;
    vector<Point> red;

    SHAPEVEC_ITERATE(ellipses, EllipseData, myellipse)
      if (IsColor("blue")(myellipse)) {
        bluefound++;
        points.push_back(myellipse->getCentroid());
      }
      if (IsColor("red")(myellipse)) {
        red.push_back(myellipse->getCentroid());
      }

    END_ITERATE;
    cout << "I found " << bluefound << " blue ellipses" << endl;

    // Create polygon
    NEW_SHAPE(mypoly, PolygonData, 
      new PolygonData(camShS, points, true, true, true));

    // Determine if it's inside
    cout << "The red ellipse " << (mypoly->isInside(red[0]) ? " IS " : " IS NOT ") << " inside the polygon." << endl;
  }
  
  $setupmachine{
    Eye =C=> Report =C=> SpeechNode("done")
  }
}

REGISTER_BEHAVIOR(Lab4Q3);