// Listing 18.1. Odczytanie szerokości i wysokości szachownicy, wczytanie i zgromadzenie żądanej liczby widoków oraz skalibrowanie aparatu
#include <iostream>
#include <opencv2/opencv.hpp>

using std::vector;
using std::cout;
using std::cerr;
using std::endl;

void help(char **argv) {  // todo rewrite this
    cout << "\n\n"
         << "Listing 18.1:\nOdczytanie szerokości i wysokości szachownicy,\n"
         << "               wczytanie i zgromadzenie żądanej liczby widoków\n" 
         << "              oraz skalibrowanie aparatu\n\n" 
         << "Wywołanie:\n" << argv[0] << " <board_width> <board_height> <number_of_boards> <if_video,_delay_between_framee_capture> <image_scaling_factor>\n\n"
         << "Przykład:\n" << argv[0] << " 9 6 15 500 0.5\n"
         << "-- dla dostarczonego obrazu checkerboard9x6.png\n\n"
         << " * Najpierw wczytuje szachownice i dokonuje kalibracji\n" 
         << " * Następnie zapisuje i ponownie wczytuje matryce kalibracji\n"
         << " * Następnie tworzy mapę redukcji zniekształceń i na koniec \n"
         << " * wyświetla poprawiony obraz.\n"
         << endl;
}

int main(int argc, char *argv[]) {
    int n_boards = 0;           // parametr ustawiany przez listę wejściową
    float image_sf = 0.5f;      // współczynnik skali obrazu
    float delay = 1.f;
    int board_w = 0;
    int board_h = 0;

    if (argc < 4 || argc > 6) {
        cout << "\nBŁĄD: nieprawidłowa liczba parametrów wejściowych\n";
        help(argv);
        return -1;
    }

    board_w = atoi(argv[1]);
    board_h = atoi(argv[2]);
    n_boards = atoi(argv[3]);

    if (argc > 4) {
        delay = atof(argv[4]);
    }
    if (argc > 5) {
        image_sf = atof(argv[5]);
    }

    int board_n = board_w * board_h;
    cv::Size board_sz = cv::Size(board_w, board_h);
    cv::VideoCapture capture(0);
    if (!capture.isOpened()) {
        cout << "\nNie udało się otworzyć aparatu\n";
        help(argv);
        return -1;
    }

	// ALOKACJA MIEJSCA NA DANE
	//
	vector< vector<cv::Point2f> > image_points;
	vector< vector<cv::Point3f> > object_points;

	// przechwytywanie widoków rogów: powtarzanie pętli aż do n_boards
	// udanych przechwyceń (udane=znaleziono wszystkie rogi na planszy)
	//
	double last_captured_timestamp = 0;
	cv::Size image_size;

	while (image_points.size() < (size_t)n_boards) {

		cv::Mat image0, image;
		capture >> image0;
		image_size = image0.size();
		cv::resize(image0, image, cv::Size(), image_sf, image_sf, cv::INTER_LINEAR);

		// szukanie planszy
		//
		vector<cv::Point2f> corners;
		bool found = cv::findChessboardCorners(image, board_sz, corners);

		// rysowanie planszy
		//
		drawChessboardCorners(image, board_sz, corners, found);

		// jeśli znaleźliśmy planszę, dodajemy ją do naszych danych
		//
		double timestamp = (double)clock() / CLOCKS_PER_SEC;

		if (found && timestamp - last_captured_timestamp > 1) {

			last_captured_timestamp = timestamp;
			image ^= cv::Scalar::all(255);

			cv::Mat mcorners(corners); // nie kopiuj danych
			mcorners *= (1. / image_sf); // skalowanie współrzędnych rogów
			image_points.push_back(corners);
			object_points.push_back(vector<Point3f>());
			vector<cv::Point3f>& opts = object_points.back();
			opts.resize(board_n);
			for (int j = 0; j<board_n; j++) {
				opts[j] = cv::Point3f((float)(j / board_w), (float)(j%board_w), 0.f);
			}
			cout << "Zebrano " << (int)image_points.size() <<
				" z " << n_boards << " potrzebnych obrazów szachownicy.\n" << endl;
		}
		cv::imshow("Calibration", image); // kolorowa prezentacja, czy obraz został wzięty

		if ((cv::waitKey(30) & 255) == 27)
			return -1;
	}
	// KONIEC PĘTLI WHILE 

	cv::destroyWindow("Calibration");
	cout << "\n\n*** KALIBROWANIE APARATU...\n" << endl;

	// KALIBRACJA APARATU!
	//
	cv::Mat intrinsic_matrix, distortion_coeffs;
	double err = cv::calibrateCamera(
		object_points,
		image_points,
		image_size,
		intrinsic_matrix,
		distortion_coeffs,
		cv::noArray(),
		cv::noArray(),
		cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_PRINCIPAL_POINT
	);

	// ZAPISANIE PARAMETRÓW WEWNĘTRZNYCH I ZNIEKSZTAŁCEŃ
	cout << " *** GOTOWE!\n\nBłędy reprojekcji znajdziesz w plikach " << err <<
		"\nStoring Intrinsics.xml i Distortions.xml.\n\n";
	cv::FileStorage fs("intrinsics.xml", FileStorage::WRITE);

	fs << "image_width" << image_size.width << "image_height" << image_size.height
		<< "camera_matrix" << intrinsic_matrix << "distortion_coefficients"
		<< distortion_coeffs;
	fs.release();

	// PRZYKŁAD ŁADOWANIA TYCH MACIERZY Z POWROTEM
	fs.open("intrinsics.xml", cv::FileStorage::READ);
	cout << "\nszerokość obrazu: " << (int)fs["image_width"];
	cout << "\nwysokość obrazu: " << (int)fs["image_height"];

	cv::Mat intrinsic_matrix_loaded, distortion_coeffs_loaded;
	fs["camera_matrix"] >> intrinsic_matrix_loaded;
	fs["distortion_coefficients"] >> distortion_coeffs_loaded;
	cout << "\nmacierz wewnętrzna:" << intrinsic_matrix_loaded;
	cout << "\nwspółczynniki zniekształceń: " << distortion_coeffs_loaded << endl;

	// budowa matrycy likwidacji zniekształceń, której użyjemy we wszystkich kolejnych klatkach
	//
	cv::Mat map1, map2;
	cv::initUndistortRectifyMap(
		intrinsic_matrix_loaded,
		distortion_coeffs_loaded,
		cv::Mat(),
		intrinsic_matrix_loaded,
		image_size,
		CV_16SC2,
		map1,
		map2
	);

	// przekazanie obrazu na ekran, aby wyświetlić nieprzetworzony i poprawiony obraz
	//
	for (;;) {
        cv::Mat image, image0;
        capture >> image0;

        if (image0.empty()) {
            break;
        }
        cv::remap(image0, image, map1, map2, cv::INTER_LINEAR,
            cv::BORDER_CONSTANT, cv::Scalar());
        cv::imshow("Undistorted", image);
        if ((cv::waitKey(30) & 255) == 27) {
            break;
        }
    }

    return 0;
}
