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

using namespace cv;
using namespace std;


int main(int argc, char *argv[]) {
    Mat img1, img2;
    img1 = imread(argv[1]);
    img2 = imread(argv[2]);
    Ptr<ORB> orb = ORB::create(50);
    vector<KeyPoint> kp1, kp2;
    Mat desc1, desc2;
    cout<<"orb\n";
    orb->detectAndCompute(img1, noArray(), kp1, desc1);
    orb->detectAndCompute(img2, noArray(), kp2, desc2);
    vector< vector<DMatch> > matches, matches1, matches2;
    /* Options
    BruteForce (it uses L2 )
    BruteForce-L1
    BruteForce-Hamming
    BruteForce-Hamming(2)
    FlannBased
    */
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce-Hamming");
    cout<<"matcher\n";
    /* Options
    match (espera só um vector<DMatch>)
    knnMatch (precisa de um int)
    radiusMatch (precisa de um float)
    */
    matcher->knnMatch(desc1, desc2, matches, 2);
    Mat out_img;
    drawMatches(img1, kp1, img2, kp2, matches, out_img, Scalar(255,255,255));
    imshow("Result",out_img);
    waitKey(0);
    vector< vector<DMatch> > good_matches;
    vector<Point2f> good_pt1, good_pt2;
    std::vector<Point2f> obj;
    for (int i = 0; i < matches.size(); i++)
        if (matches[i][0].distance < 0.7*matches[i][1].distance){
            good_matches.push_back(matches[i]);
            good_pt1.push_back(kp1[matches[i][0].queryIdx].pt) ;
            good_pt2.push_back(kp2[matches[i][1].trainIdx].pt) ;
        }
    drawMatches(img1, kp1, img2, kp2, good_matches, out_img, Scalar(255,255,255));
    imshow("Result",out_img);
    waitKey(0);

    Mat H = findHomography( good_pt1, good_pt2, RANSAC );
    std::vector<Point2f> temp_corners(4), obj_corners1(4), obj_corners2(4);
    obj_corners1[0] = cvPoint(0,0);
    obj_corners1[1] = cvPoint( img1.cols, 0 );
    obj_corners1[2] = cvPoint( img1.cols, img1.rows );
    obj_corners1[3] = cvPoint( 0, img1.rows );
    temp_corners[0] = cvPoint(0,0);
    temp_corners[1] = cvPoint( img2.cols, 0 );
    temp_corners[2] = cvPoint( img2.cols, img2.rows );
    temp_corners[3] = cvPoint( 0, img2.rows );

    perspectiveTransform( temp_corners, obj_corners2, H);
    obj_corners1.insert( obj_corners1.end(), obj_corners2.begin(), obj_corners2.end() );
    float x_max = 0, y_max = 0, x_min = 10000, y_min= 10000;
    for (int i = 0; i < obj_corners1.size(); i++) {
        if (obj_corners1[i].x > x_max) {
            x_max = obj_corners1[i].x + 0.5;
            y_max = obj_corners1[i].y + 0.5;
        }
        if (obj_corners1[i].x < x_min) {
            x_min = obj_corners1[i].x - 0.5;
            y_min = obj_corners1[i].y - 0.5;
        }
    }
    float data[9] = { 1, 0, -x_min, 0, 1, -y_min, 0, 0, 1};
    Mat transf_matrix(H.rows, H.cols, H.type(), data) ;
    std::cout<<transf_matrix<<" WP\n";
    warpPerspective(img2, out_img, transf_matrix.dot(H), out_img.size()*2);
    std::cout<<transf_matrix<<" GG\n";
    imshow("Result",out_img);
    waitKey(0);
}