#include "Behaviors/StateMachine.h"

/*! First use SiftDemoTrain to train an object database.  Then point
 *  the camera at the object and run this SiftDemoTest behavior to
 *  look for the object in the camera image.  Use the sketchGUI viewer
 *  (click on the "C" in the ControllerGUI) to see the result.
 */

$nodeclass MySiftDemoTest : VisualRoutinesStateNode {

  $nodeclass Looker : MapBuilderNode : doStart {
    mapreq.siftDatabasePath = "sift-demo.dat";
  }

  $nodeclass Display : VisualRoutinesStateNode : doStart {
// get sift objects from cam space
	NEW_SHAPEVEC(siftobjects, SiftData, select_type<SiftData>(camShS));
	for(int count = 0; count < siftobjects.size(); count++)
	{
		siftobjects[count]->displayMatchedFeatures();
		cout << siftobjects[count]->getCentroid() << endl;
//		BoundingBox2D bb2d = siftobjects[count]->getBoundingBox();
		Point topLeft = siftobjects[count]->getTopLeft();
		Point topRight = siftobjects[count]->getTopRight();
		Point bottomLeft = siftobjects[count]->getBottomLeft();
		Point bottomRight = siftobjects[count]->getBottomRight();
		NEW_SHAPE(blob, BlobData, new BlobData(camShS, topLeft, topRight, bottomLeft, bottomRight));
	}


//	get sift objects from camera space
//    NEW_SHAPEVEC(siftobjects, SiftData, select_type<SiftData>(camShS));
//    if ( siftobjects.size() > 0 )
//      siftobjects[0]->displayMatchedFeatures();
  }

	$nodeclass ProjectIt:
	MapBuilderNode(MapBuilderRequest::localMap) : doStart
	{
		mapreq.clearCamera = false;
	}

  virtual void setup() {
    $statemachine{
      Looker =MAP=> Display =C=> ProjectIt
    }
  }
}

REGISTER_BEHAVIOR(MySiftDemoTest);