#include <iostream>
#include <string>
#include <boost/timer.hpp>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/flann/flann.hpp"

using namespace std;
using namespace cv;

void readme();
string type2str(int type);

int main( int argc, char** argv )
{
  if( argc != 3 )
  { 
    readme();
    return -1;
  }

  Mat img_1 = imread( argv[1], CV_LOAD_IMAGE_GRAYSCALE );
  Mat img_2 = imread( argv[2], CV_LOAD_IMAGE_GRAYSCALE );

  if( !img_1.data || !img_2.data )
  { 
    cout<< " --(!) Error reading images " << endl; 
    return -1; 
  }

  //-- Step 1: Detect the keypoints using ORB Detector
  
  cv::Ptr<cv::ORB> orb = cv::ORB::create(200);

  std::vector<KeyPoint> keypoints_1, keypoints_2;

  orb->detect( img_1, keypoints_1 );
  orb->detect( img_2, keypoints_2 );
  //-- Step 2: Calculate descriptors (feature vectors)

  Mat descriptors_1, descriptors_2;

  // descriptor is a cv::Mat, with rows the same as nFeatures, and cols as 32 (8UC1)
  orb->compute( img_1, keypoints_1, descriptors_1 );
  orb->compute( img_2, keypoints_2, descriptors_2 );
  cout << type2str(descriptors_1.type()) << " " << descriptors_1.rows << "*" << descriptors_1.cols << endl;; 

  //-- Step 3: Matching descriptor vectors using FLANN matcher
  FlannBasedMatcher matcher;
  std::vector<DMatch> matches;

  // the descriptor for FlannBasedMatcher should has matrix element of CV_32F
  if( descriptors_1.type()!=CV_32F ) 
  {
    descriptors_1.convertTo( descriptors_1, CV_32F );
    descriptors_2.convertTo( descriptors_2, CV_32F );
  }
  matcher.match( descriptors_1, descriptors_2, matches );
  


  double min_dist = min_element( matches.begin(), 
                 matches.end(), 
                 []( const DMatch& d1, const DMatch& d2 )->double
                 {
                   return d1.distance < d2.distance;
                 } )->distance;

  cout << min_dist << endl;

  vector<DMatch> good_matches;

  for( int i = 0; i < descriptors_1.rows; i++ )
  { 
    if( matches[i].distance < max<double>( min_dist*2, 60.0 ) )
    { 
      good_matches.push_back( matches[i]); 
    }
  }

  Mat img_matches;
  drawMatches( img_1, keypoints_1, img_2, keypoints_2,
               good_matches, img_matches, Scalar::all(-1), Scalar::all(-1),
               vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );

  //-- Show detected matches
  imshow( "Good Matches", img_matches );

  for( int i = 0; i < good_matches.size(); i++ )
  { 
    cout << good_matches[i].queryIdx << " --- " << good_matches[i].trainIdx << endl; 
  }

  waitKey(0);

  return 0;
}

void readme()
{ 
  cout << " Usage: ./ORB_test <img1> <img2>" << endl; 
}

string type2str(int type) 
{
  string r;

  uchar depth = type & CV_MAT_DEPTH_MASK;
  uchar chans = 1 + (type >> CV_CN_SHIFT);

  switch ( depth ) {
    case CV_8U:  r = "8U"; break;
    case CV_8S:  r = "8S"; break;
    case CV_16U: r = "16U"; break;
    case CV_16S: r = "16S"; break;
    case CV_32S: r = "32S"; break;
    case CV_32F: r = "32F"; break;
    case CV_64F: r = "64F"; break;
    default:     r = "User"; break;
  }

  r += "C";
  r += (chans+‘0‘);

  return r;
}