// 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>

using namespace DualCoding;


#nodeclass KeypointsBehavior : VisualRoutinesStateNode
virtual void doStart() {
  SiftTekkotsu mySiftTekkotsu;
  ImageBuffer buff;
  vector<keypoint*> temp;
  vector< vector< vector<int> > > tmp;
  vector<keypoint*>& keys = temp;
  vector <vector <vector <int> > >& gaussianSpace = tmp;
  NEW_SKETCH(camGrey, uchar, sketchFromRawY());
  buff = mySiftTekkotsu.sketchToBuffer(camGrey);
  mySiftTekkotsu.detectKeypoints(buff, keys, gaussianSpace);
  
  for(unsigned int i = 1; i < keys.size(); i++ )
    if ( keys[i]->modelScale > 6 )
      keys[i]->print();
}

#endnodeclass

REGISTER_BEHAVIOR(KeypointsBehavior);