#include "Behaviors/StateMachine.h"

$nodeclass DepthFinder : VisualRoutinesStateNode {

	$nodeclass FindBoundaries : VisualRoutinesStateNode : doStart {
		camSkS.clear();
		// For display purposes only
		NEW_SKETCH(rawCam, uchar, sketchFromRawY());
		// Get the depth frame from the connect
  	NEW_SKETCH(depthFrame, usint, sketchFromDepth());
		// Convert the pixels to signed floats so we can perform
		// neighbor distance on them
		NEW_SKETCH(depthFrameFloat, float, depthFrame);
		camSkS.requireIdx8way();
		// Take the average of all of the neighbors and then make
		// the distance of the average from the value of this pixel
		// the new value of this pixel
		NEW_SKETCH(neighborSum, float, depthFrameFloat -
													 	(depthFrameFloat[*camSkS.idxS]
			       							 + depthFrameFloat[*camSkS.idxSW]
											  	 + depthFrameFloat[*camSkS.idxW]
										  		 + depthFrameFloat[*camSkS.idxNW]
											 	   + depthFrameFloat[*camSkS.idxN]
													 + depthFrameFloat[*camSkS.idxNE]
													 + depthFrameFloat[*camSkS.idxE]
													 + depthFrameFloat[*camSkS.idxSE]) / 8);
		float min = neighborSum->min();
		// Scale the values so that all of them are >= 0 (because we're
		// converting back to unsigned values
		NEW_SKETCH(neighborSumInt, uint, neighborSum - min);
		int threshold = 5;
		// Find the pixels that had high differences with their neighbors
		NEW_SKETCH(negEdges, bool, neighborSumInt <= (int)(-min - threshold));
		NEW_SKETCH(posEdges, bool, neighborSumInt >= (int)(-min + threshold));
		NEW_SKETCH(edges, bool, visops::max(posEdges, negEdges));
		// These pixels (hopefully) form a boundary around the canister, so
		// fill all of the area that is not contained within this boundary
		NEW_SKETCH(exterior, bool, visops::fillExterior(edges));
		// Now compute the distances from that boundary
		NEW_SKETCH(interiorDist, uint, visops::mdist(exterior));
		// The pixel that is furthest from the boundary should be in the
    // middle of the canister, so get the depth from there
		int maxDistIndex = interiorDist->findMax();
		
		unsigned int depth = 0;

		if (maxDistIndex > -1) {
			depth = depthFrame[maxDistIndex];
		}

		cout << "The canister is " << depth << "mm from the robot" << endl;
  }

	$setupmachine {
		FindBoundaries
	}
}

REGISTER_BEHAVIOR(DepthFinder);