#include <algorithm>
#include <boost/algorithm/string.hpp>
#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <iomanip>
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "camodocal/calib/CameraCalibration.h"
#include "camodocal/chessboard/Chessboard.h"
#include "camodocal/gpl/gpl.h"

int main(int argc, char** argv) {
  cv::Size boardSize;
  float squareSize;
  std::string inputDir;
  std::string cameraModel;
  std::string cameraName;
  std::string prefix;
  std::string fileExtension;
  bool useOpenCV;
  bool viewResults;
  bool verbose;

  //========= Handling Program options =========
  boost::program_options::options_description desc("Allowed options");
  desc.add_options()("help", "produce help message")(
      "width,w", boost::program_options::value<int>(&boardSize.width)->default_value(8),
      "Number of inner corners on the chessboard pattern in x direction")(
      "height,h", boost::program_options::value<int>(&boardSize.height)->default_value(12),
      "Number of inner corners on the chessboard pattern in y direction")(
      "size,s", boost::program_options::value<float>(&squareSize)->default_value(7.f), "Size of one square in mm")(
      "input,i", boost::program_options::value<std::string>(&inputDir)->default_value("calibrationdata"),
      "Input directory containing chessboard images")(
      "prefix,p", boost::program_options::value<std::string>(&prefix)->default_value("left-"), "Prefix of images")(
      "file-extension,e", boost::program_options::value<std::string>(&fileExtension)->default_value(".png"),
      "File extension of images")("camera-model",
                                  boost::program_options::value<std::string>(&cameraModel)->default_value("mei"),
                                  "Camera model: kannala-brandt | mei | pinhole")(
      "camera-name", boost::program_options::value<std::string>(&cameraName)->default_value("camera"),
      "Name of camera")("opencv", boost::program_options::bool_switch(&useOpenCV)->default_value(true),
                        "Use OpenCV to detect corners")(
      "view-results", boost::program_options::bool_switch(&viewResults)->default_value(false), "View results")(
      "verbose,v", boost::program_options::bool_switch(&verbose)->default_value(true), "Verbose output");

  boost::program_options::positional_options_description pdesc;
  pdesc.add("input", 1);

  boost::program_options::variables_map vm;
  boost::program_options::store(
      boost::program_options::command_line_parser(argc, argv).options(desc).positional(pdesc).run(), vm);
  boost::program_options::notify(vm);

  if (vm.count("help")) {
    std::cout << desc << std::endl;
    return 1;
  }

  if (!boost::filesystem::exists(inputDir) && !boost::filesystem::is_directory(inputDir)) {
    std::cerr << "# ERROR: Cannot find input directory " << inputDir << "." << std::endl;
    return 1;
  }

  camodocal::Camera::ModelType modelType;
  if (boost::iequals(cameraModel, "kannala-brandt")) {
    modelType = camodocal::Camera::KANNALA_BRANDT;
  } else if (boost::iequals(cameraModel, "mei")) {
    modelType = camodocal::Camera::MEI;
  } else if (boost::iequals(cameraModel, "pinhole")) {
    modelType = camodocal::Camera::PINHOLE;
  } else if (boost::iequals(cameraModel, "scaramuzza")) {
    modelType = camodocal::Camera::SCARAMUZZA;
  } else {
    std::cerr << "# ERROR: Unknown camera model: " << cameraModel << std::endl;
    return 1;
  }

  switch (modelType) {
    case camodocal::Camera::KANNALA_BRANDT:
      std::cout << "# INFO: Camera model: Kannala-Brandt" << std::endl;
      break;
    case camodocal::Camera::MEI:
      std::cout << "# INFO: Camera model: Mei" << std::endl;
      break;
    case camodocal::Camera::PINHOLE:
      std::cout << "# INFO: Camera model: Pinhole" << std::endl;
      break;
    case camodocal::Camera::SCARAMUZZA:
      std::cout << "# INFO: Camera model: Scaramuzza-Omnidirect" << std::endl;
      break;
  }

  // look for images in input directory
  std::vector<std::string> imageFilenames;
  boost::filesystem::directory_iterator itr;
  for (boost::filesystem::directory_iterator itr(inputDir); itr != boost::filesystem::directory_iterator(); ++itr) {
    if (!boost::filesystem::is_regular_file(itr->status())) {
      continue;
    }

    std::string filename = itr->path().filename().string();

    // check if prefix matches
    if (!prefix.empty()) {
      if (filename.compare(0, prefix.length(), prefix) != 0) {
        continue;
      }
    }

    // // check if file extension matches
    // if (filename.compare(filename.length() - fileExtension.length(), fileExtension.length(), fileExtension) != 0)
    // {
    //     continue;
    // }

    imageFilenames.push_back(itr->path().string());

    if (verbose) {
      std::cerr << "# INFO: Adding " << imageFilenames.back() << std::endl;
    }
  }

  if (imageFilenames.empty()) {
    std::cerr << "# ERROR: No chessboard images found." << std::endl;
    return 1;
  }

  if (verbose) {
    std::cerr << "# INFO: # images: " << imageFilenames.size() << std::endl;
  }

  std::sort(imageFilenames.begin(), imageFilenames.end());

  cv::Mat image = cv::imread(imageFilenames.front(), -1);
  const cv::Size frameSize = image.size();

  camodocal::CameraCalibration calibration(modelType, cameraName, frameSize, boardSize, squareSize);
  calibration.setVerbose(verbose);

  std::vector<bool> chessboardFound(imageFilenames.size(), false);
  for (size_t i = 0; i < imageFilenames.size(); ++i) {
    image = cv::imread(imageFilenames.at(i), -1);

    camodocal::Chessboard chessboard(boardSize, image);

    chessboard.findCorners(useOpenCV);
    if (chessboard.cornersFound()) {
      if (verbose) {
        std::cerr << "# INFO: Detected chessboard in image " << i + 1 << ", " << imageFilenames.at(i) << std::endl;
      }

      calibration.addChessboardData(chessboard.getCorners());

      cv::Mat sketch;
      chessboard.getSketch().copyTo(sketch);

      cv::imshow("Image", sketch);
      cv::waitKey(50);
    } else if (verbose) {
      std::cerr << "# INFO: Did not detect chessboard in image " << i + 1 << std::endl;
    }
    chessboardFound.at(i) = chessboard.cornersFound();
  }
  cv::destroyWindow("Image");

  if (calibration.sampleCount() < 10) {
    std::cerr << "# ERROR: Insufficient number of detected chessboards." << std::endl;
    return 1;
  }

  if (verbose) {
    std::cerr << "# INFO: Calibrating..." << std::endl;
  }

  double startTime = camodocal::timeInSeconds();

  calibration.calibrate();
  calibration.writeParams(cameraName + "_camera_calib.yaml");
  calibration.writeChessboardData(cameraName + "_chessboard_data.dat");

  if (verbose) {
    std::cout << "# INFO: Calibration took a total time of " << std::fixed << std::setprecision(3)
              << camodocal::timeInSeconds() - startTime << " sec.\n";
  }

  if (verbose) {
    std::cerr << "# INFO: Wrote calibration file to " << cameraName + "_camera_calib.yaml" << std::endl;
  }

  if (viewResults) {
    std::vector<cv::Mat> cbImages;
    std::vector<std::string> cbImageFilenames;

    for (size_t i = 0; i < imageFilenames.size(); ++i) {
      if (!chessboardFound.at(i)) {
        continue;
      }

      cbImages.push_back(cv::imread(imageFilenames.at(i), -1));
      cbImageFilenames.push_back(imageFilenames.at(i));
    }

    // visualize observed and reprojected points
    calibration.drawResults(cbImages);

    for (size_t i = 0; i < cbImages.size(); ++i) {
      cv::putText(cbImages.at(i), cbImageFilenames.at(i), cv::Point(10, 20), cv::FONT_HERSHEY_COMPLEX, 0.5,
                  cv::Scalar(255, 255, 255), 1, CV_AA);
      cv::imshow("Image", cbImages.at(i));
      cv::waitKey(0);
    }
  }

  return 0;
}
