#include <opencv2/imgproc.hpp>
#include <opencv2/ml.hpp>
#include <vector>
#include "opencv2/core/core_c.h"

using namespace std;
using namespace cv;
using namespace cv::ml;

using cv::Ptr;

int main(){

    Mat xTrainData = (Mat_<float>(5,2) << 1, 1, 1, 1, 2, 2, 2, 2, 2, 2);
    Mat yTrainLabels = (Mat_<float>(5,1) << 1, 1, 2, 2, 2);

    cout << "===============xTrainData===================="<< endl;
    cout << "xTrainData: "  << xTrainData<< endl;

    cout << "===============yTrainLabels===================="<< endl;
    cout << "yTrainLabels: "  << yTrainLabels << endl;

    // KNearest KDTree implementation
    Ptr<KNearest> knnKdt = KNearest::create();
    knnKdt->setAlgorithmType(KNearest::KDTREE);
    knnKdt->setIsClassifier(true);

   // When runs to this step, it does not run any more.
    knnKdt->train(xTrainData, ml::ROW_SAMPLE, yTrainLabels);

    float d_test[2][2] = {{1.3, 1.3}, {2.2, 2.4}};

    Mat xTestData = (Mat_<float>(2,1) << 1.1, 1.1, 2, 2.2);
    Mat zBestLabels;
    cout << "===============xTestData===================="<< endl;
    cout << "xTestData: "  << xTestData << endl;

    knnKdt->findNearest(xTestData, 1, zBestLabels);
    cout << "===============zBestLabels===================="<< endl;
    cout << "zBestLabels: "  << zBestLabels << endl;
}