#include "Behaviors/StateMachine.h"

$nodeclass Part4 : VisualRoutinesStateNode {

  // work that visual data
  $nodeclass Scan : VisualRoutinesStateNode : doStart {
    // allow edge comparisons
    camSkS.requireIdx4way();

    // get some camera data
    NEW_SKETCH(yuvFrame, yuv, sketchFromYUV());

    // get the depth data
    NEW_SKETCH(depthFrame, usint, sketchFromDepth());

    // get the finite difference vertically and horizontally
    NEW_SKETCH(ndiff, usint, depthFrame - depthFrame[*camSkS.idxN]);
    NEW_SKETCH(nuse, bool, depthFrame > depthFrame[*camSkS.idxN]);
    
    NEW_SKETCH(sdiff, usint, depthFrame - depthFrame[*camSkS.idxS]);
    NEW_SKETCH(suse, bool, depthFrame > depthFrame[*camSkS.idxS]);
    
    NEW_SKETCH(wdiff, usint, depthFrame - depthFrame[*camSkS.idxW]);
    NEW_SKETCH(wuse, bool, depthFrame > depthFrame[*camSkS.idxW]);
    
    NEW_SKETCH(ediff, usint, depthFrame - depthFrame[*camSkS.idxE]);
    NEW_SKETCH(euse, bool, depthFrame > depthFrame[*camSkS.idxE]);

    

    // we assume that an object is a region without a threshold border shift
    unsigned short int threshold = 16;
    
    // get the edge map
    NEW_SKETCH(check_n, bool, (nuse & (ndiff > threshold)));
    NEW_SKETCH(check_s, bool, (suse & (sdiff > threshold)));
    NEW_SKETCH(check_e, bool, (wuse & (wdiff > threshold)));
    NEW_SKETCH(check_w, bool, (euse & (ediff > threshold)));

    NEW_SKETCH(edges, bool, check_n | check_s | check_e | check_w);
  }

  $setupmachine {
    Scan
  }

}

REGISTER_BEHAVIOR(Part4);