#include "Behaviors/StateMachine.h"

$nodeclass EdgeDetect : VisualRoutinesStateNode {


	$nodeclass Build : VisualRoutinesStateNode : doStart{

		NEW_SKETCH(yuvFrame, yuv, sketchFromYUV());
		NEW_SKETCH(depthFrame, usint, sketchFromDepth());

		camSkS.requireIdx4way();
		NEW_SKETCH(neighborNS, usint, depthFrame[*camSkS.idxS]);

		cout << "here1\n";
		NEW_SKETCH(diffNS, usint, depthFrame - neighborNS);

		cout << "here2\n";
		NEW_SKETCH(edges, bool, diffNS > 50);

		cout << "here3\n";
		NEW_SKETCH(black_cc, uint, visops::labelcc(edges));
		NEW_SKETCH(black_area, uint, visops::areacc(black_cc));
		NEW_SKETCH(black_blobs, bool, black_area > 10);

		NEW_SHAPEVEC(blob_shapes, BlobData, BlobData::extractBlobs(black_blobs));
		Point centroid;
		SHAPEVEC_ITERATE(blob_shapes, BlobData, cur_blob)
			centroid += cur_blob->getCentroid();
		END_ITERATE
		centroid /= blob_shapes.size();
		NEW_SHAPE(my_ellipse, EllipseData, new EllipseData(camShS, centroid, 30, 30, 0.0));
		my_ellipse->setColor("blue");
		NEW_SKETCH(new_ellipse, bool, my_ellipse->getRendering());
	}

	
	$setupmachine {
		Build =N=> SpeechNode("done")
	}
}

REGISTER_BEHAVIOR(EdgeDetect);