//-*-c++-*-
#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Vision/SIFT/API/SiftTekkotsu.h"
#include "local/tekkotsu/Simulator.h"
#include <vector>

using namespace DualCoding;

#nodeclass SIFTDemoBehavior : VisualRoutinesStateNode
  virtual void doStart() {
    SiftTekkotsu mySiftTekkotsu;
    mySiftTekkotsu.setParameter("probOfMatch", 0.8);
    NEW_SKETCH(camGrey, uchar, sketchFromRawY());
    ImageBuffer buff = mySiftTekkotsu.sketchToBuffer(camGrey);
    int object1 = mySiftTekkotsu.train_addNewObject(buff);// Trace here
    // int object1 = mySiftTekkotsu.train_addNewObject("/tmp/rubik1.pgm");// Trace here
    mySiftTekkotsu.setObjectName(object1, "rubiks");
    vector<SiftMatch*> results;
    mySiftTekkotsu.findAllObjectsInImage(buff, results);
    cout << "Found " << results.size() << endl;
    if(results.size()>0)
    results[0]->print("  ");
    mySiftTekkotsu.saveToFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/TekExp/tekkotsu", true);
    cout << "\n*** ** ********\nEnd of Behavior\n*** ** ********" << endl;
  }
#endnodeclass

REGISTER_BEHAVIOR(SIFTDemoBehavior);