#include <stdio.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc_c.h>
#include <cv.h>
#include <stdio.h>
#include <opencv2/features2d/features2d.hpp>

#include <iostream>
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include<opencv2/nonfree/nonfree.hpp>

using namespace std;
using namespace cv;

				
int main(int argc, char** argv)
{
	
  Mat image1, outImg1, image2, outImg2,image3;
CvMat* gray =0; 
int removed,key = 0;
double maxdist = 100; double mindist = 2;
// vector of keypoints
vector<KeyPoint> keypoints1, keypoints2;
CvCapture* capture = cvCaptureFromCAM(-1);
SurfFeatureDetector detector(800,4,2,true,false);
FlannBasedMatcher matcher;
for(int i=0;i<2;i++)
{IplImage* frame1 = cvQueryFrame(capture);}

image1 = imread("Card.jpg",1);
 detector.detect(image1, keypoints1);

 SurfDescriptorExtractor surfDesc;
					Mat descriptors1, descriptors2;
					
					surfDesc.compute(image1, keypoints1, descriptors1);

				
					
					//camera input 

while( key != 'q' )

        {  
	         IplImage* frame = cvQueryFrame(capture);
				if(!frame)
				break;
				if(!gray)
				{
				image2 = cvCreateMat(frame->height, frame->width, CV_8UC1);
				image2=frame;
				}

               namedWindow("image3");
			imshow("image3", image2);

					// Read input images
					
					//image3 = imread("Hand2.jpg",0);
					//flip(image2,image3,1);

					detector.detect(image2, keypoints2);
					drawKeypoints(image1, keypoints1, outImg1, Scalar(255,255,255), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
					drawKeypoints(image2, keypoints2, outImg2, Scalar(255,255,255), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

					namedWindow("SURF detector img1");
					imshow("SURF detector img1", outImg1);

					namedWindow("SURF detector img2");
					imshow("SURF detector img2", outImg2);

					surfDesc.compute(image2, keypoints2, descriptors2);

		//  std::vector<std::vector<cv::DMatch>> matches;
				//	matcher.knnMatch(descriptors1,descriptors2,matches,2);
                vector<DMatch>matches;
				vector<DMatch>matches2;
	    matcher.match(descriptors1,descriptors2,matches);

		matcher.match(descriptors2,descriptors1,matches2);

////ROUGH
		std::vector< DMatch > good_matches;
for( int i = 0; i<matches.size(); i++ )
{
	for( int j = 0; j<matches2.size(); j++ )
	{if(matches[i].queryIdx==matches2[j].trainIdx && matches2[j].queryIdx==matches[i].trainIdx)
	{good_matches.push_back(matches[i]);}
	}
}

					
////ROUGH
                    //nth_element(matches.begin(), matches.begin()+30, matches.end());
				    // matches.erase(matches.begin()+31, matches.end());

						
					 
					 /*	std::vector<uchar> inliers(keypoints1.size(),0);
								Mat homography= findHomography(
								keypoints1, // corresponding
								keypoints2, // points
								inliers, // outputted inliers matches
								CV_RANSAC, // RANSAC method
								1.);      */

						

					 //-- Quick calculation of max and min distances between keypoints
					//for( int i = 0; i < goodmatches.size(); i++ )
					//	{ double dist = goodmatches[i].distance;
					//	if( dist < mindist ) mindist = dist;
					//	if( dist > maxdist ) maxdist = dist;
						//}
						//-- Draw only "good" matches (i.e. whose distance is less than 2*min_dist )
						//-- PS.- radiusMatch can also be used here.
						
				//	for( int i = 0; i < goodmatches.size(); i++ )
					//	{ if( goodmatches[i].distance < mindist )
					//	{ good_matches.push_back( matches[i]); }
				//		}
//

   // nth_element(good_matches.begin(), good_matches.begin()+10,good_matches.end());
	//	good_matches.erase(good_matches.begin()+11, good_matches.end());

Mat imageMatches;

		//drawMatches(image1, keypoints1, image2, keypoints2, matches, imageMatches, Scalar(100,255,0));
 drawMatches( image1, keypoints1, image2, keypoints2,good_matches  //matches/good_matches
	 ,imageMatches, Scalar(60,100,0), Scalar::all(-1),vector<char>(),DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
			//namedWindow("Matched");
		//imshow("Matched", imageMatches);

std::vector<Point2f> obj;
std::vector<Point2f> scene;
for( int i = 0; i<good_matches.size(); i++ )
{
//-- Get the keypoints from the good matches
obj.push_back( keypoints1[ good_matches[i].queryIdx ].pt );
scene.push_back( keypoints2[ good_matches[i].trainIdx ].pt );
}

std::vector<DMatch> inliers;
Mat H = findHomography( obj, scene,CV_RANSAC );
//-- Get the corners from the image_1 ( the object to be "detected" )
std::vector<Point2f> obj_corners(4);
obj_corners[0] = cvPoint(0,0); 
obj_corners[1] = cvPoint( image1.cols, 0 );
obj_corners[2] = cvPoint( image1.cols, image1.rows ); 
obj_corners[3] = cvPoint( 0, image1.rows );
std::vector<Point2f> scene_corners(4);
perspectiveTransform(obj_corners,scene_corners,H);

//-- Draw lines between the corners (the mapped object in the scene - image_2 )

line( imageMatches, scene_corners[0] + Point2f( image1.cols, 0), scene_corners[1] + Point2f( image1.cols, 0),Scalar(0,255,0),3,8,0); 
line( imageMatches, scene_corners[1] + Point2f( image1.cols, 0), scene_corners[2] + Point2f( image1.cols, 0),Scalar(255,0,0),3,8,0); 
line( imageMatches, scene_corners[2] + Point2f( image1.cols, 0), scene_corners[3] + Point2f( image1.cols, 0),Scalar(0,255,0),3,8,0); 
line( imageMatches, scene_corners[3] + Point2f( image1.cols, 0), scene_corners[0] + Point2f( image1.cols, 0),Scalar(0,0,255),3,8,0);  

line( image2, scene_corners[0], scene_corners[1] ,Scalar(0,255,0),6,4,0); 
line( image2, scene_corners[1], scene_corners[2],Scalar(255,0,0),6,4,0); 
line( image2, scene_corners[2], scene_corners[3],Scalar(0,255,0),6,4,0); 
line( image2, scene_corners[3], scene_corners[0] ,Scalar(100,0,100),6,4,0);  
flip(image2,image3,1);

imshow( "Good Matches & Object detection", imageMatches );

namedWindow("image2");
			imshow("image2", image3);

					waitKey(2);

 }

cvDestroyWindow("Matched");
cvDestroyWindow("SURF detector img2");
cvDestroyWindow("SURF detector img1");
cvReleaseCapture( &capture );

return 0;
}