//This is a training program for Sift-tool integration with Tekkotsu
//It is in sift's raw 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 NewTrainBehavior : VisualRoutinesStateNode : siftInfo()

struct SiftVariables {
  SiftVariables() : i(0), object(0), controller('k'), mySiftTekkotsu(), buff(), results(){}
  int i, object;
  char controller;
  SiftTekkotsu mySiftTekkotsu;
  ImageBuffer buff;
  vector<SiftMatch*> results;
} siftInfo;

#nodeclass Function1 : VisualRoutinesStateNode
virtual void doStart() {
  
  SiftVariables &siftInfo = parentAs<NewTrainBehavior>()->siftInfo;
  //ProjectInterface::sendCommand("freeze");
  //ProjectInterface::sendCommand("set Visual.Verbose=2");
  // while(siftInfo.controller == 'k') {
//     cout << "Do you wish the next photo(y/n)?: ";
//     cin>>siftInfo.controller;
//     if(siftInfo.controller == 'y') {
//       ProjectInterface::sendCommand("advance");
//       siftInfo.controller == 'k';
//     }
//     else if(siftInfo.controller == 'n');
//     else {
//       cout << "Please enter only one logical letter.\n"
// 	   << "y for yes (lower case only)\n"
// 	   << "n for no (lower case only ).\n" << endl;
//       siftInfo.controller = 'k';
//     }
//   }
  if(siftInfo.i==0) {
    NEW_SKETCH(camGrey, uchar, sketchFromRawY());
    siftInfo.buff = siftInfo.mySiftTekkotsu.sketchToBuffer(camGrey);
    siftInfo.object = siftInfo.mySiftTekkotsu.train_addNewObject(siftInfo.buff);
    siftInfo.mySiftTekkotsu.setObjectName(siftInfo.object, "bottle");
    siftInfo.i++;
    ProjectInterface::sendCommand("advance");
    siftInfo.controller = 'k';
    postStateFailure();
  }
  else {
    NEW_SKETCH(camGrey, uchar, sketchFromRawY());
    siftInfo.buff = siftInfo.mySiftTekkotsu.sketchToBuffer(camGrey);
    siftInfo.mySiftTekkotsu.findAllObjectsInImage(siftInfo.buff, siftInfo.results);
    int s = siftInfo.results.size();
    cout << "Found" << s << endl;
    for( int k = 0; k < s; k++ )
      siftInfo.results[k]->print("  ");
    
    ProjectInterface::sendCommand("advance");
    siftInfo.controller = 'k';
    
    siftInfo.mySiftTekkotsu.saveToFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/TekExp/newtrainbehavior", true);
    cout << "\n*** ** ********\nEnd of Behavior\n*** ** ********" << endl;
  }
}
#endnodeclass

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

#endnodeclass

REGISTER_BEHAVIOR(NewTrainBehavior);