// Listing 19.1. Generowanie widoku z góry
#include <opencv2/opencv.hpp>
#include <iostream>
    using namespace std;
    
void help(char *argv[]) {
	cout	<< "\nListing 19.1 Generowanie widoku z góry za pomocą homografii."
			<< "\nTen program wymaga do działania utworzonego pliku za pomocą programu example_18-01_from_disk,"
			<< "\n   ale w tym przypadku ten plik jest już utworzony: ../birdseye/intrinsics.xml"
			<< "\nWywołanie:"
			<< "\n./example_19-01 <chessboard_width> <chessboard_height> <path/camera_calib_filename> <path/chessboard_image>"
			<< "\n\nPrzykład:"
			<< "\n./example_19-01 12 12 ../birdseye/intrinsics.xml ../birdseye/IMG_0215L.jpg\n"
			<< "\nNaciśnij klawisz d, aby zmniejszyć wysokość lub u aby zwiększyć wysokość (zmienia się pozorna wysokość Z). Klawisz Esc zamyka program.\n" 
			<< endl;
}

// args: [board_w] [board_h] [intrinsics.xml] [checker_image]
//
int main(int argc, char *argv[]) {
  if (argc != 5) {
    cout << "\nBŁĄD: za mało parametrów.\n";
    help(argv);
    return -1;
  }
  // parametry wejściowe:
  //
  int board_w = atoi(argv[1]);
  int board_h = atoi(argv[2]);
  int board_n = board_w * board_h;
  cv::Size board_sz(board_w, board_h);
  cv::FileStorage fs(argv[3], cv::FileStorage::READ);
  cv::Mat intrinsic, distortion;

  fs["camera_matrix"] >> intrinsic;
  fs["distortion_coefficients"] >> distortion;

  if (!fs.isOpened() || intrinsic.empty() || distortion.empty()) {
    cout << "Błąd: nie udało się wczytać parametrów wewnętrznych z " << argv[3]
         << endl;
    return -1;
  }
  fs.release();

  cv::Mat gray_image, image, image0 = cv::imread(argv[4], 1);
  if (image0.empty()) {
    cout << "Błąd: nie udało się załadować obrazu " << argv[4] << endl;
    return -1;
  }

  // LIKWIDACJA ZNIEKSZTAŁCEŃ OBRAZU
  //
  cv::undistort(image0, image, intrinsic, distortion, intrinsic);
  cv::cvtColor(image, gray_image, cv::COLOR_BGRA2GRAY);

  // WPROWADZENIE SZACHOWNICY NA PŁASZCZYZNĘ
  //
  vector<cv::Point2f> corners;
  bool found = cv::findChessboardCorners( // true, jeśli znaleziono
	  image,    // obraz wejściowy
	  board_sz, // rozmiar wzoru
	  corners,  // wyniki
      cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FILTER_QUADS);
  if (!found) {
	  cout << "Nie udało się pobrać szachownicy z " << argv[4]
		  << ", znaleziono tylko " << corners.size() << " z " << board_n
		  << " rogów\n";
    return -1;
  }

  // określenie tych rogów z subpikselową dokładnością
  //
  cv::cornerSubPix(
	  gray_image,      // obraz wejściowy
	  corners,         // wartości początkowe, także wyjściowe
	  cv::Size(11, 11), // rozmiar okienka poszukiwań
	  cv::Size(-1, -1), // strefa zerowa (w tym przypadku nieużywana)
	  cv::TermCriteria(
		  cv::TermCriteria::EPS | cv::TermCriteria::COUNT,
		  30, 0.1
	  )
  );

  // POBRANIE PUNKTÓW OBRAZU I OBIEKTU
  // współrzędne punktów obiektu to (r,c):
  // (0,0), (board_w-1,0), (0,board_h-1), (board_w-1,board_h-1)
  // to znaczy, że rogi mają współrzędne corners[r*board_w + c]
  //
  cv::Point2f objPts[4], imgPts[4];
  objPts[0].x = 0;
  objPts[0].y = 0;
  objPts[1].x = board_w - 1;
  objPts[1].y = 0;
  objPts[2].x = 0;
  objPts[2].y = board_h - 1;
  objPts[3].x = board_w - 1;
  objPts[3].y = board_h - 1;
  imgPts[0] = corners[0];
  imgPts[1] = corners[board_w - 1];
  imgPts[2] = corners[(board_h - 1) * board_w];
  imgPts[3] = corners[(board_h - 1) * board_w + board_w - 1];

  // rysowanie punktów w ustalonej kolejności: niebieski, zielony, czerwony, żółty
  //
  cv::circle(image, imgPts[0], 9, cv::Scalar(255, 0, 0), 3);
  cv::circle(image, imgPts[1], 9, cv::Scalar(0, 255, 0), 3);
  cv::circle(image, imgPts[2], 9, cv::Scalar(0, 0, 255), 3);
  cv::circle(image, imgPts[3], 9, cv::Scalar(0, 255, 255), 3);                                              

  // RYSOWANIE ZNALEZIONEJ SZACHOWNICY
  //
  cv::drawChessboardCorners(image, board_sz, corners, found);
  cv::imshow("Checkers", image);

  // szukanie homografii
  //
  cv::Mat H = cv::getPerspectiveTransform(objPts, imgPts);

  // NIECH UŻYTKOWNIK SAM USTAWI WYSOKOŚĆ WIDOKU
  //
  cout << "\nNaciśnij klawisz d, aby zmniejszyć wysokość lub u aby zwiększyć wysokość (zmienia się pozorna wysokość Z). Klawisz Esc zamyka program." << endl;
  double Z = 15;
  cv::Mat birds_image;
  for (;;) {
	  // klawisz Esc zatrzymuje program
    H.at<double>(2, 2) = Z;
	// RZUTOWANIE WIDOKU ZA POMOCĄ HOMOGRAFII
	//
	cv::warpPerspective(
		image,        // obraz źródłowy
		birds_image,  // obraz wyjściowy
		H,            // macierz przekształcenia
		image.size(), // rozmiar obrazu wyjściowego
		cv::WARP_INVERSE_MAP | cv::INTER_LINEAR,
		cv::BORDER_CONSTANT,
		cv::Scalar::all(0) // wypełnienie obramowania kolorem czarnym
                        );
    cv::imshow("Birds_Eye", birds_image);
    int key = cv::waitKey() & 255;
    if (key == 'u')
      Z += 0.5;
    if (key == 'd')
      Z -= 0.5;
    if (key == 27)
      break;
  }

  // PREZENTACJA WEKTORÓW ROTACJI I TRANSLACJI
  //
  vector<cv::Point2f> image_points;
  vector<cv::Point3f> object_points;
  for (int i = 0; i < 4; ++i) {
    image_points.push_back(imgPts[i]);
    object_points.push_back(cv::Point3f(objPts[i].x, objPts[i].y, 0));
  }
  cv::Mat rvec, tvec, rmat;
  cv::solvePnP(
	  object_points, // punkty 3D we współrzędnych obiektu
	  image_points,  // punkty 2D we współrzędnych obrazu
	  intrinsic,     // nasza macierz kamery
	  cv::Mat(),     // ponieważ zniekształcenia poprawiliśmy na początku,
					 // teraz mamy zerowe współczynniki zniekształceń
	  rvec,          // wyjściowy *wektor* rotacji
	  tvec           // wyjściowy wektor translacji
               );
  cv::Rodrigues(rvec, rmat);

  // DRUKOWANIE I KOŃCZENIE PRACY
  cout << "Macierz rotacji: " << rmat << endl;
  cout << "Wektor translacji: " << tvec << endl;
  cout << "Macierz homografii: " << H << endl;
  cout << "Odwrócona macierz homografii: " << H.inv() << endl;

  return 1;
}
