#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/video/tracking.hpp"
#include <iostream>
#include <sstream>
#include <cmath>
#include <algorithm>

#define PI 3.14159265

using namespace std;
using namespace cv;

bool selectObject = false;
Rect selection;
Point origin;
int trackObject = 0;
Mat image;

string ZeroPadNumber(int num);
static void onMouse(int event, int x, int y, int, void*);

float evalXDisp(const vector<Point2f> evalPoints[2]);
float histComp(Mat i1, Mat i2);

int main(int argc, char** argv)
{
    //string path = argv[1];
    string path("../2009_09_08_drive_0019/");
    namedWindow("Map", CV_WINDOW_NORMAL);
    Mat m1 = Mat(1600, 800, CV_8UC1, double(0));
    imshow("Map", m1);
    Point2f mapStart = Point2f(400, m1.rows);
    Mat src, src_prev, img_prev, img;
    unsigned numPoints = 30;
    vector<Point2f> grid;
    vector<Point2f> lk_pts;
    TermCriteria termcrit(TermCriteria::COUNT | TermCriteria::EPS, 30, 0.01);
    Size winSize(31, 31);

    namedWindow("Select ROI", WINDOW_AUTOSIZE);
    setMouseCallback("Select ROI", onMouse, 0);

    while (true) { //Select ROI-----------------------------------
        src = imread(path + "I1_000000.png", 0);
        src.copyTo(image);

        if (selection.width > 0 && selection.height > 0) {
            rectangle(image, selection, Scalar(255));
        }

        imshow("Select ROI", image);
        if ((waitKey(30) & 0xff) == 32) {
            if (selection.width > 0 && selection.height > 0) {
                break;
            }
            else
                cout << "You must select a ROI" << endl;
        }
    }

    //Create ROI's
    destroyWindow("Select ROI");

    //Insert coordinate points
    int sampleSpace = 4;
    Point2f dummy;
    for (int i = 0; i<selection.width; i += sampleSpace) {
        for (int j = 0; j<selection.height; j += sampleSpace) {
            dummy.x = selection.x + i;
            dummy.y = selection.y + j;
            grid.push_back(dummy);
        }
    }


    src.copyTo(img);
    src.copyTo(img_prev);
    Mat roi(img, selection);
    Mat roi_prev(img_prev, selection);
    namedWindow("Sequence", CV_WINDOW_AUTOSIZE);

    vector<float> substract(grid.size());
    vector<float> sig(grid.size());
    vector<float> y(grid.size());
    vector<Point2f>points(1249);


    for (int j = 1; j <= 1248; j++)
    { //Display images-----------------------------------
        src_prev = imread(path + "I1_" + ZeroPadNumber(j - 1) + ".png", 0);
        src = imread(path + "I1_" + ZeroPadNumber(j) + ".png", 0);

        src.copyTo(image);
        src.copyTo(img);
        src_prev.copyTo(img_prev);

        rectangle(image, selection, Scalar(255));
        //imshow("ROI", roi);
        //imshow("ROI_PREV", roi_prev);
        vector<uchar> status;
        vector<float> err;
        calcOpticalFlowPyrLK(img_prev, img, grid, lk_pts, status, err, winSize, 3, termcrit, 0);
        float moda = 0;
        int frecuencia = 0;

        for (int k = 0; k < grid.size(); k++)
        {
            circle(image, grid[k], 1, Scalar(155), -1, 8);
            line(image, grid[k], lk_pts[k], Scalar(255));
            circle(image, lk_pts[k], 2, Scalar(255), -1, 8);
            if (status[k] == 1) //Buscamos los valores 1 dentro del vector con la igualdad
            {
                substract[k] = grid.at(k).x - lk_pts.at(k).x;
                // moda
                for (int h = k - 1; h >= 0; h--)
                {
                    if (substract[k] == substract[h])
                    {
                        sig[h]++;
                        if (sig[h] > moda)
                        {
                            moda = k;
                            frecuencia = sig[h];
                        }
                    }
                }
            }
        }
        // Compara histogramas
        if (status[j] == 1)
        {
            float score = histComp(roi, roi_prev);
            y[j] = score;
        }
        points.at(j).x = 200 + substract[moda];
        points.at(j).y = j * 2;
        cout << "[" << j << "]   Moda: " << substract[moda] << "     Score: " << y[j] << endl;

        imshow("Sequence", image);
        mapStart = points[j];
        circle(m1, mapStart, 4, Scalar(155), -1, 10);
        //line(m1, points[j], points[j + 1], Scalar(255));
        imshow("Map", m1);

        if (waitKey(10) > 10)
            break;
    }
    waitKey(0);
    return 0;
}

string ZeroPadNumber(int num) {
    stringstream ss;

    ss << num;
    string ret;
    ss >> ret;

    // Append zero chars
    int str_length = ret.length();
    for (int i = 0; i < 6 - str_length; i++)
        ret = "0" + ret;
    return ret;
}

static void onMouse(int event, int x, int y, int, void*) {
    if (selectObject) {
        selection.x = MIN(x, origin.x);
        selection.y = MIN(y, origin.y);
        selection.width = std::abs(x - origin.x);
        selection.height = std::abs(y - origin.y);

        selection &= Rect(0, 0, image.cols, image.rows);
    }

    switch (event) {
    case CV_EVENT_LBUTTONDOWN:
        origin = Point(x, y);
        selection = Rect(x, y, 0, 0);
        selectObject = true;
        break;
    case CV_EVENT_LBUTTONUP:
        selectObject = false;
        if (selection.width > 0 && selection.height > 0)
            trackObject = -1;
        break;
    }
}

float evalXDisp(const vector<Point2f> evalPoints[2]) {
    float accum = 0;
    float ax = 0;
    float x = 0;

    for (unsigned i = 0; i != evalPoints[0].size(); ++i) {
        ax = evalPoints[1][i].x;
        x = evalPoints[0][i].x;
        accum += ax - x;
    }
    accum = accum / evalPoints[1].size();
    return accum;
}

float histComp(Mat i1, Mat i2) {
    MatND hist_1;
    MatND hist_2;
    Mat mask = Mat(i1.rows, i1.cols, CV_8UC1, double(0));
    rectangle(mask, selection, Scalar(255), CV_FILLED, 8);

    int bins = 16;
    int histSize[] = { bins };
    float s_ranges[] = { 0, 256 };
    const float* ranges[] = { s_ranges };

    calcHist(&i1, 1, 0, Mat(), hist_1, 1, histSize, ranges, true, false);
    normalize(hist_1, hist_1, 0.0001, 1, NORM_MINMAX, -1, Mat());
    calcHist(&i2, 1, 0, Mat(), hist_2, 1, histSize, ranges, true, false);
    normalize(hist_2, hist_2, 0.0001, 1, NORM_MINMAX, -1, Mat());

    int numrows = bins;

    //make signature
    Mat sig1(numrows, 3, CV_32FC1);
    Mat sig2(numrows, 3, CV_32FC1);

    //fill value into signature
    for (int h = 0; h< bins; h++)
    {

        float binval = hist_1.at< float>(h, 0);
        sig1.at< float>(h, 0) = binval;
        sig1.at< float>(h, 1) = h;
        sig1.at< float>(h, 2) = 0;

        binval = hist_2.at< float>(h, 0);
        sig2.at< float>(h, 0) = binval;
        sig2.at< float>(h, 1) = h;
        sig2.at< float>(h, 2) = 0;

    }

    float emd = cv::EMD(sig1, sig2, CV_DIST_L2);

    return emd;


}