// Listing 18.1. Odczytanie szerokości i wysokości szachownicy, wczytanie i zgromadzenie żądanej liczby widoków oraz skalibrowanie aparatu

// dołączone bibliotek
// #include <windows.h>  // dla systemów Windows
#include <dirent.h>     // Linux
#include <sys/stat.h>   // Linux
#include <iostream>     // cout
#include <algorithm>    // std::sort
#include <opencv2/opencv.hpp>

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

// zwraca listę plików w katalogu (z wyjątkiem tych, których nazwy zaczynają się od kropki)
int readFilenames(vector<string>& filenames, const string& directory) {
#ifdef WINDOWS
    HANDLE dir;
    WIN32_FIND_DATA file_data;

    if ((dir = FindFirstFile((directory + "/*").c_str(), &file_data)) == INVALID_HANDLE_VALUE)
        return;  // nie znaleziono żadnych plików
    do {
        const string file_name = file_data.cFileName;
        const string full_file_name = directory + "/" + file_name;
        const bool is_directory = (file_data.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY) != 0;

        if (file_name[0] == '.')
            continue;

        if (is_directory)
            continue;

        filenames.push_back(full_file_name);
    } while (FindNextFile(dir, &file_data));

    FindClose(dir);
#else
    DIR *dir;
    class dirent *ent;
    class stat st;

    dir = opendir(directory.c_str());
    while ((ent = readdir(dir)) != NULL) {
        const string file_name = ent->d_name;
        const string full_file_name = directory + "/" + file_name;

        if (file_name[0] == '.')
            continue;

        if (stat(full_file_name.c_str(), &st) == -1)
            continue;

        const bool is_directory = (st.st_mode & S_IFDIR) != 0;

        if (is_directory)
            continue;

        // filenames.push_back(full_file_name);  // zwraca pełną ścieżkę
        filenames.push_back(file_name);  // zwraca tylko nazwę pliku
    }
    closedir(dir);
#endif
    std::sort(filenames.begin(), filenames.end());  // opcjonalne sortowanie nazw
    return(filenames.size());  // zwraca liczbę znalezionych plików
}  // GetFilesInDirectory

void help(const char **argv) {
    cout << "\n\n"
         << "Listing 18.1 (z dysku):\Podaj szerokość i wysokość szachownicy,\n"
         << "              wczytuje katalog obrazów szachownic,\n"
         << "              i kalibruje kamerę\n\n"
         << "Call:\n" << argv[0] << " <1board_width> <2board_height> <3number_of_boards>"
         << " <4ms_delay_framee_capture> <5image_scaling_factor> <6path_to_calibration_images>\n\n"
         << "\nPrzykład:\n"
         << "./example_18-01_from_disk 9 6 14 100 1.0 ../stereoData/\nor:\n"
         << "./example_18-01_from_disk 12 12 28 100 0.5 ../calibration/\n\n"
         << " * Najpierw wczytuje szachownice i przeprowadza kalibrację\n"
         << " * Następnie zapisuje i ponownie wczytuje skalibrowane matryce\n"
         << " * Później tworzy mapę redukcji zniekształceń\n"
         << " * Na koniec wyświetla poprawiony obraz\n"
         << endl;
}


int main(int argc, const char *argv[]) {
    float image_sf = 0.5f;    // współczynnik skalowania obraz
    int delay = 250;          // milisekundy
    int board_w = 0;
    int board_h = 0;

    if (argc != 7) {
        cout << "\nBŁĄD: nieprawidłowa liczba (" << argc - 1
             << ") argumentów, powinno być sześć parametrów wejściowych\n";
        help(argv);
        return -1;
    }

    board_w = atoi(argv[1]);
    board_h = atoi(argv[2]);
    int n_boards = atoi(argv[3]);  // maks. liczba plansz do wczytania
    delay = atof(argv[4]);         // opóźnienie w milisekundach
    image_sf = atof(argv[5]);
    int board_n = board_w * board_h;  // liczba rogów
    cv::Size board_sz = cv::Size(board_w, board_h);  // szerokość i wysokość planszy

    string folder = argv[6];
    cout << "Wczytywanie katalogu " << folder << endl;
    vector<string> filenames;
    int num_files = readFilenames(filenames, folder);
    cout << "   ... Gotowe. Liczba plików = " << num_files << "\n" << endl;

    // do przechowywania punktów
    //
    vector<vector<cv::Point2f> > image_points;
    vector<vector<cv::Point3f> > object_points;

    /////////// KOLEKCJA //////////////////////////////////////////////
    // Chwyta widoki rogów: przegląda za pomocą pętli wszystkie obrazy kalibracji
    // zbierając wszystkie znalezione rogi na planszach
    //
    cv::Size image_size;
    int board_count = 0;
    for (size_t i = 0; (i < filenames.size()) && (board_count < n_boards); ++i) {
        cv::Mat image, image0 = cv::imread(folder + filenames[i]);
        board_count += 1;
        if (!image0.data) {  // ochrona przed niewłaściwym formatem
            cerr << folder + filenames[i] << ", plik nr " << i << " nie jest obrazem" << endl;
            continue;
        }
        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 jej
        //
        drawChessboardCorners(image, board_sz, corners, found);  // will draw only if found

        // jeśli mamy dobrą planszę, dodajemy ją do danych
        //
        if (found) {
            image ^= cv::Scalar::all(255);
            cv::Mat mcorners(corners);

            // nie kopiuj danych
            mcorners *= (1.0 / image_sf);

            // skaluje współrzędne rogu
            image_points.push_back(corners);
            object_points.push_back(vector<cv::Point3f>());
            vector<cv::Point3f> &opts = object_points.back();

            opts.resize(board_n);
            for (int j = 0; j < board_n; j++) {
                opts[j] = cv::Point3f(static_cast<float>(j / board_w),
                                      static_cast<float>(j % board_w), 0.0f);
            }
            cout << "Zebrano " << static_cast<int>(image_points.size())
                 << "plansz. Ta z obrazu szachownicy nr "
                 << i << ", " << folder + filenames[i] << endl;
        }
        cv::imshow("Calibration", image);

        // pokazanie za pomocą koloru, czy bierzemy obraz
        if ((cv::waitKey(delay) & 255) == 27) {
            return -1;
        }
    }

    // koniec pętli while
    cv::destroyWindow("Calibration");
    cout << "\n\n*** KALIBRACJA KAMERY...\n" << endl;

    /////////// KALIBRACJA  //////////////////////////////////////////////
    // KALIBRACJA KAMERY!
    //
    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 danych wewnętrznych i zniekształceń
    cout << " *** GOTOWE!\n\nBłąd reprojekcji to " << err
       << "\nZapisywanie plików Intrinsics.xml i Distortions.xml\n\n";
    cv::FileStorage fs("intrinsics.xml", cv::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 WCZYTYWANIA TYCH MACIERZY:
    fs.open("intrinsics.xml", cv::FileStorage::READ);
    cout << "\nszerokość obrazu: " << static_cast<int>(fs["image_width"]);
    cout << "\nwysokość obrazu: " << static_cast<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 parametrów wewnętrznych:" << intrinsic_matrix_loaded;
    cout << "\nwspółczynniki zniekształceń: " << distortion_coeffs_loaded << "\n" << endl;

    // budowa mapy redukcji zniekształceń, która będzie używana
    // we wszystkich następnych klatkach
    //
    cv::Mat map1, map2;
    cv::initUndistortRectifyMap(intrinsic_matrix_loaded, distortion_coeffs_loaded,
                              cv::Mat(), intrinsic_matrix_loaded, image_size,
                              CV_16SC2, map1, map2);

    ////////// WYŚWIETLANIE //////////////////////////////////////////////
    cout << "*****************\nNaciśnij klawisz, aby wyświetlić następny obraz lub ESC, aby zakończyć\n"
         << "****************\n" << endl;
    board_count = 0;  // reset maks. liczby plansz do wczytania
    for (size_t i = 0; (i < filenames.size()) && (board_count < n_boards); ++i) {
        cv::Mat image, image0 = cv::imread(folder + filenames[i]);
        ++board_count;
        if (!image0.data) {  // ochrona przed niewłaściwym formatem
            cerr << folder + filenames[i] << ", plik nr " << i << " nie jest obrazem" << endl;
            continue;
        }
        // przybliż kamerę do ekranu, przedstawiającego teraz surowy i
        // poprawionego obrazu
        //
        cv::remap(image0, image, map1, map2, cv::INTER_LINEAR,
            cv::BORDER_CONSTANT, cv::Scalar());
        cv::imshow("Original", image0);
        cv::imshow("Undistorted", image);
        if ((cv::waitKey(0) & 255) == 27) {
            break;
        }
    }
    return 0;
}
