// This is a behavior that prints out what is inside a keypoint.
// It was originally made to try and see if a boundary box was
// possible to make.
#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>

#include "noiseremove.h"

using namespace DualCoding;
using namespace std;


#nodeclass KeypointStudyBehavior : VisualRoutinesStateNode
virtual void doStart() {
  // double minX,  minY = minX = 1000000000.00;
  //   double maxX,  maxY = maxX = 0;
  SiftTekkotsu mySiftTekkotsu;
  ImageBuffer buff;
  // vector<keypoint*> temp;
  //   vector< vector< vector<int> > > tmp;
//   vector<keypoint*>& keys = temp;
//   vector <vector <vector <int> > >& gaussianSpace = tmp;
  vector<SiftMatch*> matches;
  
  mySiftTekkotsu.loadFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/tekkotsu.dat");
  NEW_SKETCH(camGrey, uchar, sketchFromRawY());
  buff = mySiftTekkotsu.sketchToBuffer(camGrey);
  mySiftTekkotsu.findAllObjectsInImage(buff, matches);
  
    vector<point> points;
   point p = point();
   for ( vector<keypointPair>::const_iterator it = matches[0]->inliers.begin();
 	it != matches[0]->inliers.end(); it++ ) {
     p.x = it->getKey1()->modelX;
     p.y = it->getKey1()->modelY;
     points.push_back(p);
   }
   
   // points = noiseRemoveEuclidean(points, buff);
  if (matches.size() == 0) return;
  
  // double const tx = matches[0]->columnTranslation;
//   double const ty = matches[0]->rowTranslation;
//   double const scale = matches[0]->scale;
  for ( vector<point>::const_iterator it = points.begin();
 	it != points.end(); it++ ) {
     NEW_SHAPE(myPoint, PointData, PointData(camShS, Point(it->x, it->y)));
          myPoint->setColor("green");
  }

  // for ( vector<keypointPair>::const_iterator it = matches[0]->inliers.begin();
// 	it != matches[0]->inliers.end(); it++ ) {
        
//     // int s = keys.size();
// //     for ( int i = 0; i < s; i++ ) {    
// //       if ( keys[i]->modelX < minX)
// // 	minX = keys[i]->modelX;
// //       if ( keys[i]->modelY < minY)
// // 	minY = keys[i]->modelY;
// //       if ( keys[i]->modelX > maxX)
// // 	maxX = keys[i]->modelX;
// //       if ( keys[i]->modelY > maxY)
// // 	maxY = keys[i]->modelY;
      
          
      
//     // if ( it == matches[0]->inliers.begin() ) {
// //       cout << "Key1:\n";
// //       it->getKey1()->print();
// //       cout << "Key2:\n";
// //       it->getKey2()->print();
//     // }

//     NEW_SHAPE(myPoint, PointData, PointData(camShS, Point(it->getKey1()->modelX, it->getKey1()->modelY)));
//     myPoint->setColor("green");

//    //  NEW_SHAPE(urPoint, PointData, PointData(camShS, Point(it->getKey2()->modelX+pow(2.0,scale)*tx, it->getKey2()->modelY+pow(2.0,scale)*ty)));
// //     urPoint->setColor("yellow");
    

//   }
//   NEW_SHAPE(center, PointData, PointData(camShS, Point(matches[0]->columnTranslation, matches[0]->rowTranslation)));
//   center->setColor("blue");
//   matches[0]->print(" ");
//    //  NEW_SHAPE(n1, LineData, LineData(camShS, EndPoint(minX, minY, 0 ),EndPoint( minX, maxY, 0 ) ) );
// //     NEW_SHAPE(n2, LineData, LineData(camShS, EndPoint(minX, maxY, 0 ),EndPoint( maxX, maxY, 0 ) ) );
// //     NEW_SHAPE(n3, LineData, LineData(camShS, EndPoint(maxX, maxY, 0 ),EndPoint( maxX, minY, 0 ) ) );
// //     NEW_SHAPE(n4, LineData, LineData(camShS, EndPoint(maxX, minY, 0 ),EndPoint( minX, minY, 0 ) ) );
// //     cout << "Min X: " << minX << "\nMin Y: " << minY
// // 	 << "\nMax X: " << maxX << "\nMax Y: " << maxY << endl;
//   }
  cout << "*** ** ********\nEnd of Behavior\n^^^ ^^ ^^^^^^^^\n";
}

#endnodeclass

REGISTER_BEHAVIOR(KeypointStudyBehavior);