#include "Behaviors/StateMachine.h"

$nodeclass Lab5Part4 : VisualRoutinesStateNode {
  $nodeclass FindCenter : VisualRoutinesStateNode : doStart {
    camSkS.requireIdx4way();

    // High threshold to avoid usint overflows
    usint threshold = 100;
    usint low = threshold, high = 65535 - threshold;

    NEW_SKETCH(depthFrame, uint, sketchFromDepth());

    NEW_SKETCH(south, usint, depthFrame - depthFrame[*camSkS.idxS]);
    NEW_SKETCH(east, usint, depthFrame - depthFrame[*camSkS.idxE]);
    NEW_SKETCH(west, usint, depthFrame - depthFrame[*camSkS.idxW]);
    NEW_SKETCH(north, usint, depthFrame - depthFrame[*camSkS.idxN]);

    // &, | are overloaded for sketches, so use them instead of boolean
    // operators && and ||. The statement returns true iff at least one
    // of the differences is large enough and not overflowed. 
    NEW_SKETCH(boolseg, bool, ((south > low) & (south < high)) |
                              ((east > low) & (east < high)) |
                              ((west > low) & (west < high)) |
                              ((north > low) & (north < high)));

    // Find blobs.
    vector< Shape<BlobData> > blobs = BlobData::extractBlobs(boolseg, 200);

    // For each blob, display an ellipse at its bottom center.
    for (int i = 0; i < blobs.size(); i++) {
      Point p = blobs[i]->getCentroid();
      p.getCoords()[1] = blobs[i]->bottomLeft.coordY();
      NEW_SHAPE(e, EllipseData, new EllipseData(camShS, p, 5, 5));
    }

    postStateCompletion();
  }

  // This node imports our points to local space. Can be viewed in Local/World sketch.
  $nodeclass TranslateToWorld : MapBuilderNode($,MapBuilderRequest::worldMap) : doStart {
    mapreq.clearCamera = false;
  }

  $setupmachine{
    FindCenter =C=> TranslateToWorld
  }

}

REGISTER_BEHAVIOR(Lab5Part4);