// Listing 19.2. Obliczanie macierzy fundamentalnej metodą RANSAC
#include <opencv2/opencv.hpp>
#include <iostream>

using namespace std;
void help(char *argv[]) { 
	cout 	<< "\nListing 19.2. Obliczanie macierzy fundamentalnej metodą RANSAC. Pokazanie kamerze szachownicy "
			<< "\nWywołanie"
			<< "\n./example_19-2 <1:board_w> <2:board_h> <3: liczba plansz> <4:liczba ms między klatkami> <5:skala obrazów 0-1>"
			<< "\n\nPrzykładowe wywołanie:"
			<< "\n./example_19-2 9 6 20 500 0.5"
			<< "\n\n -- użyj dostarczonego pliku checkerboard9x6.png"
			<< "\n" 
			<< endl;
}


// argumnety: [board_w] [board_h] [number_of_boards] [delay]? [scale]?
//
int main(int argc, char *argv[]) {
  int n_boards = 0;
  float image_sf = 0.5f;
  float delay = 1.f;
  int board_w = 0;
  int board_h = 0;

  // ustawiane przez listę wejściową
  if (argc != 6) {
    cout << "\nBŁĄD: nieprawidłowa liczba parametrów wejściowych, powinno być, otrzymano " << argc - 1 << "\n";
    help(argv);
    return -1;
  }
  board_w = atoi(argv[1]);
  board_h = atoi(argv[2]);
  n_boards = atoi(argv[3]);
  delay = atof(argv[4]);
  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ć kamery\n";
    help(argv);
    return -1;
  }
  // alokacja pamięci
  //
  vector<vector<cv::Point2f> > image_points;
  vector<vector<cv::Point3f> > object_points;
  // chwytanie widoków; pętla powtarza się, aż znajdzie n_boards poprawnych widoków
  // (czyli takich, w których plansza zawiera wszystkie rogi)
  //
  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();
    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
    //
    cv::drawChessboardCorners(image, board_sz, corners, found);
	// jeśli plansza jest dobra, dodajemy ją do swoich 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 kopiuje danych
      mcorners *= (1. / image_sf);
	  // skaluje współrzędne rogów
      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((float)(j / board_w), (float)(j % board_w), 0.f);
      }
	  cout << "Zgromadziliśmy " << (int)image_points.size()
		  << " z " << n_boards << " potrzebnych obrazów szachownicy.\n" << endl;
    }
	// kolorowanie, jeśli pobraliśmy obraz
    //
    cv::imshow("Calibration", image);
    if ((cv::waitKey(30) & 255) == 27)
      return -1;
  }
  // koniec pętli while() gromadzenia obrazów
  cv::destroyWindow("Calibration");
  cout << "\n\n*** Kalibracja kamery...\n" << endl;
  // Kalibracja kamery!
  //
  cv::Mat intrinsic_matrix, distortion_coeffs;
  double err = cv::calibrateCamera(
	  object_points,     // wektor wektorów punktów z wzoru kalibracji
	  image_points,      // wektor wektorów rzutowanych lokalizacji (na obrazach)
	  image_size,        // rozmiar użytych obrazów
	  intrinsic_matrix,  // wyjściowa macierz kamery
	  distortion_coeffs, // wyjściowe współczynniki zniekształceń
	  cv::noArray(),     // przekażemy wektory rotacji…
	  cv::noArray(),     // …i translacji
	  cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_PRINCIPAL_POINT
  );


  // zapisanie parametrów wewnętrznych i zniekształceń
  cout << " *** GOTOWE!\n\nBłędy reprojekcji zapisano w plikach " << err
	  << "\nStoring 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 wczytania 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;

  // obliczenie macierzy fundamentalnej między pierwszą i drugą klatką:
  //
  cv::undistortPoints(
	  image_points[0],   // współrzędne obserwowanego punktu (z klatki 0)
	  image_points[0],   // współrzędne po likwidacji zniekształceń (w tym przypadku ta sama tablica co powyżej)
	  intrinsic_matrix,  // parametry wewnętrzne z cv::calibrateCamera()
	  distortion_coeffs, // współczynniki zniekształceń, także z cv::calibrateCamera()
	  cv::Mat(),         // rektyfikacja (ale w tym przypadku jej nie potrzebujemy)
	  intrinsic_matrix   // nowa macierz kamery
  );
  cv::undistortPoints(
	  image_points[1],   // współrzędne obserwowanego punktu (z klatki 1)
	  image_points[1],   // współrzędne po likwidacji zniekształceń (w tym przypadku ta sama tablica co powyżej)
	  intrinsic_matrix,  // parametry wewnętrzne z cv::calibrateCamera()
	  distortion_coeffs, // współczynniki zniekształceń, także z cv::calibrateCamera()
	  cv::Mat(),         // rektyfikacja (ale w tym przypadku jej nie potrzebujemy)
	  intrinsic_matrix   // nowa macierz kamery
  );

  // ponieważ wszystkie znalezione rogi szachownicy to punkty bliskie, tzn.
  // muszą spełniać warunki epipolarne, posługujemy się najszybszym
  // i najdokładniejszym (w tym przypadku) algorytmem 8-punktowym
  //
  cv::Mat F = cv::findFundamentalMat( // zwraca obliczoną macierz
	  image_points[0],                  // punkty z klatki 0
	  image_points[1],                  // punkty z klatki 1
	  cv::FM_8POINT                     // włączenie algorytmu 8-punktowego
  );
  cout << "Macierz fundamentalna: " << F << endl;

  // budowa matrycy likwidacji zniekształceń, która zostanie zastosowana do wszystkich następnych klatek
  //
  cv::Mat map1, map2;
  cv::initUndistortRectifyMap(
	  intrinsic_matrix_loaded,  // nasza macierz kamery
	  distortion_coeffs_loaded, // nasze współczynniki zniekształceń
	  cv::Mat(),                // (opcjonalne) rektyfikacja niepotrzebna
	  intrinsic_matrix_loaded,  // „nowa” macierz, tu jest taka sama jak w pierwszym argumencie
	  image_size,               // rozmiar obrazu po likwidacji zniekształceń
	  CV_16SC2,                 // format matrycy
	  map1,                     // współrzędne zamienione na całkowitoliczbowe
	  map2                      // stałoprzecinkowe przesunięcia
								// elementów matrycy of map1
  );
  // wyświetlenie obrazu nieprzetworzonego i obrazu poprawionego
  //
  for (;;) {
	  cv::Mat image, image0;
	  capture >> image0;
	  if (image0.empty()) break;
	  cv::remap(
		  image0, // obraz wejściowy
		  image,  // obraz wyjściowy
		  map1,   // całkowitoliczbowa część matrycy
		  map2,   // stałoprzecinkowa część matrycy
		  cv::INTER_LINEAR,
		  cv::BORDER_CONSTANT,
		  cv::Scalar() // koloruje wartości graniczne na czarno
	  );
	  cv::imshow("Undistorted", image);
	  if ((cv::waitKey(30) & 255) == 27) break;
  }
  return 1;
}
