// Copyright (c), ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include "colmap/exe/model.h"

#include "colmap/controllers/option_manager.h"
#include "colmap/controllers/reconstruction_clustering.h"
#include "colmap/estimators/alignment.h"
#include "colmap/estimators/coordinate_frame.h"
#include "colmap/geometry/bbox.h"
#include "colmap/geometry/gps.h"
#include "colmap/math/math.h"
#include "colmap/optim/ransac.h"
#include "colmap/scene/database.h"
#include "colmap/scene/reconstruction_io.h"
#include "colmap/sfm/observation_manager.h"
#include "colmap/util/file.h"
#include "colmap/util/misc.h"
#include "colmap/util/threading.h"

#include <fstream>
#include <locale>
#include <unordered_map>

namespace colmap {
namespace {

Eigen::Vector3d TransformLatLonAltToModelCoords(const Sim3d& tform,
                                                const double lat,
                                                const double lon,
                                                const double alt) {
  // Since this is intended for use in ENU aligned models we want to define the
  // altitude along the ENU frame z axis and not the Earth's radius. Thus, we
  // set the altitude to 0 when converting from LLA to ECEF and then we use the
  // altitude at the end, after scaling, to set it as the z coordinate in the
  // ENU frame.
  Eigen::Vector3d xyz =
      tform * GPSTransform(GPSTransform::Ellipsoid::WGS84)
                  .EllipsoidToECEF({Eigen::Vector3d(lat, lon, 0.0)})[0];
  xyz(2) = tform.scale() * alt;
  return xyz;
}

void WriteBoundingBox(const std::filesystem::path& reconstruction_path,
                      const Eigen::AlignedBox3d& bbox,
                      const std::string& suffix = "") {
  const Eigen::Vector3d extent = bbox.diagonal();
  // write axis-aligned bounding box
  {
    const auto path = reconstruction_path / ("bbox_aligned" + suffix + ".txt");
    std::ofstream file(path, std::ios::trunc);
    THROW_CHECK_FILE_OPEN(file, path);

    // Ensure that we don't lose any precision by storing in text.
    file.imbue(std::locale::classic());
    file.precision(17);
    file << bbox.min().transpose() << '\n';
    file << bbox.max().transpose() << '\n';
  }
  // write oriented bounding box
  {
    const auto path = reconstruction_path / ("bbox_oriented" + suffix + ".txt");
    std::ofstream file(path, std::ios::trunc);
    THROW_CHECK_FILE_OPEN(file, path);

    // Ensure that we don't lose any precision by storing in text.
    file.imbue(std::locale::classic());
    file.precision(17);
    const Eigen::Vector3d center = (bbox.min() + bbox.max()) * 0.5;
    file << center.transpose() << "\n\n";
    file << "1 0 0\n0 1 0\n0 0 1\n\n";
    file << extent.transpose() << '\n';
  }
}

std::vector<Eigen::Vector3d> ConvertCameraLocations(
    const bool ref_is_gps,
    const std::string& alignment_type,
    const std::vector<Eigen::Vector3d>& ref_locations) {
  if (ref_is_gps) {
    const GPSTransform gps_transform(GPSTransform::Ellipsoid::WGS84);
    if (alignment_type != "enu") {
      LOG(INFO) << "Converting Alignment Coordinates from GPS (lat/lon/alt) "
                   "to ECEF.";
      return gps_transform.EllipsoidToECEF(ref_locations);
    } else {
      THROW_CHECK(!ref_locations.empty());
      LOG(INFO) << "Converting Alignment Coordinates from GPS (lat/lon/alt) "
                   "to ENU. Using the first GPS coordinate as the ENU origin: "
                << ref_locations[0].transpose();
      return gps_transform.EllipsoidToENU(ref_locations,
                                          ref_locations[0](0),
                                          ref_locations[0](1),
                                          ref_locations[0](2));
    }
  } else {
    LOG(INFO) << "Cartesian Alignment Coordinates extracted (MUST NOT BE "
                 "GPS coords!).";
    return ref_locations;
  }
}

void ReadFileCameraLocations(const std::filesystem::path& ref_images_path,
                             const bool ref_is_gps,
                             const std::string& alignment_type,
                             std::vector<std::string>* ref_image_names,
                             std::vector<Eigen::Vector3d>* ref_locations) {
  for (const auto& line : ReadTextFileLines(ref_images_path)) {
    std::stringstream line_parser(line);
    line_parser.imbue(std::locale::classic());
    std::string image_name;
    Eigen::Vector3d camera_position;
    THROW_CHECK(line_parser >> image_name >> camera_position[0] >>
                camera_position[1] >> camera_position[2]);
    ref_image_names->push_back(image_name);
    ref_locations->push_back(camera_position);
  }

  *ref_locations =
      ConvertCameraLocations(ref_is_gps, alignment_type, *ref_locations);
}

void ReadDatabaseCameraLocations(const std::filesystem::path& database_path,
                                 const bool ref_is_gps,
                                 const std::string& alignment_type,
                                 std::vector<std::string>* ref_image_names,
                                 std::vector<Eigen::Vector3d>* ref_locations) {
  auto database = Database::Open(database_path);

  // Index pose priors by their associated data ID.
  std::unordered_map<data_t, PosePrior> pose_priors_by_data_id;
  for (const auto& pose_prior : database->ReadAllPosePriors()) {
    pose_priors_by_data_id.emplace(pose_prior.corr_data_id, pose_prior);
  }

  for (const auto& image : database->ReadAllImages()) {
    const auto it = pose_priors_by_data_id.find(image.DataId());
    if (it != pose_priors_by_data_id.end()) {
      ref_image_names->push_back(image.Name());
      const auto& pose_prior = it->second;
      if (ref_is_gps) {
        THROW_CHECK_EQ(static_cast<int>(pose_prior.coordinate_system),
                       static_cast<int>(PosePrior::CoordinateSystem::WGS84));
      }
      ref_locations->push_back(pose_prior.position);
    }
  }

  *ref_locations =
      ConvertCameraLocations(ref_is_gps, alignment_type, *ref_locations);
}

void WriteComparisonErrorsCSV(const std::filesystem::path& path,
                              const std::vector<ImageAlignmentError>& errors) {
  std::ofstream file(path, std::ios::trunc);
  THROW_CHECK_FILE_OPEN(file, path);

  file.imbue(std::locale::classic());
  file.precision(17);
  file << "# Model comparison pose errors: one entry per common image\n";
  file << "# <rotation error (deg)>, <proj center error>\n";
  for (size_t i = 0; i < errors.size(); ++i) {
    file << errors[i].rotation_error_deg << ", " << errors[i].proj_center_error
         << '\n';
  }
}

void PrintErrorStats(std::ostream& out,
                     const AlignmentErrorSummary::Statistics& stats) {
  out << "Min:    " << stats.min << '\n';
  out << "Max:    " << stats.max << '\n';
  out << "Mean:   " << stats.mean << '\n';
  out << "Median: " << stats.median << '\n';
  out << "P90:    " << stats.p90 << '\n';
  out << "P99:    " << stats.p99 << '\n';
}

void PrintComparisonSummary(std::ostream& out,
                            const std::vector<ImageAlignmentError>& errors) {
  if (errors.empty()) {
    out << "Cannot extract error statistics from empty input\n";
    return;
  }
  AlignmentErrorSummary summary = AlignmentErrorSummary::Compute(errors);
  out << "\nRotation errors (degrees)\n";
  PrintErrorStats(out, summary.rotation_errors_deg);
  out << "\nProjection center errors\n";
  PrintErrorStats(out, summary.proj_center_errors);
}

}  // namespace

// Align given reconstruction with user provided cameras positions
// (can be used for geo-registration for instance).
// The cameras positions to be used for aligning the reconstruction
// model must be provided either by a txt file (with each line being: img_name x
// y z) or through a colmap database file containing a prior position for the
// registered images.
//
// Required Options:
// - input_path: path to initial reconstruction model
// - output_path: path to store the aligned reconstruction model
//
// Additional Options:
// - database_path: path to database file with prior positions for
// reconstruction images
// - ref_images_path: path to txt file with prior positions for reconstruction
// images (WARNING: provide only one of the above)
// - ref_is_gps: if true the prior positions are converted from GPS
// (lat/lon/alt) to ECEF or ENU
// - merge_image_and_ref_origins: if true the reconstruction will be shifted so
// that the first prior position is used for its camera position
// - transform_path: path to store the Sim3 transformation used for the
// alignment
// - alignment_type:
//    > plane: align with reconstruction principal plane
//    > ecef: align with ecef coords. (requires gps coords. or user provided
//    ecef coords.)
//    > enu: align with enu coords. (requires gps coords. or user provided enu
//    coords.)
//    > enu-plane: align to ecef and then to enu plane (requires gps
//    coords. or user provided ecef coords.)
//    > enu-plane-unscaled: same as above but do not apply the computed
//    scale when aligning the reconstruction
//    > custom: align to provided coords.
// - min_common_images: minimum number of images with prior positions to perform
// the estimate an alignment
// - estimate_scale: if true apply the computed scale when aligning the
// reconstruction
// - alignment_max_error: ransac error to use
int RunModelAligner(int argc, char** argv) {
  std::filesystem::path input_path;
  std::filesystem::path output_path;
  std::filesystem::path database_path;
  std::filesystem::path ref_model_path;
  std::filesystem::path ref_images_path;
  bool ref_is_gps = true;
  bool merge_origins = false;
  std::filesystem::path transform_path;
  std::string alignment_type = "custom";
  int min_common_images = 3;
  RANSACOptions ransac_options;

  OptionManager options;
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("output_path", &output_path);
  options.AddDefaultOption("database_path", &database_path);
  options.AddDefaultOption("ref_model_path", &ref_model_path);
  options.AddDefaultOption("ref_images_path", &ref_images_path);
  options.AddDefaultOption("ref_is_gps", &ref_is_gps);
  options.AddDefaultOption("merge_image_and_ref_origins", &merge_origins);
  options.AddDefaultOption("transform_path", &transform_path);
  options.AddDefaultOption(
      "alignment_type",
      &alignment_type,
      "{plane, ecef, enu, enu-plane, enu-plane-unscaled, custom}");
  options.AddDefaultOption("min_common_images", &min_common_images);
  options.AddDefaultOption("alignment_max_error", &ransac_options.max_error);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  StringToLower(&alignment_type);
  const std::unordered_set<std::string> alignment_options{
      "plane", "ecef", "enu", "enu-plane", "enu-plane-unscaled", "custom"};
  if (alignment_options.count(alignment_type) == 0) {
    LOG(ERROR) << "Invalid `alignment_type` - supported values are "
                  "{'plane', 'ecef', 'enu', 'enu-plane', 'enu-plane-unscaled', "
                  "'custom'}";
    return EXIT_FAILURE;
  }

  if (ransac_options.max_error <= 0) {
    LOG(ERROR) << "You must provide a maximum alignment error > 0";
    return EXIT_FAILURE;
  }

  if (ref_model_path.empty() && database_path.empty() &&
      ref_images_path.empty() && alignment_type != "plane") {
    LOG(ERROR) << "One of the following arguments must be specified: "
                  "--ref_model_path, --database_path, "
                  "--ref_images_path, --alignment_type=plane";
    return EXIT_FAILURE;
  }

  std::vector<std::string> ref_image_names;
  std::vector<Eigen::Vector3d> ref_locations;
  if (!ref_model_path.empty() && database_path.empty()) {
    Reconstruction reconstruction;
    reconstruction.Read(ref_model_path);
    for (const auto& image : reconstruction.Images()) {
      ref_image_names.push_back(image.second.Name());
      ref_locations.push_back(image.second.ProjectionCenter());
    }
  } else if (!ref_images_path.empty() && database_path.empty()) {
    ReadFileCameraLocations(ref_images_path,
                            ref_is_gps,
                            alignment_type,
                            &ref_image_names,
                            &ref_locations);
  } else if (!database_path.empty() && ref_images_path.empty()) {
    ReadDatabaseCameraLocations(database_path,
                                ref_is_gps,
                                alignment_type,
                                &ref_image_names,
                                &ref_locations);
  } else if (alignment_type != "plane") {
    LOG(ERROR) << "Use location file or database, not both";
    return EXIT_FAILURE;
  }

  if (alignment_type != "plane" &&
      static_cast<int>(ref_locations.size()) < min_common_images) {
    LOG(ERROR) << "Cannot align with insufficient reference locations.";
    return EXIT_FAILURE;
  }

  Reconstruction reconstruction;
  reconstruction.Read(input_path);
  Sim3d tform;

  if (alignment_type == "plane") {
    LOG_HEADING2("Aligning reconstruction to principal plane");
    AlignToPrincipalPlane(&reconstruction, &tform);
  } else {
    LOG_HEADING2("Aligning reconstruction to " + alignment_type);
    LOG(INFO) << StringPrintf("=> Using %d reference images",
                              ref_image_names.size());

    const bool alignment_success =
        AlignReconstructionToLocations(reconstruction,
                                       ref_image_names,
                                       ref_locations,
                                       min_common_images,
                                       ransac_options,
                                       &tform);

    if (!alignment_success) {
      LOG(ERROR) << "=> Alignment failed";
      return EXIT_FAILURE;
    }

    reconstruction.Transform(tform);

    std::vector<double> errors;
    errors.reserve(ref_image_names.size());

    for (size_t i = 0; i < ref_image_names.size(); ++i) {
      const Image* image = reconstruction.FindImageWithName(ref_image_names[i]);
      if (image != nullptr) {
        errors.push_back((image->ProjectionCenter() - ref_locations[i]).norm());
      }
    }
    LOG(INFO) << StringPrintf("=> Alignment error: %f (mean), %f (median)",
                              Mean(errors),
                              Median(errors));

    if (alignment_success && StringStartsWith(alignment_type, "enu-plane")) {
      LOG_HEADING2("Aligning ECEF aligned reconstruction to ENU plane");
      AlignToENUPlane(
          &reconstruction, &tform, alignment_type == "enu-plane-unscaled");
    }
  }

  if (merge_origins) {
    for (size_t i = 0; i < ref_image_names.size(); i++) {
      const Image* first_image =
          reconstruction.FindImageWithName(ref_image_names[i]);

      if (first_image != nullptr) {
        const Eigen::Vector3d& first_img_position = ref_locations[i];
        const Eigen::Vector3d trans_align =
            first_img_position - first_image->ProjectionCenter();
        const Sim3d origin_align(
            1.0, Eigen::Quaterniond::Identity(), trans_align);

        LOG(INFO) << "\n Aligning reconstruction's origin with ref origin: "
                  << first_img_position.transpose() << '\n';

        reconstruction.Transform(origin_align);

        // Update the Sim3 transformation in case it is stored next.
        tform = Sim3d(
            tform.scale(), tform.rotation(), tform.translation() + trans_align);

        break;
      }
    }
  }

  LOG(INFO) << "=> Alignment succeeded";
  reconstruction.Write(output_path);
  if (!transform_path.empty()) {
    tform.ToFile(transform_path);
  }

  return EXIT_SUCCESS;
}

int RunModelAnalyzer(int argc, char** argv) {
  std::filesystem::path path;
  bool verbose = false;

  OptionManager options;
  options.AddRequiredOption("path", &path);
  options.AddDefaultOption("verbose", &verbose);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  Reconstruction reconstruction;
  reconstruction.Read(path);

  LOG(INFO) << StringPrintf("Rigs: %d", reconstruction.NumRigs());
  LOG(INFO) << StringPrintf("Cameras: %d", reconstruction.NumCameras());
  LOG(INFO) << StringPrintf("Frames: %d", reconstruction.NumFrames());
  LOG(INFO) << StringPrintf("Registered frames: %d",
                            reconstruction.NumRegFrames());
  LOG(INFO) << StringPrintf("Images: %d", reconstruction.NumImages());
  LOG(INFO) << StringPrintf("Registered images: %d",
                            reconstruction.NumRegImages());
  LOG(INFO) << StringPrintf("Points: %d", reconstruction.NumPoints3D());
  LOG(INFO) << StringPrintf("Observations: %d",
                            reconstruction.ComputeNumObservations());
  LOG(INFO) << StringPrintf("Mean track length: %f",
                            reconstruction.ComputeMeanTrackLength());
  LOG(INFO) << StringPrintf(
      "Mean observations per image: %f",
      reconstruction.ComputeMeanObservationsPerRegImage());
  LOG(INFO) << StringPrintf("Mean reprojection error: %fpx",
                            reconstruction.ComputeMeanReprojectionError());

  // verbose information
  if (verbose) {
    LOG_HEADING2("Cameras");
    for (const auto& camera : reconstruction.Cameras()) {
      LOG(INFO) << StringPrintf(" - Camera Id: %d, Model Name: %s, Params: %s",
                                camera.first,
                                camera.second.ModelName().c_str(),
                                camera.second.ParamsToString().c_str());
    }

    LOG_HEADING2("Images");
    for (const auto& image_id : reconstruction.RegImageIds()) {
      LOG(INFO) << StringPrintf(" - Registered Image Id: %d, Name: %s",
                                image_id,
                                reconstruction.Image(image_id).Name().c_str());
    }
  }

  return EXIT_SUCCESS;
}

int RunModelClusterer(int argc, char** argv) {
  std::filesystem::path input_path;
  std::filesystem::path output_path;

  OptionManager options;
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("output_path", &output_path);
  options.AddReconstructionClustererOptions();
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  if (!ExistsDir(input_path)) {
    LOG(ERROR) << "`input_path` is not a directory";
    return EXIT_FAILURE;
  }

  if (!ExistsDir(output_path)) {
    LOG(ERROR) << "`output_path` is not a directory";
    return EXIT_FAILURE;
  }

  LOG_HEADING1("Loading model");
  auto reconstruction = std::make_shared<Reconstruction>();
  reconstruction->Read(input_path);

  auto reconstruction_manager = std::make_shared<ReconstructionManager>();

  ReconstructionClustererController controller(
      *options.reconstruction_clusterer,
      reconstruction,
      reconstruction_manager);
  controller.Run();

  LOG_HEADING1("Writing clustered model(s)");
  reconstruction_manager->Write(output_path);

  return EXIT_SUCCESS;
}

int RunModelComparer(int argc, char** argv) {
  std::filesystem::path input_path1;
  std::filesystem::path input_path2;
  std::filesystem::path output_path;
  std::string alignment_error = "reprojection";
  double min_inlier_observations = 0.3;
  double max_reproj_error = 8.0;
  double max_proj_center_error = 0.1;

  OptionManager options;
  options.AddRequiredOption("input_path1", &input_path1);
  options.AddRequiredOption("input_path2", &input_path2);
  options.AddDefaultOption("output_path", &output_path);
  options.AddDefaultOption(
      "alignment_error", &alignment_error, "{reprojection, proj_center}");
  options.AddDefaultOption("min_inlier_observations", &min_inlier_observations);
  options.AddDefaultOption("max_reproj_error", &max_reproj_error);
  options.AddDefaultOption("max_proj_center_error", &max_proj_center_error);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  if (!output_path.empty() && !ExistsDir(output_path)) {
    LOG(ERROR) << "Provided output path is not a valid directory";
    return EXIT_FAILURE;
  }

  Reconstruction reconstruction1;
  reconstruction1.Read(input_path1);
  Reconstruction reconstruction2;
  reconstruction2.Read(input_path2);
  std::vector<ImageAlignmentError> errors;
  Sim3d rec2_from_rec1;
  bool success = CompareModels(reconstruction1,
                               reconstruction2,
                               alignment_error,
                               min_inlier_observations,
                               max_reproj_error,
                               max_proj_center_error,
                               errors,
                               rec2_from_rec1);
  if (!success) {
    return EXIT_FAILURE;
  }
  if (!output_path.empty()) {
    const auto errors_path = output_path / "errors.csv";
    WriteComparisonErrorsCSV(errors_path, errors);
    const auto summary_path = output_path / "errors_summary.txt";
    std::ofstream file(summary_path, std::ios::trunc);
    THROW_CHECK_FILE_OPEN(file, summary_path);
    PrintComparisonSummary(file, errors);
  }
  return EXIT_SUCCESS;
}

bool CompareModels(const Reconstruction& reconstruction1,
                   const Reconstruction& reconstruction2,
                   const std::string& alignment_error,
                   const double min_inlier_observations,
                   const double max_reproj_error,
                   const double max_proj_center_error,
                   std::vector<ImageAlignmentError>& errors,
                   Sim3d& rec2_from_rec1) {
  LOG_HEADING1("Reconstruction 1");
  LOG(INFO) << StringPrintf("Frames: %d", reconstruction1.NumRegFrames());
  LOG(INFO) << StringPrintf("Images: %d", reconstruction1.NumRegImages());
  LOG(INFO) << StringPrintf("Points: %d", reconstruction1.NumPoints3D());

  LOG_HEADING1("Reconstruction 2");
  LOG(INFO) << StringPrintf("Frames: %d", reconstruction2.NumRegFrames());
  LOG(INFO) << StringPrintf("Images: %d", reconstruction2.NumRegImages());
  LOG(INFO) << StringPrintf("Points: %d", reconstruction2.NumPoints3D());

  LOG_HEADING1("Comparing reconstructed image poses");
  const std::vector<std::pair<image_t, image_t>> common_image_ids =
      reconstruction1.FindCommonRegImageIds(reconstruction2);
  LOG(INFO) << StringPrintf("Common images: %d", common_image_ids.size());

  bool success = false;
  if (alignment_error == "reprojection") {
    success = AlignReconstructionsViaReprojections(
        reconstruction1,
        reconstruction2,
        /*min_inlier_observations=*/min_inlier_observations,
        /*max_reproj_error=*/max_reproj_error,
        &rec2_from_rec1);
  } else if (alignment_error == "proj_center") {
    success = AlignReconstructionsViaProjCenters(
        reconstruction1,
        reconstruction2,
        /*max_proj_center_error=*/max_proj_center_error,
        &rec2_from_rec1);
  } else {
    LOG(ERROR) << "Invalid alignment_error specified.";
    return false;
  }

  if (!success) {
    LOG(INFO) << "=> Reconstruction alignment failed";
    return false;
  }

  LOG(INFO) << "Computed alignment transform:\n" << rec2_from_rec1.ToMatrix();

  errors = ComputeImageAlignmentError(
      reconstruction1, reconstruction2, rec2_from_rec1);

  LOG_HEADING2("Image alignment error summary");
  PrintComparisonSummary(std::cout, errors);

  return true;
}

int RunModelConverter(int argc, char** argv) {
  std::filesystem::path input_path;
  std::filesystem::path output_path;
  std::string output_type;
  bool skip_distortion = false;

  OptionManager options;
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("output_path", &output_path);
  options.AddRequiredOption("output_type",
                            &output_type,
                            "{BIN, TXT, NVM, Bundler, VRML, PLY, R3D, CAM}");
  options.AddDefaultOption("skip_distortion", &skip_distortion);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  Reconstruction reconstruction;
  reconstruction.Read(input_path);

  StringToLower(&output_type);
  if (output_type == "bin") {
    reconstruction.WriteBinary(output_path);
  } else if (output_type == "txt") {
    reconstruction.WriteText(output_path);
  } else if (output_type == "nvm") {
    ExportNVM(reconstruction, output_path, skip_distortion);
  } else if (output_type == "bundler") {
    ExportBundler(reconstruction,
                  AddFileExtension(output_path, ".bundle.out"),
                  AddFileExtension(output_path, ".list.txt"),
                  skip_distortion);
  } else if (output_type == "r3d") {
    ExportRecon3D(reconstruction, output_path, skip_distortion);
  } else if (output_type == "cam") {
    ExportCam(reconstruction, output_path, skip_distortion);
  } else if (output_type == "ply") {
    ExportPLY(reconstruction, output_path);
  } else if (output_type == "vrml") {
    const auto base_path = output_path.parent_path() / output_path.stem();
    ExportVRML(reconstruction,
               AddFileExtension(base_path, ".images.wrl"),
               AddFileExtension(base_path, ".points3D.wrl"),
               1,
               Eigen::Vector3d(1, 0, 0));
  } else {
    LOG(ERROR) << "Invalid `output_type`";
    return EXIT_FAILURE;
  }

  return EXIT_SUCCESS;
}

int RunModelCropper(int argc, char** argv) {
  Timer timer;
  timer.Start();

  std::filesystem::path input_path;
  std::filesystem::path output_path;
  std::string boundary;
  std::filesystem::path gps_transform_path;
  bool is_gps = false;

  OptionManager options;
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("output_path", &output_path);
  options.AddRequiredOption("boundary", &boundary);
  options.AddDefaultOption("gps_transform_path", &gps_transform_path);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  if (!ExistsDir(input_path)) {
    LOG(ERROR) << "`input_path` is not a directory";
    return EXIT_FAILURE;
  }

  if (!ExistsDir(output_path)) {
    LOG(ERROR) << "`output_path` is not a directory";
    return EXIT_FAILURE;
  }

  std::vector<double> boundary_elements = CSVToVector<double>(boundary);
  if (boundary_elements.size() != 2 && boundary_elements.size() != 6) {
    LOG(ERROR) << "Invalid `boundary` - supported values are "
                  "'x1,y1,z1,x2,y2,z2' or 'p1,p2'.";
    return EXIT_FAILURE;
  }

  Reconstruction reconstruction;
  reconstruction.Read(input_path);

  LOG_HEADING2("Calculating boundary coordinates");
  Eigen::AlignedBox3d bounding_box;
  if (boundary_elements.size() == 6) {
    Sim3d tform;
    if (!gps_transform_path.empty()) {
      LOG_HEADING2("Reading model to ECEF transform");
      is_gps = true;
      tform = Inverse(Sim3d::FromFile(gps_transform_path));
    }
    bounding_box.min() =
        is_gps ? TransformLatLonAltToModelCoords(tform,
                                                 boundary_elements[0],
                                                 boundary_elements[1],
                                                 boundary_elements[2])
               : Eigen::Vector3d(boundary_elements[0],
                                 boundary_elements[1],
                                 boundary_elements[2]);
    bounding_box.max() =
        is_gps ? TransformLatLonAltToModelCoords(tform,
                                                 boundary_elements[3],
                                                 boundary_elements[4],
                                                 boundary_elements[5])
               : Eigen::Vector3d(boundary_elements[3],
                                 boundary_elements[4],
                                 boundary_elements[5]);
  } else {
    bounding_box = reconstruction.ComputeBoundingBox(boundary_elements[0],
                                                     boundary_elements[1]);
  }

  LOG_HEADING2("Cropping reconstruction");
  reconstruction.Crop(bounding_box).Write(output_path);
  WriteBoundingBox(output_path, bounding_box);

  LOG(INFO) << "=> Cropping succeeded";
  timer.PrintMinutes();
  return EXIT_SUCCESS;
}

int RunModelMerger(int argc, char** argv) {
  std::filesystem::path input_path1;
  std::filesystem::path input_path2;
  std::filesystem::path output_path;
  double max_reproj_error = 64.0;

  OptionManager options;
  options.AddRequiredOption("input_path1", &input_path1);
  options.AddRequiredOption("input_path2", &input_path2);
  options.AddRequiredOption("output_path", &output_path);
  options.AddDefaultOption("max_reproj_error", &max_reproj_error);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  Reconstruction reconstruction1;
  reconstruction1.Read(input_path1);
  LOG_HEADING2("Reconstruction 1");
  LOG(INFO) << StringPrintf("Images: %d", reconstruction1.NumRegImages());
  LOG(INFO) << StringPrintf("Points: %d", reconstruction1.NumPoints3D());

  Reconstruction reconstruction2;
  reconstruction2.Read(input_path2);
  LOG_HEADING2("Reconstruction 2");
  LOG(INFO) << StringPrintf("Images: %d", reconstruction2.NumRegImages());
  LOG(INFO) << StringPrintf("Points: %d", reconstruction2.NumPoints3D());

  LOG_HEADING2("Merging reconstructions");
  if (MergeAndFilterReconstructions(
          max_reproj_error, reconstruction1, reconstruction2)) {
    LOG(INFO) << "=> Merge succeeded";
    LOG_HEADING2("Merged reconstruction");
    LOG(INFO) << StringPrintf("Images: %d", reconstruction2.NumRegImages());
    LOG(INFO) << StringPrintf("Points: %d", reconstruction2.NumPoints3D());
  } else {
    LOG(INFO) << "=> Merge failed";
  }

  reconstruction2.Write(output_path);

  return EXIT_SUCCESS;
}

int RunModelOrientationAligner(int argc, char** argv) {
  std::filesystem::path input_path;
  std::filesystem::path output_path;
#ifdef COLMAP_LSD_ENABLED
  std::string method = "MANHATTAN-WORLD";
#else
  std::string method = "IMAGE-ORIENTATION";
#endif

  ManhattanWorldFrameEstimationOptions frame_estimation_options;

  OptionManager options;
  options.AddImageOptions();
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("output_path", &output_path);
#ifdef COLMAP_LSD_ENABLED
  options.AddDefaultOption(
      "method", &method, "{MANHATTAN-WORLD, IMAGE-ORIENTATION}");
#else
  options.AddDefaultOption("method", &method, "{IMAGE-ORIENTATION}");
#endif
  options.AddDefaultOption("max_image_size",
                           &frame_estimation_options.max_image_size);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  StringToLower(&method);
#ifdef COLMAP_LSD_ENABLED
  if (method != "manhattan-world" && method != "image-orientation") {
    LOG(ERROR) << "Invalid `method` - supported values are "
                  "'MANHATTAN-WORLD' or 'IMAGE-ORIENTATION'.";
    return EXIT_FAILURE;
  }
#else
  if (method != "image-orientation") {
    LOG(ERROR) << "Invalid `method` - supported values are "
                  "'IMAGE-ORIENTATION'.";
    return EXIT_FAILURE;
  }
#endif

  Reconstruction reconstruction;
  reconstruction.Read(input_path);

  LOG_HEADING1("Aligning Reconstruction");

  Sim3d new_from_old_world;

#ifdef COLMAP_LSD_ENABLED
  if (method == "manhattan-world") {
    const Eigen::Matrix3d frame = EstimateManhattanWorldFrame(
        frame_estimation_options, reconstruction, options.image_path->string());

    if (frame.col(0).lpNorm<1>() == 0) {
      LOG(INFO) << "Only aligning vertical axis";
      new_from_old_world.rotation() = Eigen::Quaterniond::FromTwoVectors(
          frame.col(1), Eigen::Vector3d(0, 1, 0));
    } else if (frame.col(1).lpNorm<1>() == 0) {
      new_from_old_world.rotation() = Eigen::Quaterniond::FromTwoVectors(
          frame.col(0), Eigen::Vector3d(1, 0, 0));
      LOG(INFO) << "Only aligning horizontal axis";
    } else {
      new_from_old_world.rotation() = Eigen::Quaterniond(frame.transpose());
      LOG(INFO) << "Aligning horizontal and vertical axes";
    }
  } else if (method == "image-orientation") {
#else
  if (method == "image-orientation") {
#endif
    const Eigen::Vector3d gravity_axis =
        EstimateGravityVectorFromImageOrientation(reconstruction);
    new_from_old_world.rotation() = Eigen::Quaterniond::FromTwoVectors(
        gravity_axis, Eigen::Vector3d(0, 1, 0));

  } else {
    LOG(FATAL_THROW) << "Alignment method not supported";
  }

  LOG(INFO) << "Using the rotation matrix:";
  LOG(INFO) << new_from_old_world.rotation().toRotationMatrix();

  reconstruction.Transform(new_from_old_world);

  LOG(INFO) << "Writing aligned reconstruction...";
  reconstruction.Write(output_path);

  return EXIT_SUCCESS;
}

int RunModelSplitter(int argc, char** argv) {
  Timer timer;
  timer.Start();

  std::filesystem::path input_path;
  std::filesystem::path output_path;
  std::string split_type;
  std::string split_params;
  std::filesystem::path gps_transform_path;
  int num_threads = -1;
  int min_reg_images = 10;
  int min_num_points = 100;
  double overlap_ratio = 0.0;
  double min_area_ratio = 0.0;
  bool is_gps = false;

  OptionManager options;
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("output_path", &output_path);
  options.AddRequiredOption(
      "split_type", &split_type, "{tiles, extent, parts}");
  options.AddRequiredOption("split_params", &split_params);
  options.AddDefaultOption("gps_transform_path", &gps_transform_path);
  options.AddDefaultOption("num_threads", &num_threads);
  options.AddDefaultOption("min_reg_images", &min_reg_images);
  options.AddDefaultOption("min_num_points", &min_num_points);
  options.AddDefaultOption("overlap_ratio", &overlap_ratio);
  options.AddDefaultOption("min_area_ratio", &min_area_ratio);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  if (!ExistsDir(input_path)) {
    LOG(ERROR) << "`input_path` is not a directory";
    return EXIT_FAILURE;
  }

  if (!ExistsDir(output_path)) {
    LOG(ERROR) << "`output_path` is not a directory";
    return EXIT_FAILURE;
  }

  if (overlap_ratio < 0) {
    LOG(WARNING) << "Invalid `overlap_ratio`; resetting to 0";
    overlap_ratio = 0.0;
  }

  LOG_HEADING1("Splitting sparse model");
  LOG(INFO) << StringPrintf("=> Using \"%s\" split type", split_type.c_str());

  Reconstruction reconstruction;
  reconstruction.Read(input_path);

  Sim3d tform;
  if (!gps_transform_path.empty()) {
    LOG_HEADING2("Reading model to ECEF transform");
    is_gps = true;
    tform = Inverse(Sim3d::FromFile(gps_transform_path));
  }

  // Create the necessary number of reconstructions based on the split method
  // and get the bounding boxes for each sub-reconstruction
  LOG_HEADING2("Computing bounding boxes");
  std::vector<std::string> tile_keys;
  std::vector<Eigen::AlignedBox3d> exact_bboxes;
  StringToLower(&split_type);
  if (split_type == "tiles") {
    std::ifstream file(split_params);
    THROW_CHECK_FILE_OPEN(file, split_params);
    file.imbue(std::locale::classic());

    double x1, y1, z1, x2, y2, z2;
    std::string tile_key;
    std::vector<Eigen::AlignedBox3d> bounds;
    tile_keys.clear();
    file >> tile_key >> x1 >> y1 >> z1 >> x2 >> y2 >> z2;
    while (!file.fail()) {
      tile_keys.push_back(tile_key);
      if (is_gps) {
        exact_bboxes.emplace_back(
            TransformLatLonAltToModelCoords(tform, x1, y1, z1),
            TransformLatLonAltToModelCoords(tform, x2, y2, z2));
      } else {
        exact_bboxes.emplace_back(Eigen::Vector3d(x1, y1, z1),
                                  Eigen::Vector3d(x2, y2, z2));
      }
      file >> tile_key >> x1 >> y1 >> z1 >> x2 >> y2 >> z2;
    }
  } else if (split_type == "extent") {
    std::vector<double> parts = CSVToVector<double>(split_params);
    Eigen::Vector3d extent(std::numeric_limits<double>::max(),
                           std::numeric_limits<double>::max(),
                           std::numeric_limits<double>::max());
    for (size_t i = 0; i < parts.size(); ++i) {
      extent(i) = parts[i] * tform.scale();
    }

    const Eigen::AlignedBox3d bbox = reconstruction.ComputeBoundingBox();
    const Eigen::Vector3d full_bbox = bbox.diagonal();
    const Eigen::Vector3i split(static_cast<int>(full_bbox(0) / extent(0)) + 1,
                                static_cast<int>(full_bbox(1) / extent(1)) + 1,
                                static_cast<int>(full_bbox(2) / extent(2)) + 1);

    exact_bboxes = ComputeEqualPartsBboxes(bbox, split);
  } else if (split_type == "parts") {
    auto parts = CSVToVector<int>(split_params);
    Eigen::Vector3i split(1, 1, 1);
    for (size_t i = 0; i < parts.size(); ++i) {
      split(i) = parts[i];
      if (split(i) < 1) {
        LOG(ERROR) << "Cannot split in less than 1 parts for dim " << i;
        return EXIT_FAILURE;
      }
    }
    exact_bboxes =
        ComputeEqualPartsBboxes(reconstruction.ComputeBoundingBox(), split);
  } else {
    LOG(ERROR) << "Invalid split type: " << split_type;
    return EXIT_FAILURE;
  }

  std::vector<Eigen::AlignedBox3d> padded_bboxes;
  for (const auto& bbox : exact_bboxes) {
    const Eigen::Vector3d padding = overlap_ratio * bbox.diagonal();
    padded_bboxes.emplace_back(bbox.min() - padding, bbox.max() + padding);
  }

  LOG_HEADING2("Applying split and writing reconstructions");
  const size_t num_parts = padded_bboxes.size();
  LOG(INFO) << StringPrintf("=> Splitting to %d parts", num_parts);

  const bool use_tile_keys = split_type == "tiles";

  auto SplitReconstruction = [&](const int idx) {
    Reconstruction tile_recon = reconstruction.Crop(padded_bboxes[idx]);
    // calculate area covered by model as proportion of box area
    const Eigen::Vector3d bbox_extent = padded_bboxes[idx].diagonal();
    const Eigen::AlignedBox3d model_bbox = tile_recon.ComputeBoundingBox();
    const Eigen::Vector3d model_extent = model_bbox.diagonal();
    const double area_ratio =
        (model_extent(0) * model_extent(1)) / (bbox_extent(0) * bbox_extent(1));
    const int tile_num_points = tile_recon.NumPoints3D();

    const std::string name =
        use_tile_keys ? tile_keys[idx] : std::to_string(idx);
    const bool include_tile =
        area_ratio >= min_area_ratio &&       //
        tile_num_points >= min_num_points &&  //
        tile_recon.NumRegImages() >= static_cast<size_t>(min_reg_images);

    if (include_tile) {
      LOG(INFO) << StringPrintf(
          "Writing reconstruction %s with %d images, %d points, "
          "and %.2f%% area coverage",
          name.c_str(),
          tile_recon.NumRegImages(),
          tile_num_points,
          100.0 * area_ratio);
      const auto reconstruction_path = output_path / name;
      CreateDirIfNotExists(reconstruction_path);
      tile_recon.Write(reconstruction_path);
      WriteBoundingBox(reconstruction_path, padded_bboxes[idx]);
      WriteBoundingBox(reconstruction_path, exact_bboxes[idx], "_exact");
    } else {
      LOG(INFO) << StringPrintf(
          "Skipping reconstruction %s with %d images, %d points, "
          "and %.2f%% area coverage",
          name.c_str(),
          tile_recon.NumRegImages(),
          tile_num_points,
          100.0 * area_ratio);
    }
  };

  ThreadPool thread_pool(GetEffectiveNumThreads(num_threads));
  for (size_t idx = 0; idx < num_parts; ++idx) {
    thread_pool.AddTask(SplitReconstruction, idx);
  }
  thread_pool.Wait();

  timer.PrintMinutes();
  return EXIT_SUCCESS;
}

int RunModelTransformer(int argc, char** argv) {
  std::filesystem::path input_path;
  std::filesystem::path output_path;
  std::filesystem::path transform_path;
  bool is_inverse = false;

  OptionManager options;
  options.AddRequiredOption("input_path", &input_path);
  options.AddRequiredOption("output_path", &output_path);
  options.AddRequiredOption("transform_path", &transform_path);
  options.AddDefaultOption("is_inverse", &is_inverse);
  if (!options.Parse(argc, argv)) {
    return EXIT_FAILURE;
  }

  LOG(INFO) << "Reading points input: " << input_path;
  Reconstruction recon;
  bool is_dense = false;
  if (HasFileExtension(input_path, ".ply")) {
    is_dense = true;
    recon.ImportPLY(input_path);
  } else if (ExistsDir(input_path)) {
    recon.Read(input_path);
  } else {
    LOG(ERROR)
        << "Invalid model input; not a PLY file or sparse reconstruction "
           "directory.";
    return EXIT_FAILURE;
  }

  LOG(INFO) << "Reading transform input: " << transform_path;
  Sim3d tform = Sim3d::FromFile(transform_path);
  if (is_inverse) {
    tform = Inverse(tform);
  }

  LOG(INFO) << "Applying transform to recon with " << recon.NumPoints3D()
            << " points";
  recon.Transform(tform);

  LOG(INFO) << "Writing output: " << output_path;
  if (is_dense) {
    ExportPLY(recon, output_path);
  } else {
    recon.Write(output_path);
  }

  return EXIT_SUCCESS;
}

}  // namespace colmap
