//-*-c++-*-
#include "Behaviors/StateMachine.h"
#include "DualCoding/DualCoding.h"
#include "Vision/SIFT/API/SiftTekkotsu.h"

#include "BoundingBoxOps.h"

#include <vector>

using namespace DualCoding;

#nodeclass SIFTOpenBehavior : VisualRoutinesStateNode
  virtual void doStart() {
    SiftTekkotsu mySiftTekkotsu;
    vector<SiftMatch*> rt;
    NEW_SKETCH(camGrey, uchar, sketchFromRawY());
    // NEW_SKETCH(camGrey, uchar, sketchFromChannel(RawCameraGenerator::CHAN_Y));
    ImageBuffer buff = mySiftTekkotsu.sketchToBuffer(camGrey);
    mySiftTekkotsu.setParameter("probOfMatch", 0.8);
    mySiftTekkotsu.loadFile("/afs/cs.cmu.edu/user/joehan/sift-toolData/tekkotsu.dat");
    mySiftTekkotsu.getObjectID("bottle1");
    mySiftTekkotsu.findAllObjectsInImage(buff, rt);//rt is a reference!
    cout << "Found " << rt.size() << endl;//Start from here Debug
    if(rt.size()>0)
    rt[0]->print("  ");
    
    // Not part of the original behavior, actually part of SIFTOpenBehavior2.0
    
    double minX, minY = minX = 10000000.00;
    double maxX, maxY = maxX = 0;
    cout<<mySiftTekkotsu.testSIFTImage.keypoints[0]->modelX<<endl;
    
    int s = mySiftTekkotsu.testSIFTImage.keypoints.size();
  vector<point> points;
  point pointA;
  for ( int i = 0; i < s; i++ ) {
    pointA.x = mySiftTekkotsu.testSIFTImage.keypoints[i]->modelX;
    pointA.y = mySiftTekkotsu.testSIFTImage.keypoints[i]->modelY;
    points.push_back(pointA);
  }
   
  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;
    
    if ( i < 50 ) {
      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";
  
  
  cout << "\n*** ** ********\nEnd of Behavior\n*** ** ********" << endl;
  }
#endnodeclass

REGISTER_BEHAVIOR(SIFTOpenBehavior);