#include <iostream>
using namespace std;

#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>
using namespace cv;

void corner_fast()
{
    Mat src = imread("building.jpg", IMREAD_GRAYSCALE);

    if (src.empty())
    {
        cerr << "Image load failed !" << endl;
        return;
    }

    vector<KeyPoint> keypoints;
    FAST(src, keypoints, 60, true);

    Mat dst;
    cvtColor(src, dst, COLOR_GRAY2BGR);

    for (KeyPoint kp : keypoints)
    {
        Point pt(cvRound(kp.pt.x), cvRound(kp.pt.y));
        circle(dst, pt, 5, Scalar(0, 0, 255), 2);
    }

    imshow("src", src);
    imshow("dst", dst);
    imwrite("dst1.png", dst);

    waitKey(0);
    destroyAllWindows();
}

void detect_keypoints()
{
    Mat src = imread("box_in_scene.png", IMREAD_GRAYSCALE);

    if (src.empty())
    {
        cerr << "Image load failed !" << endl;
        return;
    }

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

    vector<KeyPoint> keypoints;
    feature->detect(src, keypoints);

    Mat desc;
    feature->compute(src, keypoints, desc);

    cout << "keypoints.size(): " << keypoints.size() << endl;
    cout << "desc.size(): " << desc.size() << endl;

    Mat dst;
    drawKeypoints(src, keypoints, dst, Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

    imshow("src", src);
    imshow("dst", dst);
    imwrite("dst2.png", dst);

    waitKey();
    destroyAllWindows();
}

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

    if (src1.empty() || src2.empty())
    {
        cerr << "Image load failed !" << endl;
        return;
    }

    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);
    imwrite("dst3.png", dst);

    waitKey();
    destroyAllWindows();
}

int main()
{
    // ex 14-2) FAST 
    corner_fast();

    // ex 14-4) ORB, Keypoints
    detect_keypoints();

    // ex 14-6) ORB, Keypoint Matching
    keypoint_matching();
}