#include "Behaviors/StateMachine.h"
#include "DualCoding/ShapeTypes.h"

#define ELLIPSE_THRESHOLD 100

$nodeclass Lab5Q3 : VisualRoutinesStateNode {

  // Looks at scene and extracts ellipses
  $nodeclass Eye : MapBuilderNode : doStart {
    cout << "adding to my map" << endl;
    //mapreq.pursueShapes = true;

    mapreq.addObjectColor(lineDataType, "red");
//    mapreq.addObjectColor(ellipseDataType, "blue");


    cout << "done adding to my map" << endl;
  }

  $nodeclass DstBehavior : VisualRoutinesStateNode : doStart {
    NEW_SHAPEVEC(lines, LineData, select_type<LineData>(camShS));

    NEW_SKETCH(camFrame, uchar, sketchFromSeg());
    NEW_SKETCH(p1, bool, visops::topHalfPlane(lines[0]));
    NEW_SKETCH(p2, bool, visops::topHalfPlane(lines[1]));

    NEW_SKETCH(p3, bool, p1&p2);
    NEW_SKETCH(p4, bool, visops::colormask(camFrame,"blue") & p3);

    NEW_SKETCH(p5, uint, visops::labelcc(p4));
    NEW_SKETCH(p6, uint, visops::areacc(p5));

    NEW_SKETCH(p7, bool, p6 >= ELLIPSE_THRESHOLD);

/*    SHAPEVEC_ITERATE(p1, cur)
        cout << "FAIL" << endl;
    END_ITERATE;*/

  }

  $setupmachine {
    a : Eye =MAP=> d
    d : DstBehavior =C=> SpeechNode("done")

  }
}

REGISTER_BEHAVIOR(Lab5Q3);