#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);