#include "Behaviors/StateMachine.h"

$nodeclass FindVeeShape : VisualRoutinesStateNode
{

$nodeclass FindStuff : MapBuilderNode : doStart
{
    mapreq.addObjectColor(lineDataType,"blue");
//    mapreq.addObjectColor(ellipseDataType,"blue");
 //   mapreq.addObjectColor(ellipseDataType,"red");
  //  mapreq.addObjectColor(ellipseDataType,"green");

    mapreq.pursueShapes = false;
}

$nodeclass MakeSketch : VisualRoutinesStateNode : doStart
{
    camSkS.clear();
    NEW_SKETCH(camFrame, uchar, sketchFromSeg());

    Shape<LineData> big_line;
    Shape<LineData> short_line;
    int max = 0;
    int second_max = 0;
    NEW_SHAPEVEC(line_shapes, LineData, select_type<LineData>(camShS));
    SHAPEVEC_ITERATE(line_shapes, LineData, line)
    if(line->getLength() > max)
    {
        short_line = big_line;
        big_line = line;
        second_max = max;
        max = line->getLength();
    }
    else if(line->getLength() > second_max)
    {
        short_line = line;
        second_max = line->getLength();
    }
    END_ITERATE;

    NEW_SKETCH(b_cc1, bool, visops::topHalfPlane(big_line));
    NEW_SKETCH(b_cc2, bool, visops::topHalfPlane(short_line));
    NEW_SKETCH(result, bool, b_cc1 & b_cc2);

    NEW_SKETCH(red_stuff, bool, visops::colormask(camFrame, "red"));
    NEW_SKETCH(green_stuff, bool, visops::colormask(camFrame, "green"));

    NEW_SKETCH(red_blobs, uint, visops::labelcc(red_stuff));
    NEW_SKETCH(green_blobs, uint, visops::labelcc(green_stuff));

    NEW_SKETCH(new_sketch, bool, red_stuff & green_stuff);
//  NEW_SKETCH(new_sketch, uint, empty_stuff);

//  Sketch<uint> new_sketch = Sketch(camSkS, "new_sketch");

    NEW_SKETCH(red_area_sketch, uint, visops::areacc(red_blobs));
    for(int count = 0; count < 5; count++)
    {
        int const max_red_area = red_area_sketch->max();
        NEW_SKETCH(red_max, bool, red_area_sketch == max_red_area);
        NEW_SKETCH(intersection, bool, red_max & result);
        int total = 0;
        for(unsigned int inCount = 0; inCount < intersection.pixels->size(); inCount++)
            if(intersection[inCount])
                total++;
        if(total > 10)
        {
            cout << "red blob in vee" << endl;
            new_sketch += intersection;
        }
        else
            cout << "red blob outside vee" << endl;

        NEW_SKETCH(blah, uint, visops::labelcc(red_max));
        NEW_SKETCH(blah_area, uint, visops::areacc(blah));
        red_area_sketch -= blah_area;
    }

    cout << "done with makeSketch" << endl;
}

$setupmachine {
    FindStuff =C=> MakeSketch
}

}

REGISTER_BEHAVIOR(FindVeeShape);