#include <iostream>
#include "opencv2/opencv.hpp"

using namespace std;
using namespace cv;

void keypoint_matching() {
	Mat src1 = imread("../../resources/images/box.png", IMREAD_GRAYSCALE);
	Mat src2 = imread("../../resources/images/box_in_scene.png", IMREAD_GRAYSCALE);

	Ptr<Feature2D> feature = ORB::create();

	vector<KeyPoint> keypoints1, keypoints2;
	Mat desc1, desc2;
	feature->detectAndCompute(src1, Mat(), keypoints1, desc1);
	feature->detectAndCompute(src2, Mat(), keypoints2, desc2);

	Ptr<DescriptorMatcher> matcher = BFMatcher::create(NORM_HAMMING);

	vector<DMatch> matches;
	matcher->match(desc1, desc2, matches);

	Mat dst;
	drawMatches(src1, keypoints1, src2, keypoints2, matches, dst);

	imshow("dst", dst);

	waitKey(0);
	destroyAllWindows();
}

void good_matching() {
	Mat src1 = imread("../../resources/images/box.png", IMREAD_GRAYSCALE);
	Mat src2 = imread("../../resources/images/box_in_scene.png", IMREAD_GRAYSCALE);

	Ptr<Feature2D> feature = ORB::create();

	vector<KeyPoint> keypoints1, keypoints2;
	Mat desc1, desc2;
	feature->detectAndCompute(src1, Mat(), keypoints1, desc1);
	feature->detectAndCompute(src2, Mat(), keypoints2, desc2);

	Ptr<DescriptorMatcher> matcher = BFMatcher::create(NORM_HAMMING);

	vector<DMatch> matches;
	matcher->match(desc1, desc2, matches);

	sort(matches.begin(), matches.end());

	vector<DMatch> good_matches(matches.begin(), matches.begin() + 50);

	Mat dst;
	drawMatches(src1, keypoints1, src2, keypoints2, good_matches, dst, Scalar::all(-1), Scalar::all(-1), vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

	imshow("dst", dst);

	waitKey(0);
	destroyAllWindows();
}

void find_homography() {
	Mat src1 = imread("../../resources/images/box.png", IMREAD_GRAYSCALE);
	Mat src2 = imread("../../resources/images/box_in_scene.png", IMREAD_GRAYSCALE);

	Ptr<Feature2D> orb = ORB::create();

	vector<KeyPoint> keypoints1, keypoints2;
	Mat desc1, desc2;
	orb->detectAndCompute(src1, Mat(), keypoints1, desc1);
	orb->detectAndCompute(src2, Mat(), keypoints2, desc2);

	Ptr<DescriptorMatcher> matcher = BFMatcher::create(NORM_HAMMING);

	vector<DMatch> matches;
	matcher->match(desc1, desc2, matches);

	sort(matches.begin(), matches.end());

	vector<DMatch> good_matches(matches.begin(), matches.begin() + 50);

	Mat dst;
	drawMatches(src1, keypoints1, src2, keypoints2, good_matches, dst, Scalar::all(-1), Scalar::all(-1), vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

	vector<Point2f> pts1, pts2;
	for (size_t i = 0; i < good_matches.size(); i++) {
		pts1.push_back(keypoints1[good_matches[i].queryIdx].pt);
		pts2.push_back(keypoints2[good_matches[i].trainIdx].pt);
	}

	Mat H = findHomography(pts1, pts2, RANSAC);

	vector<Point2f> corners1, corners2;
	corners1.emplace_back(0.f, 0.f);
	corners1.emplace_back(src1.cols - 1.f, 0.f);
	corners1.emplace_back(src1.cols - 1.f, src1.rows - 1.f);
	corners1.emplace_back(0.f, src1.rows - 1.f);
	perspectiveTransform(corners1, corners2, H);

	vector<Point> corners_dst;
	for (Point2f pt : corners2) {
		corners_dst.emplace_back(cvRound(pt.x + src1.cols), cvRound(pt.y));
	}

	polylines(dst, corners_dst, true, Scalar(0, 255, 0), 2, LINE_AA);

	imshow("dst", dst);

	waitKey(0);
	destroyAllWindows();
}

int main() {
	find_homography();
}