#include "Feature.hpp"
#include "utils.hpp"

#include "opencv2/highgui/highgui.hpp"
//#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/features2d/features2d.hpp"

#include <iostream>
//#include <list>
#include <vector>

using namespace std;
using namespace cv;

//#include "cv.h"

using namespace std;

int main(int argc, char *argv[]) {

  BriefDescriptorExtractor brief(32);
  
  VideoCapture capture;

  Mat frame, display;

  if( argc == 1 || (argc == 2 && strlen(argv[1]) == 1 && isdigit(argv[1][0]))) // if no parameter or number parameter
    capture.open(argc == 2 ? argv[1][0] - '0' : 0);
  else if( argc >= 2 )
    {
      capture.open(argv[1]);
      if( capture.isOpened() )
	cout << "Video " << argv[1] <<
	  ": width=" << capture.get(CV_CAP_PROP_FRAME_WIDTH) <<
	  ", height=" << capture.get(CV_CAP_PROP_FRAME_HEIGHT) <<
	  ", nframes=" << capture.get(CV_CAP_PROP_FRAME_COUNT) << endl;
      if( argc > 2 && isdigit(argv[2][0]) ) // could be used to reach first frame, dumping library messages to log file (2> /tmp/log.txt)
        {
      	  int pos;
      	  sscanf(argv[2], "%d", &pos);
      	  cout << "seeking to frame #" << pos << endl;
      	  //cap.set(CV_CAP_PROP_POS_FRAMES, pos);
	  for (int i=0; i<pos; i++)
	    capture >> frame;
        }
    }

    //  capture.open(atoi(argv[1]));
  if (!capture.isOpened())
    {
      //help(argv);
      cout << "capture device " << argv[1] << " failed to open!" << endl;
      return 1;
    }
  
  vector<DMatch> matches;
  
  BruteForceMatcher<Hamming> desc_matcher;
  
  vector<Point2f> train_pts, query_pts;
  vector<KeyPoint> train_kpts, query_kpts;
  vector<unsigned char> match_mask;
  
  Mat gray;
  
  Mat train_desc, query_desc;
  const int DESIRED_FTRS = 500;
  GridAdaptedFeatureDetector detector(new FastFeatureDetector(10, true), DESIRED_FTRS, 4, 4);
  
  int frameNum = 0;
  for (;;)
    {
      frameNum+=2;
      capture >> frame;
      cout << capture.get(CV_CAP_PROP_POS_FRAMES) << endl;
      if (frame.empty())
	break;
      
      cvtColor(frame, gray, CV_RGB2GRAY);
      
      detector.detect(gray, query_kpts); //Find interest points
      
      brief.compute(gray, query_kpts, query_desc); //Compute brief descriptors at each keypoint location
      
      // find how keypoints descriptions are matched to previous ones (in train kpts probably)
      display = frame.clone();
      if (!train_kpts.empty())
        {
	  //vector<KeyPoint> test_kpts;
	  //warpKeypoints(H_prev.inv(), query_kpts, test_kpts);
	  //Mat mask = windowedMatchingMask(test_kpts, train_kpts, 25, 25);
	  desc_matcher.match(query_desc, train_desc, matches);
	  drawMatches(frame, train_kpts, frame, query_kpts, matches, display);//, Scalar::all(-1), Scalar::all(-1), vector<vector<char> >(), DrawMatchesFlags::DRAW_OVER_OUTIMG);
	} // TODO do something like the match relative of the sample

      imshow("frame", display);
      train_kpts = query_kpts;
      int key = waitKey(0);
      if (::interruptionKey(key))
	break;
    }  
  
  Feature f;

  return 0;
}


/* ------------------ DOCUMENTATION ------------------ */


/*! \mainpage 

This project is a collection of software tools for transportation called Traffic Intelligence. Other documents are:

- \ref feature_based_tracking

The code is partially self-described using the doxygen tool and comment formatting. The documentation can be extracted using doxygen, typing \c doxygen in the main directory (or <tt>make doc</tt> on a system with the Makefile tool installed). 

*/

/*! \page feature_based_tracking Feature-based Tracking: User Manual

This document describes a software tool for object tracking in video data, developed for road traffic monitoring and safety diagnosis. It is part of a larger collection of software tools for transportation called Traffic Intelligence. 

The tool relies on feature-based tracking, a robust object tracking methods, particularly suited for the extraction of traffic data such as trajectories and speeds. The best description of this method is given in <a href="http://nicolas.saunier.confins.net/data/saunier06crv.html">this paper</a>. The program has a command line interface and this document will shortly explain how to use the tool. Keep in mind this is a work in progress and major changes are continuously being made. 

\section License

The code is licensed under the MIT open source license (http://www.opensource.org/licenses/mit-license).

If you make use of this piece of software, please cite one of my paper, e.g. N. Saunier, T. Sayed and K. Ismail. Large Scale Automated Analysis of Vehicle Interactions and Collisions. Transportation Research Record: Journal of the Transportation Research Board, 2147:42-50, 2010. I would be very happy in any case to know about any use of the code, and to discuss any opportunity for collaboration. 

Contact me at nicolas.saunier@polymtl.ca and learn more about my work at http://nicolas.saunier.confins.net.

*/