// part 2 mapbuilder egg finder

#include "Behaviors/StateMachine.h"
#include "DualCoding/ShapeTypes.h"
#include "DualCoding/VisualRoutinesStateNode.h"
#include <vector>

$nodeclass PartThree : VisualRoutinesStateNode {
  
  // finds ellipses
  $nodeclass Find : MapBuilderNode : doStart {

    // polygon (first 3 ellipses) color is red
    mapreq.addObjectColor(ellipseDataType, "red");

    // target (4th ellipse) color is blue
    mapreq.addObjectColor(ellipseDataType, "blue");

  }

  // Report Left/Right Ellipses
  $nodeclass Classify : VisualRoutinesStateNode : doStart {
    // get the dividing line
    NEW_SHAPEVEC(lines,LineData,select_type<LineData>(camShS));
    lines[0]->setInfinite(true);

    int countL = 0;
    int countR = 0;

    NEW_SHAPEVEC(ellipses,EllipseData,select_type<EllipseData>(camShS))
    SHAPEVEC_ITERATE(ellipses,EllipseData,myEllipse)
      if (lines[0]->pointIsLeftOf(myEllipse->getCentroid())) {
        countL += 1;
      } else {
        countR += 1;
      }
    END_ITERATE    

    cout << "Ellipses on Left: " << countL << endl;
    cout << "Ellipses on Right: " << countR << endl;
  }

  // Register polygon formed by three ellipses
  $nodeclass MakePoly : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(ellipses,EllipseData,select_type<EllipseData>(camShS))

    // get red ellipses in order to form polygon, number of corners limited to 3
    int currentCorner = 0;
    vector<Point> pts;

    SHAPEVEC_ITERATE(ellipses,EllipseData,myEllipse)
      if (myEllipse->getColor() == ProjectInterface::getColorRGB("red") && currentCorner < 3) {
	currentCorner += 1;
	pts.push_back(myEllipse->getCentroid());
      }
    END_ITERATE

    // make the actual polygon
    NEW_SHAPE(myPoly, PolygonData, new PolygonData(camShS, pts, true, true, true));
    myPoly->setColor("red");

    postStateCompletion();
  }

  // Check to see if the blue ellipse's center is in the polygon or not
  $nodeclass InnerCheck : VisualRoutinesStateNode : doStart {
    bool alreadyDone = false; // boolean so that we only check one blue ellipse
    bool inPoly = false;

    NEW_SHAPEVEC(ellipses,EllipseData,select_type<EllipseData>(camShS)) 
    NEW_SHAPEVEC(polys,PolygonData,select_type<PolygonData>(camShS)) 

    SHAPEVEC_ITERATE(ellipses,EllipseData,myEllipse)
      if (myEllipse->getColor() == ProjectInterface::getColorRGB("blue") && !alreadyDone) {
        alreadyDone = true;

	inPoly = polys[0]->isInside(myEllipse->getCentroid());
      }
    END_ITERATE

    // add a return state because we can
    if (inPoly) {
      cout << "SUCCESS: Blue ellipse center is in polygon!" << endl; 
      postStateSuccess();
    } else {
      cout << "FAILURE: Blue ellipse center is not in polygon!" << endl; 
      postStateFailure();
    }
  }

  // general behavior
  $setupmachine {
    Find =C=> MakePoly =C=> InnerCheck
  }
}

REGISTER_BEHAVIOR(PartThree);