#include "Behaviors/StateMachine.h"

$nodeclass AprilTagEllipses : VisualRoutinesStateNode {

  $nodeclass Look : MapBuilderNode : constructor {
    mapreq.addObjectColor(ellipseDataType, "red");
    mapreq.addObjectColor(ellipseDataType, "green");
    mapreq.addObjectColor(ellipseDataType, "blue");
    mapreq.setAprilTagFamily();  // Use the default tag family
  }

  $nodeclass Report : VisualRoutinesStateNode : doStart {

    NEW_SHAPEVEC(cameraTags, AprilTagData, select_type<AprilTagData>(camShS));
    
    // Assume there is one april tag.
    AprilTagData firstTag = *cameraTags[0];
    LineData leftLine(camShS, firstTag.getTopLeft(), firstTag.getBottomLeft());
    LineData rightLine(camShS, firstTag.getTopRight(), firstTag.getBottomRight());

    NEW_SHAPEVEC(ellipses, EllipseData, select_type<EllipseData>(camShS));
    uint numOnLeft = 0;
    SHAPEVEC_ITERATE(ellipses, EllipseData, e) {
        numOnLeft += leftLine.pointIsLeftOf(e->getCentroid());
    } END_ITERATE;
    
    if (numOnLeft == 0)
        cout << "All ellipses on the right side!" << endl;
    else if (numOnLeft == ellipses.size())
        cout << "All ellipses on the left side!" << endl;
    else
        cout << "Ellipses are mixed." << endl;
  }

  $setupmachine{
    look: Look =MAP=> Report
  }

}

REGISTER_BEHAVIOR(AprilTagEllipses);