// Listing 19.3. Kalibracja stereo, rektyfikacja i korespondencja
#pragma warning(disable : 4996)
#include <opencv2/opencv.hpp>
#include <iostream>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>

using namespace std;

void help(char *argv[]) {
  cout
      << "\n\nListing 19.3. Kalibracja stereo, rektyfikacja i korespondencja"
      << "\n    Wczytuje listę lokalizacji sekwencji obiektów kalibracji "
         "szachownic"
      << "\n    z pary kamer stereo. Kalibruje, "
         "prostuje i"
      << "\n    wykonuje korespondencję stereo."
      << "\n"
      << "\n    Ten program działa z domyślnymi parametrami przy założeniu, "
         "że utworzono katalog kompilacji"
      << "\n    bezpośrednio pod katalogiem Learning-OpenCV-3 i tam uruchamiasz "
         "programy.   UWAGA: lista par stereo musi "
      << "\n    określać pełną ścieżkę do lewych i prawych obrazów "
         "naprzemiennych "
      << "\n    linijkach: lewy obraz, prawy obraz, po jednej ścieżce/nazwie pliku na linię, zobacz"
      << "\n    plik stereoData/example_19-03_list.txt, możesz wyłączać wiersze za pomocą "
         "komentarzy"
      << "\n    zaczynających się od znaku #."
      << "\n"
      << "\nDomyślne wywołanie (z parametrami: board_w = 9, board_h = 6, list = "
         "../stereoData_19-03_list.txt):"
      << "\n" << argv[0] << "\n"
      << "\nRęczne wywołanie:"
      << "\n" << argv[0] << " [<board_w> <board_h> <path/list_of_stereo_pairs>]"
      << "\n\n Naciśnij dowolny klawisz, aby przeglądać wyniki na każdym etapie."
      << "\n" << endl;
}

static void StereoCalib(const char *imageList, int nx, int ny,
                        bool useUncalibrated) {
  bool displayCorners = true;
  bool showUndistorted = true;
  bool isVerticalStereo = false; // kamery poziome czy pionowe
  const int maxScale = 1;
  const float squareSize = 1.f;

  // rzeczywisty rozmiar kwadratu
  FILE *f = fopen(imageList, "rt");
  int i, j, lr;
  int N = nx * ny;
  cv::Size board_sz = cv::Size(nx, ny);
  vector<string> imageNames[2];
  vector<cv::Point3f> boardModel;
  vector<vector<cv::Point3f> > objectPoints;
  vector<vector<cv::Point2f> > points[2];
  vector<cv::Point2f> corners[2];
  bool found[2] = {false, false};
  cv::Size imageSize;

  // WCZYTANIE LISTY SIATEK KÓŁ
  //
  if (!f) {
    cout << "Nie udało się otworzyć pliku " << imageList << endl;
    return;
  }
  for (i = 0; i < ny; i++)
    for (j = 0; j < nx; j++)
      boardModel.push_back(
          cv::Point3f((float)(i * squareSize), (float)(j * squareSize), 0.f));
  i = 0;
  for (;;) {
    char buf[1024];
    lr = i % 2;
    if (lr == 0)
      found[0] = found[1] = false;
    if (!fgets(buf, sizeof(buf) - 3, f))
      break;
    size_t len = strlen(buf);
    while (len > 0 && isspace(buf[len - 1]))
      buf[--len] = '\0';
    if (buf[0] == '#')
      continue;
    cv::Mat img = cv::imread(buf, 0);
    if (img.empty())
      break;
    imageSize = img.size();
    imageNames[lr].push_back(buf);
    i++;

	// jeśli nie udało się znaleźć planszy na lewym obrazie, nie ma sensu szukać jej na prawym
    //
    if (lr == 1 && !found[0])
      continue;

	// znajdowanie siatek kół i ich środków
    for (int s = 1; s <= maxScale; s++) {
      cv::Mat timg = img;
      if (s > 1)
        resize(img, timg, cv::Size(), s, s, cv::INTER_CUBIC);
      // W ramach przykładu, tak wyglądałoby wywołanie w przypadku okrągłych plansz kalibracji
      //      found[lr] = cv::findCirclesGrid(timg, cv::Size(nx, ny),
      //      corners[lr],
      //                                      cv::CALIB_CB_ASYMMETRIC_GRID |
      //                                          cv::CALIB_CB_CLUSTERING);
      //...ale na naszych obrazach są szachownice
      found[lr] = cv::findChessboardCorners(timg, board_sz, corners[lr]);

      if (found[lr] || s == maxScale) {
        cv::Mat mcorners(corners[lr]);
        mcorners *= (1. / s);
      }
      if (found[lr])
        break;
    }
    if (displayCorners) {
      cout << buf << endl;
      cv::Mat cimg;
      cv::cvtColor(img, cimg, cv::COLOR_GRAY2BGR);

	  // rysowanie rogów szachownic działa także w odniesieniu do siatek kół
      cv::drawChessboardCorners(cimg, cv::Size(nx, ny), corners[lr], found[lr]);
      cv::imshow("Corners", cimg);
      if ((cv::waitKey(0) & 255) == 27) // Allow ESC to quit
        exit(-1);
    } else
      cout << '.';
    if (lr == 1 && found[0] && found[1]) {
      objectPoints.push_back(boardModel);
      points[0].push_back(corners[0]);
      points[1].push_back(corners[1]);
    }
  }
  fclose(f);

  // Kalibracja kamer stereo
  cv::Mat M1 = cv::Mat::eye(3, 3, CV_64F);
  cv::Mat M2 = cv::Mat::eye(3, 3, CV_64F);
  cv::Mat D1, D2, R, T, E, F;
  cout << "\nKalibracja stereo...\n";
  cv::stereoCalibrate(
      objectPoints, points[0], points[1], M1, D1, M2, D2, imageSize, R, T, E, F,
      cv::CALIB_FIX_ASPECT_RATIO | cv::CALIB_ZERO_TANGENT_DIST |
          cv::CALIB_SAME_FOCAL_LENGTH,
      cv::TermCriteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 100,
                       1e-5));
  cout << "Gotowe! Naciśnij dowolny klawisz, aby przeglądać obrazy lub ESC, aby zakończyć.\n\n";

  // TEST JAKOŚCI KALIBRACJI
  // ponieważ wyjściowa macierz fundamentalna zawiera wszystkie informacje wyjściowe,
  // jakość kalibracji możemy sprawdzić przy użyciu kryterium geometrii epipolarnej: m2^t*F*m1=0
  vector<cv::Point3f> lines[2];
  double avgErr = 0;
  int nframes = (int)objectPoints.size();
  for (i = 0; i < nframes; i++) {
    vector<cv::Point2f> &pt0 = points[0][i];
    vector<cv::Point2f> &pt1 = points[1][i];
    cv::undistortPoints(pt0, pt0, M1, D1, cv::Mat(), M1);
    cv::undistortPoints(pt1, pt1, M2, D2, cv::Mat(), M2);
    cv::computeCorrespondEpilines(pt0, 1, F, lines[0]);
    cv::computeCorrespondEpilines(pt1, 2, F, lines[1]);

    for (j = 0; j < N; j++) {
      double err = fabs(pt0[j].x * lines[1][j].x + pt0[j].y * lines[1][j].y +
                        lines[1][j].z) +
                   fabs(pt1[j].x * lines[0][j].x + pt1[j].y * lines[0][j].y +
                        lines[0][j].z);
      avgErr += err;
    }
  }
  cout << "średni błąd = " << avgErr / (nframes * N) << endl;

  // OBLICZANIE I WIZUALIZACJA REKTYFIKACJI
  //
  if (showUndistorted) {
    cv::Mat R1, R2, P1, P2, map11, map12, map21, map22;

	// JEŚLI SKALIBROWANE (METODĄ BOUGUETA)
    //
    if (!useUncalibrated) {
      stereoRectify(M1, D1, M2, D2, imageSize, R, T, R1, R2, P1, P2,
                    cv::noArray(), 0);
      isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));
	  // obliczenie matryc dla cvRemap()
      initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11,
                              map12);
      initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21,
                              map22);
    }

	// W PRZECIWNYM PRZYPADKU STOSUJEMY METODĘ HARTLEYA
    //
    else {

		// używamy wewnętrznych parametrów każdej kamery, 
		// ale obliczenia rektyfikacji wykonujemy bezpośrednio na podstawie macierzy fundamentalnej
      vector<cv::Point2f> allpoints[2];
      for (i = 0; i < nframes; i++) {
        copy(points[0][i].begin(), points[0][i].end(),
             back_inserter(allpoints[0]));
        copy(points[1][i].begin(), points[1][i].end(),
             back_inserter(allpoints[1]));
      }
      cv::Mat F = findFundamentalMat(allpoints[0], allpoints[1], cv::FM_8POINT);
      cv::Mat H1, H2;
      cv::stereoRectifyUncalibrated(allpoints[0], allpoints[1], F, imageSize,
                                    H1, H2, 3);
      R1 = M1.inv() * H1 * M1;
      R2 = M2.inv() * H2 * M2;

	  // obliczenie matrycy dla cvRemap()
      //
      cv::initUndistortRectifyMap(M1, D1, R1, P1, imageSize, CV_16SC2, map11,
                                  map12);
      cv::initUndistortRectifyMap(M2, D2, R2, P2, imageSize, CV_16SC2, map21,
                                  map22);
    }

	// REKTYFIKACJA OBRAZÓW I SZUKANIE MATRYC ROZBIEŻNOŚCI
    //
    cv::Mat pair;
    if (!isVerticalStereo)
      pair.create(imageSize.height, imageSize.width * 2, CV_8UC3);
    else
      pair.create(imageSize.height * 2, imageSize.width, CV_8UC3);

	// konfiguracja do szukania korespondencji stereo
    //
    cv::Ptr<cv::StereoSGBM> stereo = cv::StereoSGBM::create(
        -64, 128, 11, 100, 1000, 32, 0, 15, 1000, 16, cv::StereoSGBM::MODE_HH);

    for (i = 0; i < nframes; i++) {
      cv::Mat img1 = cv::imread(imageNames[0][i].c_str(), 0);
      cv::Mat img2 = cv::imread(imageNames[1][i].c_str(), 0);
      cv::Mat img1r, img2r, disp, vdisp;
      if (img1.empty() || img2.empty())
        continue;
      cv::remap(img1, img1r, map11, map12, cv::INTER_LINEAR);
      cv::remap(img2, img2r, map21, map22, cv::INTER_LINEAR);
      if (!isVerticalStereo || !useUncalibrated) {

		  // gdy kamera stereo jest zorientowana pionowo, metoda Hartleya nie przestawia obrazu,
		  // przez co linie epipolarne w rektyfikowanych obrazach są pionowe;
		  // funkcja obliczania korespondencji stereo w takim przypadku nie działa.
        stereo->compute(img1r, img2r, disp);
        cv::normalize(disp, vdisp, 0, 256, cv::NORM_MINMAX, CV_8U);
        cv::imshow("disparity", vdisp);
      }
      if (!isVerticalStereo) {
        cv::Mat part = pair.colRange(0, imageSize.width);
        cvtColor(img1r, part, cv::COLOR_GRAY2BGR);
        part = pair.colRange(imageSize.width, imageSize.width * 2);
        cvtColor(img2r, part, cv::COLOR_GRAY2BGR);
        for (j = 0; j < imageSize.height; j += 16)
          cv::line(pair, cv::Point(0, j), cv::Point(imageSize.width * 2, j),
                   cv::Scalar(0, 255, 0));
      } else {
        cv::Mat part = pair.rowRange(0, imageSize.height);
        cv::cvtColor(img1r, part, cv::COLOR_GRAY2BGR);
        part = pair.rowRange(imageSize.height, imageSize.height * 2);
        cv::cvtColor(img2r, part, cv::COLOR_GRAY2BGR);
        for (j = 0; j < imageSize.width; j += 16)
          line(pair, cv::Point(j, 0), cv::Point(j, imageSize.height * 2),
               cv::Scalar(0, 255, 0));
      }
      cv::imshow("rectified", pair);
      if ((cv::waitKey() & 255) == 27)
        break;
    }
  }
}

//
// Domyślne wywołanie (z parametrami : board_w = 9, board_h = 6, list =
//  ../stereoData_19-03_list.txt):
//./example_19-03
//
//Ręczne wywołanie:
//./example_19-03 [<board_w> <board_h> <path/list_of_stereo_pairs>]
//
// Naciśnij dowolny klawisz, aby przeglądać wyniki albo ESC, aby zakończyć.
//


int main(int argc, char **argv) {
  help(argv);
  int board_w = 9, board_h = 6;
  const char *board_list = "../stereoData/example_19-03_list.txt";
  if (argc == 4) {
    board_list = argv[1];
    board_w = atoi(argv[2]);
    board_h = atoi(argv[3]);
  }
  StereoCalib(board_list, board_w, board_h, true);
  return 0;
}
