//This is one program of a series of behaviors for Sift-tool training on Tekkotsu.
//It is in sift's native form with API calls to sift itself.
#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Vision/SIFT/API/SiftTekkotsu.h"
#include "local/tekkotsu/Simulator.h"
#include <vector>
#include <string>
#include <iostream>
#include <sstream>

using namespace DualCoding;



#nodeclass DatabaseTrainBehavior : VisualRoutinesStateNode : siftInfo()

struct SiftVariables {
  SiftVariables() : i(0), count(0), object(0), objectname(""), typeobject("JuiceBottle"), mySiftTekkotsu(), buff(), results(){}
  int i, count, object;
  string objectname, typeobject;
  SiftTekkotsu mySiftTekkotsu;
  ImageBuffer buff;
  vector<SiftMatch*> results;
} siftInfo;


#nodeclass Function1 : VisualRoutinesStateNode
virtual void doStart() {
  SiftVariables &siftInfo = parentAs<DatabaseTrainBehavior>()->siftInfo;

  // The main operation during the whole behavior
  ostringstream os;
  os << siftInfo.i;
  siftInfo.objectname =  siftInfo.typeobject + os.str();
  cout << siftInfo.objectname << endl;
  NEW_SKETCH(camGrey, uchar, sketchFromRawY());
  siftInfo.buff = siftInfo.mySiftTekkotsu.sketchToBuffer(camGrey);
  if (siftInfo.i==0)
    siftInfo.object = siftInfo.mySiftTekkotsu.train_addNewObject(siftInfo.buff);
  else
    siftInfo.mySiftTekkotsu.train_addToObject(siftInfo.object, siftInfo.buff);
  siftInfo.mySiftTekkotsu.setObjectName(siftInfo.object, siftInfo.objectname);
  siftInfo.i++;

  ProjectInterface::sendCommand("advance");
 //  if((siftInfo.i==5)&&(siftInfo.count==0)) {
//     siftInfo.count++;
//     siftInfo.i=0;
//   }
  if(siftInfo.i==3) { // To add more images change the number of max images here.
    siftInfo.mySiftTekkotsu.saveToFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/tekkotsu.dat", true);
	cout << "\n*** ** ********\nEnd of Behavior\n*** ** ********" << endl;
  }
  else
    postStateFailure();
}
#endnodeclass

virtual void setup() {
    #statemachine
      startnode: Function1 =F=> StateNode =T(1000)=> startnode
      #endstatemachine
  }

#endnodeclass

REGISTER_BEHAVIOR(DatabaseTrainBehavior);