#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;
using namespace std;

#nodeclass SiftAppendBehavior : VisualRoutinesStateNode : siftInfo()

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


#nodeclass DBLoad : StateNode
virtual void doStart() {
  SiftVariables &siftInfo = parentAs<SiftAppendBehavior>()->siftInfo;
  siftInfo.mySiftTekkotsu.loadFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/tekkotsu.dat");
  postStateCompletion();
}
#endnodeclass


#nodeclass Append : StateNode
virtual void doStart() {
  char ans = '\n';
  cout << "Do you wish to add this picture to the database(y/n)? "; 
  cin >> ans;
  if (tolower(ans) == 'n') {
    cout << "Do you wish to change the picture(y/n)? ";
    cin >> ans;
    
    if (tolower(ans) == 'y') {
      ProjectInterface::sendCommand("advance");
      ans = '\n';
    }
    else;
    
    postStateFailure();
    
  }
  else if (tolower(ans) == 'y') {
    SiftVariables &siftInfo = parentAs<SiftAppendBehavior>()->siftInfo;
    
    cout << "New object or adding a model to an object (n/a)? ";
    cin >> ans;
    
    if (tolower(ans) == 'n') {
      cout << "Object name: ";
      getline(cin, siftInfo.objectname);
      cout << siftInfo.objectname << endl;
      NEW_SKETCH(camGrey, uchar, sketchFromRawY());
      siftInfo.buff = siftInfo.mySiftTekkotsu.sketchToBuffer(camGrey);
      siftInfo.object = siftInfo.mySiftTekkotsu.train_addNewObject(siftInfo.buff);
      siftInfo.mySiftTekkotsu.setObjectName(siftInfo.object, siftInfo.objectname);
    }
    else if (tolower(ans) == 'a') {
      cout << "Current model or other model(c/o)?";
      cin >> ans;
      if (tolower(ans) == 'c')
	siftInfo.mySiftTekkotsu.train_addToObject(siftInfo.object, siftInfo.buff);
      else if (tolower(ans) == 'o') {
	cout << "What is the object name?\n";
	getline(cin, siftInfo.objectname);
	siftInfo.object = siftInfo.mySiftTekkotsu.getObjectID(siftInfo.objectname);
	if ( siftInfo.object < 0 ) {
	  cout << "Said object does not exist. Try again.\n";
	  postStateFailure();
	}
	else
	  siftInfo.mySiftTekkotsu.train_addToObject(siftInfo.object, siftInfo.buff);	
      }
    }
    
    cout << "Do you wish to continue (y/n)?";
    cin >> ans;
    
    if (tolower(ans) == 'y') {
      ans = '\n';
      postStateFailure();
    }
    else {
      cout << "Do you wish to save changes(y/n)? ";
      cin >> ans;
      
      if (tolower(ans) == 'y') {
	system("mkdir -p ~/sift-toolData/");
	siftInfo.mySiftTekkotsu.saveToFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/tekkotsu.date", true);
	cout << "Changes saved!\n";
	ans = '\n';
      }
      else {
	cout << "Changes were not saved!\n";
	ans = '\n';
      } 
      postStateCompletion();
    }
    postStateFailure();
  }
}
#endnodeclass

virtual void setup() {
#statemachine
  DBLoad =C=> Append
    Append =F=> StateNode =T(100)=> Append
    Append =C=> StateNode
#endstatemachine
    }

#endnodeclass

REGISTER_BEHAVIOR(SiftAppendBehavior);

//  
    // siftInfo.object = siftInfo.mySiftTekkotsu.train_addNewObject(siftInfo.buff);
//     siftInfo.mySiftTekkotsu.train_addToObject(siftInfo.object, siftInfo.buff);
//     siftInfo.mySiftTekkotsu.setObjectName(siftInfo.object, siftInfo.objectname);
    

//   ProjectInterface::sendCommand("advance");
  
  
  
//   if(siftInfo.i==10) { // 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);