//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 SiftTrainBehavior : VisualRoutinesStateNode : siftInfo()

struct SiftVariables {
  SiftVariables() : i(0), count(0), object(0), objectname(""), typeobject(""), mySiftTekkotsu(), buff(), results(),
		    minX(1000000000.00), minY(1000000000.00), maxX(0), maxY(0), temp(), tmp(), keys(temp), gaussianSpace(tmp){}
  int i, count, object;
  string objectname, typeobject;
  SiftTekkotsu mySiftTekkotsu;
  ImageBuffer buff;
  vector<SiftMatch*> results;

  // Taken from keypoint Study behavior
  double minX,  minY;
  double maxX,  maxY;
   vector<keypoint*> temp;
   vector< vector< vector<int> > > tmp;
  vector<keypoint*>& keys;
   vector <vector <vector <int> > >& gaussianSpace;
} siftInfo;

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

  switch(siftInfo.count) {
  case 0:
    siftInfo.typeobject = "bottle"; break;
      
  case 1:
    siftInfo.typeobject = "rubiks"; break;
          
  default:
    cout << "Please check me in the switch,\n it might have to do with the counter." << endl; // Error message 
  } // End Of Switch
  
  // 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");// HERE!
  if((siftInfo.i==5)&&(siftInfo.count==0)) {
    siftInfo.count++;
    siftInfo.i=0;
  }
  if((siftInfo.count==1)&&(siftInfo.i==5))
    postStateCompletion();//function2()//=C=> endnode
  else
    postStateFailure();//function1();// =T(1000)=> DoStart() //startnode
}
#endnodeclass

#nodeclass Function2 : VisualRoutinesStateNode
  virtual void doStart() {
    SiftVariables &siftInfo = parentAs<SiftTrainBehavior>()->siftInfo;
    siftInfo.mySiftTekkotsu.setParameter("probOfMatch", 0.8);

    if(siftInfo.i>=3) {
      siftInfo.count++;
      siftInfo.i=0;
    } // Condition setter
    switch(siftInfo.count) {
      case 2:
	siftInfo.typeobject = "all"; break; //Possibly not used
      
      case 3:
	siftInfo.typeobject = "world"; break; // Possibly not used
      
      default:
	cout << "Please check me in the switch,\n it might have to do with the counter." << endl; // Error message 
      }
    NEW_SKETCH(camGrey, uchar, sketchFromRawY());
    siftInfo.buff = siftInfo.mySiftTekkotsu.sketchToBuffer(camGrey);
    siftInfo.results.clear();
    //siftInfo.mySiftTekkotsu.findAllObjectsInImage(siftInfo.buff, siftInfo.results);
    siftInfo.mySiftTekkotsu.findInImage( siftInfo.keys, siftInfo.results, false, -1);// It is -1 because we are looking for all objects
    
    int s = siftInfo.results.size();
    cout << "Found " << s << endl;
    for (int k = 0; k < s; k++) {
      siftInfo.results[k]->print("  ");
      
       for(unsigned int j = 1; j < siftInfo.keys.size(); j++ ) { // Studying keypionts
 	//  if ( keys[j]->modelScale > 6 )
 	//       keys[j]->print();
	
 	if( siftInfo.keys[j]->modelX < siftInfo.minX)
 	  siftInfo.minX = siftInfo.keys[j]->modelX;
 	if( siftInfo.keys[j]->modelY < siftInfo.minY)
 	  siftInfo.minY = siftInfo.keys[j]->modelY;
 	if( siftInfo.keys[j]->modelX > siftInfo.maxX)
 	  siftInfo.maxX = siftInfo.keys[j]->modelX;
 	if( siftInfo.keys[j]->modelY > siftInfo.maxY)
 	  siftInfo.maxY = siftInfo.keys[j]->modelY;
	
 	NEW_SHAPE(myPoint, PointData, PointData(camShS, Point(siftInfo.keys[k]->modelX, siftInfo.keys[k]->modelY, 0)));
       }
       cout << "Min X: " << siftInfo.minX << "\nMin Y: " << siftInfo.minY
 	   << "\nMax X: " << siftInfo.maxX << "\nMax Y: " << siftInfo.maxY << endl;
     }
    ProjectInterface::sendCommand("advance");// HERE TOO!
    
    siftInfo.i++;
  
    if(siftInfo.count<3)
      postStateFailure();//function2() // =T(1000)=> endnode
    else {

      //string passed = "";
	

	  //for(int k=0; k < s; k++){
	    //passed = "Element"+k+":\n";
	   
	siftInfo.mySiftTekkotsu.saveToFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/TekExp/tekkotsu", true);
	cout << "\n*** ** ********\nEnd of Behavior\n*** ** ********" << endl;
    }
  }
#endnodeclass

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

#endnodeclass

REGISTER_BEHAVIOR(SiftTrainBehavior);