// 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 "BoundingBoxOps.h"

#include <vector>
#include <cmath>
#include <string>
#include <iostream>
#include <sstream>

using namespace DualCoding;


#nodeclass KeypointMoveBehavior : VisualRoutinesStateNode
virtual void doStart() {
  double minX,  minY = 1000000000.00;
  minX = minY;
  double maxX,  maxY = 0;
  maxX = 0;
  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);
  int s = keys.size();
  vector<point> points;
  point pointA;
  for( int i = 0; i < s; i++ ) {
    pointA.x = keys[i]->modelX;
    pointA.y = keys[i]->modelY;
    points.push_back(pointA);
  }
   
  rotatePoints( M_PI/2, points );
  
  for( int i = 0; i < s; i++ ) {
    if( points[i].x < minX)
      minX = points[i].x;
    if( points[i].y < minY)
      minY = points[i].y;
    if( points[i].x > maxX)
      maxX = points[i].x;
    if( points[i].y > maxY)
      maxY = points[i].y;
    
    NEW_SHAPE(myPoint, PointData, PointData(camShS, Point(points[i].x, points[i].y, 0)));
    myPoint->setColor("green");
  }
  
  
  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### ## #######\n";
}

#endnodeclass

REGISTER_BEHAVIOR(KeypointMoveBehavior);