#include "Behaviors/StateMachine.h"

$nodeclass Lab5Part4 : VisualRoutinesStateNode : doStart {
    NEW_SKETCH(depthFrame, usint, sketchFromDepth());
    uint i,j;
    int threshold = 1000;
    for (i = 0; i < 400; i++) {
        for (j = 0; j < 400; j++) {
	    bool isEdge = false;
	    usint pix = depthFrame->at(i,j);
	    if (i > 0 && abs(pix-depthFrame->at(i-1,j)) > threshold) {
	        isEdge = true;
	    }
	    if (i < depthFrame->getHeight() - 2 && abs(pix-depthFrame->at(i+1,j)) > threshold) {
	        isEdge = true;
	    }
	    if (j > 0 && abs(pix-depthFrame->at(i,j-1)) > threshold) {
	        isEdge = true;
	    }
	    if (j < depthFrame->getWidth() - 2 && abs(pix-depthFrame->at(i,j+1)) > threshold) {
	        isEdge = true;
	    }

	    if (isEdge) {
	        cout << "There is a cliff at (" << i << ", " << j << ")" << endl;
	    }
	}
    }
}

REGISTER_BEHAVIOR(Lab5Part4);