#include "Behaviors/StateMachine.h"

$nodeclass Depth : VisualRoutinesStateNode : doStart {
	NEW_SKETCH(depth_frame, usint, sketchFromDepth());
	camSkS.requireIdx4way();
	NEW_SKETCH(southern_frame, usint, depth_frame-depth_frame[*camSkS.idxN]);
	NEW_SKETCH(t_southern_frame, bool, (southern_frame > 500) & (southern_frame < 10000));
	NEW_SKETCH(northern_frame, usint, depth_frame-depth_frame[*camSkS.idxS]);
	NEW_SKETCH(t_northern_frame, bool, (northern_frame > 500) & (northern_frame < 10000));
	NEW_SKETCH(eastern_frame, usint, depth_frame-depth_frame[*camSkS.idxE]);
	NEW_SKETCH(t_eastern_frame, bool, (eastern_frame > 500) & (eastern_frame < 10000));
	NEW_SKETCH(western_frame, usint, depth_frame-depth_frame[*camSkS.idxW]);
	NEW_SKETCH(t_western_frame, bool, (western_frame > 500) & (western_frame < 10000));
	NEW_SKETCH(raw_edge_frame, bool, t_southern_frame | t_northern_frame | t_eastern_frame | t_western_frame);
	NEW_SKETCH(edge_frame, bool, visops::fillin(raw_edge_frame, 3, 2, 15));
	NEW_SHAPE(edge_polygon, PolygonData, PolygonData::convexHull(edge_frame));
	edge_polygon->setColor("red");
	
}

REGISTER_BEHAVIOR(Depth);