#include "Crew/MapBuilder.h"
#include "Crew/MapBuilderRequest.h"
#include "DualCoding/VisualRoutinesStateNode.h"
#include "DualCoding/ShapeTypes.h"
#include "Behaviors/StateMachine.h"

$nodeclass Lab4Q2 : VisualRoutinesStateNode {

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

  // Examines the scene and reports number of ellipses
  $nodeclass Report : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));
    if ( lines.size() == 0) {
        cout << "Did not find any lines" << endl;
        return;
    }
    LineData myline = lines[0];
    myline.setInfinite();

    int left = 0;
    int right = 0;

    SHAPEVEC_ITERATE(ellipses, EllipseData, myellipse)
      if (myline.pointIsRightOf(myellipse->getCentroid())) {
        left++;
      }
      if (myline.pointIsLeftOf(myellipse->getCentroid())) {
        right++;
      }
      
      NEW_SHAPE(redrawn, EllipseData, 
        new EllipseData(camShS, Point(myellipse->getCentroid()),
          myellipse->getSemimajor() * 1.5f, 
          myellipse->getSemiminor() * 1.5f,
          myellipse->getOrientation()));
      redrawn->setColor(myellipse->getColor());
    END_ITERATE;
    cout << "I found " << left << " ellipses left of the line" << endl;
    cout << "I found " << right << " ellipses right of the line" << endl;
  }
  
  $setupmachine{
    Eye =C=> Report =C=> SpeechNode("done")
  }
}

REGISTER_BEHAVIOR(Lab4Q2);