//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 DatabaseOpenBehavior : 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 StartFunction : StateNode 
virtual void doStart() {
  SiftVariables &siftInfo = parentAs<DatabaseOpenBehavior>()->siftInfo;
  siftInfo.mySiftTekkotsu.loadFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/tekkotsu.dat");
  postStateCompletion();
}
#endnodeclass

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

  if(siftInfo.i<7) { // Change parameters for ammount of test images here
    string name = "camGrey" + siftInfo.i;// name for the current sketch
    NEW_SKETCH(camGrey, uchar, sketchFromRawY());
    siftInfo.buff = siftInfo.mySiftTekkotsu.sketchToBuffer(camGrey);
    siftInfo.results.clear();
    siftInfo.mySiftTekkotsu.findAllObjectsInImage(siftInfo.buff, siftInfo.results);
    //siftInfo..findInImage(buffer, matchesFound, false, -1);
    //siftInfo.mySiftTekkotsu.findInImage(siftInfo.keys, siftInfo.results, false, -1);// It is -1 because we are looking for all objects
    
    // string ne; // actual name of the points per picture
    int s = siftInfo.results.size();
    cout << "Found " << s << endl;
    for (int k = 0; k < s; k++) {
      siftInfo.results[k]->print("  "); //testSIFTImage.keypoints
      
      // ne = "myPoint" + k;
      unsigned int max;
      if(siftInfo.mySiftTekkotsu.testSIFTImage.keypoints.size() >= 100)
	max = 100;
	else
	  max = siftInfo.mySiftTekkotsu.testSIFTImage.keypoints.size();
      //VRmixin::camShS.clear();
      for(unsigned int j = 0; j < max; j++ ) { // Studying keypionts
	//  if ( keys[j]->modelScale > 6 )
	//       keys[j]->print();
	
	if( siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelX < siftInfo.minX)
	  siftInfo.minX = siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelX;
	if( siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelY < siftInfo.minY)
	  siftInfo.minY = siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelY;
	if( siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelX > siftInfo.maxX)
	  siftInfo.maxX = siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelX;
	if( siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelY > siftInfo.maxY)
	  siftInfo.maxY = siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelY;
	
	// NEW_SHAPE( n, PointData, PointData(camShS, Point(siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelX,
	// 							      siftInfo.mySiftTekkotsu.testSIFTImage.keypoints[j]->modelY, 0)));
	
      }
      NEW_SHAPE(n1, LineData, LineData(camShS, EndPoint(siftInfo.minX, siftInfo.minY, 0 ),EndPoint( siftInfo.minX, siftInfo.maxY, 0 ) ) );
      NEW_SHAPE(n2, LineData, LineData(camShS, EndPoint(siftInfo.minX, siftInfo.maxY, 0 ),EndPoint( siftInfo.maxX, siftInfo.maxY, 0 ) ) );
      NEW_SHAPE(n3, LineData, LineData(camShS, EndPoint(siftInfo.maxX, siftInfo.maxY, 0 ),EndPoint( siftInfo.maxX, siftInfo.minY, 0 ) ) );
      NEW_SHAPE(n4, LineData, LineData(camShS, EndPoint(siftInfo.maxX, siftInfo.minY, 0 ),EndPoint( siftInfo.minX, siftInfo.minY, 0 ) ) );
      cout << "Min X: " << siftInfo.minX << "\nMin Y: " << siftInfo.minY
	   << "\nMax X: " << siftInfo.maxX << "\nMax Y: " << siftInfo.maxY << endl;
    }
    siftInfo.i++;
    ProjectInterface::sendCommand("advance");
    postStateFailure();
  }
  else {
    cout << "\n*** ** ********\nEnd of Behavior\n*** ** ********" << endl;
  }
}
#endnodeclass

virtual void setup() {
#statemachine
 startnode: StartFunction =C=> node2
    node2 : Function =F=> StateNode =T(100)=> node2 
#endstatemachine
    }

#endnodeclass

REGISTER_BEHAVIOR(DatabaseOpenBehavior);