#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 SiftShowBehavior : VisualRoutinesStateNode : doStart

SiftTekkotsu mySiftTekkotsu;
ImageBuffer buff;
vector<SiftMatch*> matches;
double minX, minY = minX = 1000000.00;
double maxX, maxY = maxX = 0.0;

mySiftTekkotsu.loadFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/tekkotsu.date");
NEW_SKETCH(camGrey, uchar, sketchFromRawY());
buff = mySiftTekkotsu.sketchToBuffer(camGrey);
mySiftTekkotsu.findAllObjectsInImage(buff, matches);

if (matches.size() == 0) return;

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);
 }



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");
 }

int s = points.size();    
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(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 ) ) );

#endnodeclass

REGISTER_BEHAVIOR(SiftShowBehavior);