#include "Behaviors/StateMachine.h"
#include "DualCoding/ShapeTypes.h"

#define DEPTH_THRESHOLD 100

$nodeclass Lab5Q4 : VisualRoutinesStateNode {

  $nodeclass Eye : MapBuilderNode : doStart {
    cout << "LOL" << endl;


  }

  $nodeclass DstBehavior : VisualRoutinesStateNode : doStart {
    usint max = 0-1;

    camSkS.requireIdx4way();
    NEW_SKETCH(depth, usint, sketchFromDepth());
    NEW_SKETCH(edgess, usint, depth - depth[*camSkS.idxS]);
    NEW_SKETCH(edgesw, usint, depth - depth[*camSkS.idxW]);
    NEW_SKETCH(threshold, bool, ((edgess >= DEPTH_THRESHOLD) & (edgess <= max - DEPTH_THRESHOLD)) | ((edgesw >= DEPTH_THRESHOLD) & (edgesw <= max - DEPTH_THRESHOLD)));

  }

  $setupmachine {
    a : Eye =MAP=> d
    d : DstBehavior =C=> SpeechNode("done")

  }
}

REGISTER_BEHAVIOR(Lab5Q4);