#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/features2d.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/ml.hpp>

using namespace cv;
using namespace cv::ml;

void demoSVM()
{
    int labels[20] = { 1, 2, 3, 4, 1, 2, 3, 4, 1, 2, 3, 4, 1, 2, 3, 4, 1, 2, 3, 4 };
    float trainingData[20][2] = { 
        {501, 10}, {255, 10}, {501, 255}, {10, 501}, 
        {511, 10}, {245, 12}, {505, 235}, {12, 506},
        {521, 11}, {232, 16}, {508, 245}, {16, 498},
        {516, 14}, {265, 15}, {510, 242}, {18, 481},
        {491, 19}, {275, 17}, {515, 237}, {17, 520}
    };
    Mat trainingDataMat(20, 2, CV_32F, trainingData);
    Mat labelsMat(20, 1, CV_32SC1, labels);
    // Train the SVM
    Ptr<SVM> svm = SVM::create();
    svm->setType(SVM::C_SVC);
    svm->setKernel(SVM::LINEAR);
    svm->setTermCriteria(TermCriteria(TermCriteria::MAX_ITER, 100, 1e-6));
    svm->train(trainingDataMat, ROW_SAMPLE, labelsMat);
    // Data for visual representation
    int width = 512, height = 512;
    Mat image = Mat::zeros(height, width, CV_8UC3);
    // Show the decision regions given by the SVM
    Vec3b green(0, 255, 0), blue(255, 0, 0), red(0,0,255), yellow(0,255,255);
    for (int i = 0; i < image.rows; i++)
    {
        for (int j = 0; j < image.cols; j++)
        {
            Mat sampleMat = (Mat_<float>(1, 2) << j, i);
            float response = svm->predict(sampleMat);
            if (response == 1)
                image.at<Vec3b>(i, j) = green;
            else if (response == 2)
                image.at<Vec3b>(i, j) = blue;
            else if (response == 3)
                image.at<Vec3b>(i, j) = red;
            else if (response == 4)
                image.at<Vec3b>(i, j) = yellow;
        }
    }
    // Show the training data
    int thickness = -1;
    circle(image, Point(501, 10), 5, Scalar(0, 0, 0), thickness);
    circle(image, Point(255, 10), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(501, 255), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(10, 501), 5, Scalar(255, 255, 255), thickness);

    circle(image, Point(511, 10), 5, Scalar(0, 0, 0), thickness);
    circle(image, Point(245, 12), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(505, 235), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(12, 506), 5, Scalar(255, 255, 255), thickness);

    circle(image, Point(521, 11), 5, Scalar(0, 0, 0), thickness);
    circle(image, Point(232, 16), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(508, 245), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(16, 498), 5, Scalar(255, 255, 255), thickness);

    circle(image, Point(516, 14), 5, Scalar(0, 0, 0), thickness);
    circle(image, Point(265, 15), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(510, 242), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(18, 481), 5, Scalar(255, 255, 255), thickness);

    circle(image, Point(491, 19), 5, Scalar(0, 0, 0), thickness);
    circle(image, Point(275, 17), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(515, 237), 5, Scalar(255, 255, 255), thickness);
    circle(image, Point(17, 520), 5, Scalar(255, 255, 255), thickness);
    // Show support vectors
    thickness = 2;
    Mat sv = svm->getUncompressedSupportVectors();
    for (int i = 0; i < sv.rows; i++)
    {
        const float* v = sv.ptr<float>(i);
        circle(image, Point((int)v[0], (int)v[1]), 6, Scalar(128, 128, 128), thickness);
    }
    imwrite("result.png", image);        // save the image
    imshow("SVM Simple Example", image); // show it to the user
    waitKey();
}

void demoExtractInterestPoints()
{
    const char* img = "F:/jarmu/datasets/tsinghua/data/marks/pad-all/i1.png";
    const char* img2 = "F:/jarmu/datasets/tsinghua/data/train/23.jpg";

    cv::Mat I = cv::imread(img);
    cv::Mat I2 = cv::imread(img2);
    cv::Mat gray, gray2, gray_smaller, gray_smaller_2;

    cv::cvtColor(I, gray, cv::COLOR_BGR2GRAY);
    cv::cvtColor(I2, gray2, cv::COLOR_BGR2GRAY);

    //cv::Mat GrayA = cv::imread(PimA);
    //cv::Mat GrayB = cv::imread(PimB);
    //std::vector<cv::KeyPoint> keypointsA, keypointsB;
    //cv::Mat descriptorsA, descriptorsB;
    int thresh = 80;
    int octaves = 8;

    int  	nfeatures = 500;
    float  	scaleFactor = 1.2f;
    int  	nlevels = 4;
    int  	edgeThreshold = 1;
    int  	firstLevel = 0;
    int  	WTA_K = 1;
    cv::ORB::ScoreType scoreType = ORB::HARRIS_SCORE;
    int  	patchSize = 31;
    int  	fastThreshold = 5;

    cv::resize(gray, gray_smaller, cv::Size(gray.cols * 0.75, gray.rows * 0.75), 0, 0);
    cv::resize(gray2, gray_smaller_2, cv::Size(), 0.25, 0.25);

    Ptr<BRISK> BRISKD = BRISK::create();//thresh, octaves, 1.0);
    //Ptr<ORB> BRISKD = ORB::create(nfeatures,scaleFactor,nlevels,edgeThreshold,firstLevel,WTA_K,scoreType,patchSize,fastThreshold);// Threshl, Octaves, PatternScales); //initialize algoritm
    std::vector<KeyPoint> keypoints, keypoints2;

    for(int i=0;i<keypoints.size();i++)
            keypoints[i]

    // be kell jaratni?!
    BRISKD->detect(gray, keypoints);
    std::chrono::steady_clock::time_point begin = std::chrono::steady_clock::now();
    BRISKD->detect(gray_smaller, keypoints2);
    std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();


    std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::milliseconds>(end - begin).count() << "[ms]" << std::endl;
    //-- Draw keypoints
    Mat img_keypoints;
    drawKeypoints(gray_smaller, keypoints2, img_keypoints);
    //-- Show detected (drawn) keypoints
    imshow("BRISK Keypoints", img_keypoints);
    waitKey();

    std::vector<KeyPoint> keypointsD;
    Ptr<FastFeatureDetector> detector = FastFeatureDetector::create(200);
    std::vector<Mat> descriptor;

    detector->detect(gray, keypointsD, Mat());

    std::chrono::steady_clock::time_point begin2 = std::chrono::steady_clock::now();
    detector->detect(gray_smaller_2, keypointsD, Mat());
    std::chrono::steady_clock::time_point end2 = std::chrono::steady_clock::now();
    std::cout << "Time difference = " << std::chrono::duration_cast<std::chrono::milliseconds>(end2 - begin2).count() << "[ms]" << std::endl;
    drawKeypoints(I, keypointsD, img_keypoints);
    imshow("keypoints", img_keypoints);
    waitKey();
}

int main()
{
    demoExtractInterestPoints();
    //demoSVM();
    return 0;
}