#include <iostream>
#include "opencv2/core.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/features2d.hpp"
#include "opencv2/xfeatures2d.hpp"
#include "opencv2/xfeatures2d/nonfree.hpp"
#include <opencv2/core/utils/logger.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/videoio.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/highgui.hpp>      //for imshow
#include <vector>
#include <iostream>
#include <iomanip>
#include <opencv2/features2d.hpp>

// #include <opencv2/xfeatures2d.hpp>
using namespace cv;
using namespace cv::xfeatures2d;
using namespace std;
using std::cout;
using std::endl;
using namespace std::chrono;


int main()
{

    auto start = high_resolution_clock::now();
    Mat img1 = imread("C:/Users/emrec/Documents/MATLAB/Test/63.jpg", IMREAD_GRAYSCALE);
    Mat img2 = imread("C:/Users/emrec/Documents/MATLAB/Test/64.jpg", IMREAD_GRAYSCALE);

    vector<cv::KeyPoint> keypoints1, keypoints2;
    Mat descriptors1, descriptors2;
    vector<cv::DMatch> matches;

    Ptr<FastFeatureDetector> detector = FastFeatureDetector::create(100);
    Ptr<FREAK> freak = FREAK::create();

    detector->detect(img1, keypoints1);
    detector->detect(img2, keypoints2);

    cout << "annen";

    freak->compute(img1, keypoints1, descriptors1);
    freak->compute(img2, keypoints2, descriptors2);
    Ptr<cv::DescriptorMatcher> Matcher_ORB = cv::BFMatcher::create(cv::NORM_HAMMING);
    Matcher_ORB->match(descriptors1, descriptors2, matches);

    sort(matches.begin(), matches.end());
    const int match_size = matches.size();
    vector<cv::DMatch> good_matches(matches.begin(), matches.begin() + (int)(match_size * 0.5f));

    for (int i = 0; i < good_matches.size(); i++) {
        cout << "asdasd" << endl;
        cout << (keypoints1[good_matches[i].queryIdx].pt.y - keypoints2[good_matches[i].trainIdx].pt.y) << endl;
        cout << (keypoints1[good_matches[i].queryIdx].pt.x - keypoints2[good_matches[i].trainIdx].pt.x) << endl;
    }

    //-- Draw matches
    Mat img_matches;
    drawMatches(img1, keypoints1, img2, keypoints2, good_matches, img_matches, Scalar::all(-1),
        Scalar::all(-1), std::vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
    //-- Show detected matches
    imshow("Good Matches", img_matches);

    cout << (keypoints1[good_matches[0].queryIdx].pt.y - keypoints2[good_matches[0].trainIdx].pt.y) << endl;
    auto stop = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(stop - start);
    cout << duration.count() << endl;
    waitKey();
    return 0;
}