// Listing 16.1. Przykład zastosowania piramidowego algorytmu przepływu optycznego L-K
//
#include <iostream>
#include <vector>
#include <opencv2/opencv.hpp>

static const int MAX_CORNERS = 1000;
using std::cout;
using std::endl;
using std::vector;


void help( char** argv ) {
  cout << "\nListing 16.1: Przykład zastosowania piramidowego algorytmu przepływu optycznego L-K.\n" << endl;
  cout << "Wywołanie: " <<argv[0] <<" [image1] [image2]\n" << endl;
  cout << "\nPrzykład:\n" << argv[0] << " ../example_16-01-imgA.png ../example_16-01-imgB.png\n" << endl;
  cout << "Demonstruje przykład zastosowania piramidowego algorytmu przepływu optycznego L-K.\n" << endl;
}

int main(int argc, char** argv) {
    if (argc != 3) {
        help(argv);
        exit(-1);
    }
	// inicjalizacja, załadowanie dwóch obrazów z systemu plików oraz
	// alokacja obrazów i innych struktur, które będą potrzebne do zwrócenia wyników
    //
    cv::Mat imgA = cv::imread(argv[1], CV_LOAD_IMAGE_GRAYSCALE);
    cv::Mat imgB = cv::imread(argv[2], CV_LOAD_IMAGE_GRAYSCALE);
    cv::Size img_sz = imgA.size();
    int win_size = 10;
    cv::Mat imgC = cv::imread(argv[2], CV_LOAD_IMAGE_UNCHANGED);
    // The first thing we need to do is get the features
    // we want to track.
    //
    vector< cv::Point2f > cornersA, cornersB;
    const int MAX_CORNERS = 500;
    cv::goodFeaturesToTrack(
		imgA,           // obraz do śledzenia
		cornersA,       // wektor wykrytych rogów (wyjściowy)
		MAX_CORNERS,    // chcemy tyle rogów
		0.01,           // poziom jakości (procent wartości maksymalnej)
		5,              // minimalna odległość między rogami
		cv::noArray(),  // maska
		3,              // rozmiar bloku
		false,          // true: Harris, false: Shi-Tomasi
		0.04            // parametr specyficzny dla metody
	);
	cv::cornerSubPix(
		imgA,           // obraz wejściowy
		cornersA,       // wektor rogów (wejście i wyjście)
		cv::Size(win_size, win_size), // połowa boku okienka poszukiwań
		cv::Size(-1, -1),              // połowa długości boku martwej strefy (-1=brak)
		cv::TermCriteria(
			cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS,
			20,   // maksymalna liczba iteracji
			0.03  // minimalna zmiana na iterację
		)
	);

	// wywołanie algorytmu Lucasa-Kanade'a
	//

    vector<uchar> features_found;
    cv::calcOpticalFlowPyrLK(
		imgA, // poprzedni obraz
		imgB,           // następny obraz
		cornersA,       // poprzedni zbiór rogów (z imgA)
		cornersB,       // następny zbiór rogów (z imgB)
		features_found, // wektor wyjściowy, śledzone elementy są oznaczane 1
		noArray(),      // wektor wyjściowy, listy błędów (opcjonalny)
		cv::Size(win_size * 2 + 1, win_size * 2 + 1), // rozmiar okienka poszukiwań
		5,              // maksymalna liczba poziomów piramidy
		cv::TermCriteria(
			cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS,
			20,           // maksymalna liczba iteracji
			0.3           // minimalna zmiana na iterację
		)
	);

	// tworzenie obrazu z tego, co przeglądamy:
	// jeśli chcesz dalej śledzić cornersB, tzn. przekazać te rogi na wejściu do kolejnego wywołania calcOpticalFlowPyrLK,
	// musisz „skompresować” wektor, czyli usunąć punkty, dla których
	// features_found[i] == false
    for (int i = 0; i < static_cast<int>(cornersA.size()); ++i) {
        if (!features_found[i]) {
            continue;
        }
        line(
            imgC,                        // rysuje na tym obrazie
            cornersA[i],                 // początek
            cornersB[i],                 // koniec
            cv::Scalar(0, 255, 0),       // ten kolor
            1,                           // tyle pikseli szerokości
            cv::LINE_AA                  // rysuje linię w tym stylu
        );
    }

    cv::imshow("ImageA", imgA);
    cv::imshow("ImageB", imgB);
    cv::imshow("Przykład zastosowania piramidowego algorytmu przepływu optycznego L-K", imgC);
    cv::waitKey(0);

    return 0;
}
