#include "Behaviors/StateMachine.h"
#include "vector"
$nodeclass DisconDetection : VisualRoutinesStateNode {
	//Create depth map
	$nodeclass GetDepthMap : VisualRoutinesStateNode : doStart {
		//Create the depth sketch
		NEW_SKETCH(depthMap, usint, sketchFromDepth());

		//Prepare for comparison
		int threshold = 100;
		int brightness = 1;
		camSkS.requireIdx4way();
		NEW_SKETCH(difMap, usint, depthMap - depthMap[*camSkS.idxS] > threshold);

		//Use the susan_edge_points algorithm to find edges
		NEW_SKETCH(edgeSusan, uint, visops::susan_edges(difMap,brightness));

		//Create a label cc from edgeSusan
		NEW_SKETCH(black_stuff, bool, visops::colormask(edgeSusan, "black"));
		NEW_SKETCH(black_stuff_fill, bool, visops::fillin(black_stuff,3,3,8, false));
		
		
		//Get vector of blobs from sketch data
		vector< Shape< BlobData > > myBlobs = BlobData::extractBlobs(black_stuff_fill, 600, 					BlobData::groundplane, 0, 50);
		NEW_SHAPEVEC(testShapes, BlobData, myBlobs);


		//Print out centroid position of the shape
		cout << "The shape is at the location " << testShapes[0]->getCentroid() << endl;		
	}

	$setupmachine {
		GetDepthMap
	}
}
REGISTER_BEHAVIOR(DisconDetection);