/* Copyright (c) 2021, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 *
 * All rights reserved.
 *
 * The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
 * platform" software is licensed under the Apache License, Version 2.0
 * (the "License"); you may not use this file except in compliance with the
 * License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations
 * under the License.
 */

#include <opencv2/xfeatures2d.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>

#include <interest_point.h>           // from the isaac repo
#include <camera_image.h>             // from the isaac repo
#include <interest_point/matching.h>  // from the astrobee repo

// Get rid of warnings beyond our control
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#pragma GCC diagnostic ignored "-Wunused-function"
#pragma GCC diagnostic push
#include <openMVG/multiview/projection.hpp>
#include <openMVG/multiview/rotation_averaging_l1.hpp>
#include <openMVG/multiview/triangulation_nview.hpp>
#include <openMVG/numeric/numeric.h>
#include <openMVG/tracks/tracks.hpp>
#pragma GCC diagnostic pop

#include <boost/filesystem.hpp>

#include <iostream>
#include <fstream>

// SIFT is doing so much better than SURF for haz cam images.
DEFINE_string(refiner_feature_detector, "SIFT", "The feature detector to use. SIFT or SURF.");
DEFINE_int32(sift_nFeatures, 10000, "Number of SIFT features");
DEFINE_int32(sift_nOctaveLayers, 3, "Number of SIFT octave layers");
DEFINE_double(sift_contrastThreshold, 0.02,
              "SIFT contrast threshold");  // decrease for more ip
DEFINE_double(sift_edgeThreshold, 10, "SIFT edge threshold");
DEFINE_double(sift_sigma, 1.6, "SIFT sigma");

namespace dense_map {

void detectFeatures(const cv::Mat& image, bool verbose,
                    // Outputs
                    cv::Mat* descriptors, Eigen::Matrix2Xd* keypoints) {
  bool histogram_equalization = false;

  // If using histogram equalization, need an extra image to store it
  cv::Mat* image_ptr = const_cast<cv::Mat*>(&image);
  cv::Mat hist_image;
  if (histogram_equalization) {
    cv::equalizeHist(image, hist_image);
    image_ptr = &hist_image;
  }

  std::vector<cv::KeyPoint> storage;

  if (FLAGS_refiner_feature_detector == "SIFT") {
    cv::Ptr<cv::xfeatures2d::SIFT> sift =
      cv::xfeatures2d::SIFT::create(FLAGS_sift_nFeatures, FLAGS_sift_nOctaveLayers, FLAGS_sift_contrastThreshold,
                                    FLAGS_sift_edgeThreshold, FLAGS_sift_sigma);
    sift->detect(image, storage);
    sift->compute(image, storage, *descriptors);

  } else if (FLAGS_refiner_feature_detector == "SURF") {
    interest_point::FeatureDetector detector("SURF");
    detector.Detect(*image_ptr, &storage, descriptors);

    // Undo the shift in the detector
    for (cv::KeyPoint& key : storage) {
      key.pt.x += image.cols / 2.0;
      key.pt.y += image.rows / 2.0;
    }

  } else {
    LOG(FATAL) << "Unknown feature detector: " << FLAGS_refiner_feature_detector;
  }

  if (verbose) std::cout << "Features detected " << storage.size() << std::endl;

  // Copy to data structures expected by subsequent code
  keypoints->resize(2, storage.size());
  Eigen::Vector2d output;
  for (size_t j = 0; j < storage.size(); j++) {
    keypoints->col(j) = Eigen::Vector2d(storage[j].pt.x, storage[j].pt.y);
  }
}

// This really likes haz cam first and nav cam second
void matchFeatures(std::mutex* match_mutex, int left_image_index, int right_image_index,
                   cv::Mat const& left_descriptors, cv::Mat const& right_descriptors,
                   Eigen::Matrix2Xd const& left_keypoints,
                   Eigen::Matrix2Xd const& right_keypoints, bool verbose,
                   // output
                   MATCH_PAIR* matches) {
  std::vector<cv::DMatch> cv_matches;
  interest_point::FindMatches(left_descriptors, right_descriptors, &cv_matches);

  std::vector<cv::Point2f> left_vec;
  std::vector<cv::Point2f> right_vec;
  for (size_t j = 0; j < cv_matches.size(); j++) {
    int left_ip_index = cv_matches.at(j).queryIdx;
    int right_ip_index = cv_matches.at(j).trainIdx;

    // Get the keypoints from the good matches
    left_vec.push_back(cv::Point2f(left_keypoints.col(left_ip_index)[0], left_keypoints.col(left_ip_index)[1]));
    right_vec.push_back(cv::Point2f(right_keypoints.col(right_ip_index)[0], right_keypoints.col(right_ip_index)[1]));
  }

  if (left_vec.empty()) return;

  // These may need some tweaking but works reasonably well.
  double ransacReprojThreshold = 20.0;
  cv::Mat inlier_mask;
  int maxIters = 10000;
  double confidence = 0.8;

  // affine2D works better than homography
  // cv::Mat H = cv::findHomography(left_vec, right_vec, cv::RANSAC,
  // ransacReprojThreshold, inlier_mask, maxIters, confidence);
  cv::Mat H = cv::estimateAffine2D(left_vec, right_vec, inlier_mask, cv::RANSAC,
                                   ransacReprojThreshold, maxIters, confidence);

  std::vector<InterestPoint> left_ip, right_ip;
  for (size_t j = 0; j < cv_matches.size(); j++) {
    int left_ip_index = cv_matches.at(j).queryIdx;
    int right_ip_index = cv_matches.at(j).trainIdx;

    if (inlier_mask.at<uchar>(j, 0) == 0) continue;

    cv::Mat left_desc = left_descriptors.row(left_ip_index);
    cv::Mat right_desc = right_descriptors.row(right_ip_index);

    InterestPoint left;
    left.setFromCvKeypoint(left_keypoints.col(left_ip_index), left_desc);

    InterestPoint right;
    right.setFromCvKeypoint(right_keypoints.col(right_ip_index), right_desc);

    left_ip.push_back(left);
    right_ip.push_back(right);
  }

  // Update the shared variable using a lock
  match_mutex->lock();

  // Print the verbose message inside the lock, otherwise the text
  // may get messed up.
  if (verbose)
    std::cout << "Number of matches for pair "
              << left_image_index << ' ' << right_image_index << ": "
              << left_ip.size() << std::endl;

  *matches = std::make_pair(left_ip, right_ip);
  match_mutex->unlock();
}

// Match features while assuming that the input cameras can be used to filter out
// outliers by reprojection error.
void matchFeaturesWithCams(std::mutex* match_mutex, int left_image_index, int right_image_index,
                           camera::CameraParameters const& left_params,
                           camera::CameraParameters const& right_params,
                           Eigen::Affine3d const& left_world_to_cam,
                           Eigen::Affine3d const& right_world_to_cam,
                           double reprojection_error,
                           cv::Mat const& left_descriptors, cv::Mat const& right_descriptors,
                           Eigen::Matrix2Xd const& left_keypoints,
                           Eigen::Matrix2Xd const& right_keypoints,
                           bool verbose,
                           // output
                           MATCH_PAIR* matches) {
  // Match by using descriptors first
  std::vector<cv::DMatch> cv_matches;
  interest_point::FindMatches(left_descriptors, right_descriptors, &cv_matches);

  // Do filtering
  std::vector<cv::Point2f> left_vec;
  std::vector<cv::Point2f> right_vec;
  std::vector<cv::DMatch> filtered_cv_matches;
  for (size_t j = 0; j < cv_matches.size(); j++) {
    int left_ip_index = cv_matches.at(j).queryIdx;
    int right_ip_index = cv_matches.at(j).trainIdx;

    Eigen::Vector2d dist_left_ip(left_keypoints.col(left_ip_index)[0],
                                 left_keypoints.col(left_ip_index)[1]);

    Eigen::Vector2d dist_right_ip(right_keypoints.col(right_ip_index)[0],
                                  right_keypoints.col(right_ip_index)[1]);

    Eigen::Vector2d undist_left_ip;
    Eigen::Vector2d undist_right_ip;
    left_params.Convert<camera::DISTORTED,  camera::UNDISTORTED_C>
      (dist_left_ip, &undist_left_ip);
    right_params.Convert<camera::DISTORTED, camera::UNDISTORTED_C>
      (dist_right_ip, &undist_right_ip);

    Eigen::Vector3d X =
      dense_map::TriangulatePair(left_params.GetFocalLength(), right_params.GetFocalLength(),
                                 left_world_to_cam, right_world_to_cam,
                                 undist_left_ip, undist_right_ip);

    // Project back into the cameras
    Eigen::Vector3d left_cam_X = left_world_to_cam * X;
    Eigen::Vector2d undist_left_pix
      = left_params.GetFocalVector().cwiseProduct(left_cam_X.hnormalized());
    Eigen::Vector2d dist_left_pix;
    left_params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undist_left_pix,
                                                                  &dist_left_pix);

    Eigen::Vector3d right_cam_X = right_world_to_cam * X;
    Eigen::Vector2d undist_right_pix
      = right_params.GetFocalVector().cwiseProduct(right_cam_X.hnormalized());
    Eigen::Vector2d dist_right_pix;
    right_params.Convert<camera::UNDISTORTED_C, camera::DISTORTED>(undist_right_pix,
                                                                   &dist_right_pix);

    // Filter out points whose reprojection error is too big
    bool is_good = ((dist_left_ip - dist_left_pix).norm() <= reprojection_error &&
                    (dist_right_ip - dist_right_pix).norm() <= reprojection_error);

    // If any values above are Inf or NaN, is_good will be false as well
    if (!is_good) continue;

    // Get the keypoints from the good matches
    left_vec.push_back(cv::Point2f(left_keypoints.col(left_ip_index)[0],
                                   left_keypoints.col(left_ip_index)[1]));
    right_vec.push_back(cv::Point2f(right_keypoints.col(right_ip_index)[0],
                                    right_keypoints.col(right_ip_index)[1]));

    filtered_cv_matches.push_back(cv_matches[j]);
  }

  if (left_vec.empty()) return;

  // Filter using geometry constraints
  // These may need some tweaking but works reasonably well.
  double ransacReprojThreshold = 20.0;
  cv::Mat inlier_mask;
  int maxIters = 10000;
  double confidence = 0.8;

  // affine2D works better than homography
  // cv::Mat H = cv::findHomography(left_vec, right_vec, cv::RANSAC,
  // ransacReprojThreshold, inlier_mask, maxIters, confidence);
  cv::Mat H = cv::estimateAffine2D(left_vec, right_vec, inlier_mask, cv::RANSAC,
                                   ransacReprojThreshold, maxIters, confidence);

  std::vector<InterestPoint> left_ip, right_ip;
  for (size_t j = 0; j < filtered_cv_matches.size(); j++) {
    int left_ip_index = filtered_cv_matches.at(j).queryIdx;
    int right_ip_index = filtered_cv_matches.at(j).trainIdx;

    if (inlier_mask.at<uchar>(j, 0) == 0) continue;

    cv::Mat left_desc = left_descriptors.row(left_ip_index);
    cv::Mat right_desc = right_descriptors.row(right_ip_index);

    InterestPoint left;
    left.setFromCvKeypoint(left_keypoints.col(left_ip_index), left_desc);

    InterestPoint right;
    right.setFromCvKeypoint(right_keypoints.col(right_ip_index), right_desc);

    left_ip.push_back(left);
    right_ip.push_back(right);
  }

  // Update the shared variable using a lock
  match_mutex->lock();

  // Print the verbose message inside the lock, otherwise the text
  // may get messed up.
  if (verbose)
    std::cout << "Number of matches for pair "
              << left_image_index << ' ' << right_image_index << ": "
              << left_ip.size() << std::endl;

  *matches = std::make_pair(left_ip, right_ip);
  match_mutex->unlock();
}

void writeIpRecord(std::ofstream& f, InterestPoint const& p) {
  f.write(reinterpret_cast<const char*>(&(p.x)), sizeof(p.x));
  f.write(reinterpret_cast<const char*>(&(p.y)), sizeof(p.y));
  f.write(reinterpret_cast<const char*>(&(p.ix)), sizeof(p.ix));
  f.write(reinterpret_cast<const char*>(&(p.iy)), sizeof(p.iy));
  f.write(reinterpret_cast<const char*>(&(p.orientation)), sizeof(p.orientation));
  f.write(reinterpret_cast<const char*>(&(p.scale)), sizeof(p.scale));
  f.write(reinterpret_cast<const char*>(&(p.interest)), sizeof(p.interest));
  f.write(reinterpret_cast<const char*>(&(p.polarity)), sizeof(p.polarity));
  f.write(reinterpret_cast<const char*>(&(p.octave)), sizeof(p.octave));
  f.write(reinterpret_cast<const char*>(&(p.scale_lvl)), sizeof(p.scale_lvl));
  uint64_t size = p.size();
  f.write(reinterpret_cast<const char*>((&size)), sizeof(uint64));
  for (size_t i = 0; i < p.descriptor.size(); ++i)
    f.write(reinterpret_cast<const char*>(&(p.descriptor[i])), sizeof(p.descriptor[i]));
}

// Write matches to disk
void writeMatchFile(std::string match_file, std::vector<InterestPoint> const& ip1,
                    std::vector<InterestPoint> const& ip2) {
  std::ofstream f;
  f.open(match_file.c_str(), std::ios::binary | std::ios::out);
  std::vector<InterestPoint>::const_iterator iter1 = ip1.begin();
  std::vector<InterestPoint>::const_iterator iter2 = ip2.begin();
  uint64 size1 = ip1.size();
  uint64 size2 = ip2.size();
  f.write(reinterpret_cast<const char*>(&size1), sizeof(uint64));
  f.write(reinterpret_cast<const char*>(&size2), sizeof(uint64));
  for (; iter1 != ip1.end(); ++iter1) writeIpRecord(f, *iter1);
  for (; iter2 != ip2.end(); ++iter2) writeIpRecord(f, *iter2);
  f.close();
}

void saveImagesAndMatches(std::string const& left_prefix, std::string const& right_prefix,
                          std::pair<int, int> const& index_pair, MATCH_PAIR const& match_pair,
                          std::vector<cv::Mat> const& images) {
  // Add 10000 to have them be listed nicely
  std::ostringstream oss_left;
  oss_left << left_prefix << "_image" << index_pair.first + 10000 << ".jpg";
  std::string left_image_file = oss_left.str();
  std::cout << "Writing: " << left_image_file << std::endl;
  cv::imwrite(left_image_file, images[index_pair.first]);

  std::ostringstream oss_right;
  oss_right << right_prefix << "_image" << index_pair.second + 10000 << ".jpg";
  std::string right_image_file = oss_right.str();
  std::cout << "Writing: " << right_image_file << std::endl;
  cv::imwrite(right_image_file, images[index_pair.second]);

  std::string left_stem = boost::filesystem::path(left_image_file).stem().string();
  std::string right_stem = boost::filesystem::path(right_image_file).stem().string();

  std::string match_file = left_stem + "__" + right_stem + ".match";
  std::cout << "Writing: " << left_image_file << ' ' << right_image_file << ' ' << match_file << std::endl;
  writeMatchFile(match_file, match_pair.first, match_pair.second);
}

// Triangulate rays emanating from given undistorted and centered pixels
Eigen::Vector3d TriangulatePair(double focal_length1, double focal_length2,
                                Eigen::Affine3d const& world_to_cam1,
                                Eigen::Affine3d const& world_to_cam2,
                                Eigen::Vector2d const& pix1,
                                Eigen::Vector2d const& pix2) {
  Eigen::Matrix3d k1;
  k1 << focal_length1, 0, 0, 0, focal_length1, 0, 0, 0, 1;

  Eigen::Matrix3d k2;
  k2 << focal_length2, 0, 0, 0, focal_length2, 0, 0, 0, 1;

  openMVG::Mat34 cid_to_p1, cid_to_p2;
  openMVG::P_From_KRt(k1, world_to_cam1.linear(), world_to_cam1.translation(), &cid_to_p1);
  openMVG::P_From_KRt(k2, world_to_cam2.linear(), world_to_cam2.translation(), &cid_to_p2);

  openMVG::Triangulation tri;
  tri.add(cid_to_p1, pix1);
  tri.add(cid_to_p2, pix2);

  Eigen::Vector3d solution = tri.compute();
  return solution;
}

// Triangulate n rays emanating from given undistorted and centered pixels
Eigen::Vector3d Triangulate(std::vector<double>          const& focal_length_vec,
                            std::vector<Eigen::Affine3d> const& world_to_cam_vec,
                            std::vector<Eigen::Vector2d> const& pix_vec) {
  if (focal_length_vec.size() != world_to_cam_vec.size() ||
      focal_length_vec.size() != pix_vec.size())
    LOG(FATAL) << "All inputs to Triangulate() must have the same size.";

  if (focal_length_vec.size() <= 1)
    LOG(FATAL) << "At least two rays must be passed to Triangulate().";

  openMVG::Triangulation tri;

  for (size_t it = 0; it < focal_length_vec.size(); it++) {
    Eigen::Matrix3d k;
    k << focal_length_vec[it], 0, 0, 0, focal_length_vec[it], 0, 0, 0, 1;

    openMVG::Mat34 cid_to_p;
    openMVG::P_From_KRt(k, world_to_cam_vec[it].linear(), world_to_cam_vec[it].translation(),
                        &cid_to_p);

    tri.add(cid_to_p, pix_vec[it]);
  }

  Eigen::Vector3d solution = tri.compute();
  return solution;
}

void detectMatchFeatures(  // Inputs
                         std::vector<dense_map::cameraImage> const& cams,
                         std::vector<std::string> const& cam_names,
                         std::vector<camera::CameraParameters> const& cam_params,
                         std::vector<Eigen::Affine3d> const& world_to_cam, int num_overlaps,
                         int initial_max_reprojection_error, int num_match_threads,
                         bool verbose,
                         // Outputs
                         std::vector<std::vector<std::pair<float, float>>>& keypoint_vec,
                         std::vector<std::map<int, int>>& pid_to_cid_fid,
                         std::vector<std::string> & image_files) {
  // Wipe the outputs
  keypoint_vec.clear();
  pid_to_cid_fid.clear();
  image_files.clear();

  if (verbose) {
    int count = 10000;
    for (size_t it = 0; it < cams.size(); it++) {
      std::ostringstream oss;
      oss << count << "_" << cam_names[cams[it].camera_type] << ".jpg";
      std::string name = oss.str();
      std::cout << "Writing: " << name << std::endl;
      cv::imwrite(name, cams[it].image);
      count++;
      image_files.push_back(name);
    }
  }

  // Detect features using multiple threads. Too many threads may result
  // in high memory usage.
  std::ostringstream oss;
  oss << num_match_threads;
  std::string num_threads = oss.str();
  google::SetCommandLineOption("num_threads", num_threads.c_str());
  if (!gflags::GetCommandLineOption("num_threads", &num_threads))
    LOG(FATAL) << "Failed to get the value of --num_threads in Astrobee software.\n";
  std::cout << "Using " << num_threads << " threads for feature detection/matching." << std::endl;

  std::cout << "Detecting features." << std::endl;

  std::vector<cv::Mat> cid_to_descriptor_map;
  std::vector<Eigen::Matrix2Xd> cid_to_keypoint_map;
  cid_to_descriptor_map.resize(cams.size());
  cid_to_keypoint_map.resize(cams.size());
  {
    // Make the thread pool go out of scope when not needed to not use up memory
    ff_common::ThreadPool thread_pool;
    for (size_t it = 0; it < cams.size(); it++) {
      thread_pool.AddTask
        (&dense_map::detectFeatures,    // multi-thread  // NOLINT
         // dense_map::detectFeatures(  // single-thread // NOLINT
         cams[it].image, verbose, &cid_to_descriptor_map[it], &cid_to_keypoint_map[it]);
    }
    thread_pool.Join();
  }

  MATCH_MAP matches;

  std::vector<std::pair<int, int> > image_pairs;
  for (size_t it1 = 0; it1 < cams.size(); it1++) {
    for (size_t it2 = it1 + 1; it2 < std::min(cams.size(), it1 + num_overlaps + 1); it2++) {
      image_pairs.push_back(std::make_pair(it1, it2));
    }
  }

  {
    std::cout << "Matching features." << std::endl;
    ff_common::ThreadPool thread_pool;
    std::mutex match_mutex;
    for (size_t pair_it = 0; pair_it < image_pairs.size(); pair_it++) {
      auto pair = image_pairs[pair_it];
      int left_image_it = pair.first, right_image_it = pair.second;
      thread_pool.AddTask
        (&dense_map::matchFeaturesWithCams,   // multi-threaded  // NOLINT
         // dense_map::matchFeaturesWithCams( // single-threaded // NOLINT
         &match_mutex, left_image_it, right_image_it, cam_params[cams[left_image_it].camera_type],
         cam_params[cams[right_image_it].camera_type], world_to_cam[left_image_it],
         world_to_cam[right_image_it], initial_max_reprojection_error,
         cid_to_descriptor_map[left_image_it], cid_to_descriptor_map[right_image_it],
         cid_to_keypoint_map[left_image_it], cid_to_keypoint_map[right_image_it], verbose,
         &matches[pair]);
    }
    thread_pool.Join();
  }
  cid_to_descriptor_map = std::vector<cv::Mat>();  // Wipe, takes memory

  // Give all interest points in a given image a unique id, and put
  // them in a vector with the id corresponding to the interest point
  std::vector<std::map<std::pair<float, float>, int>> keypoint_map(cams.size());
  for (auto it = matches.begin(); it != matches.end(); it++) {
    std::pair<int, int> const& index_pair = it->first;     // alias

    int left_index = index_pair.first;
    int right_index = index_pair.second;

    dense_map::MATCH_PAIR const& match_pair = it->second;  // alias
    std::vector<dense_map::InterestPoint> const& left_ip_vec = match_pair.first;
    std::vector<dense_map::InterestPoint> const& right_ip_vec = match_pair.second;
    for (size_t ip_it = 0; ip_it < left_ip_vec.size(); ip_it++) {
      auto dist_left_ip  = std::make_pair(left_ip_vec[ip_it].x,  left_ip_vec[ip_it].y);
      auto dist_right_ip = std::make_pair(right_ip_vec[ip_it].x, right_ip_vec[ip_it].y);
      // Initialize to zero for the moment
      keypoint_map[left_index][dist_left_ip] = 0;
      keypoint_map[right_index][dist_right_ip] = 0;
    }
  }
  keypoint_vec.resize(cams.size());
  for (size_t cid = 0; cid < cams.size(); cid++) {
    keypoint_vec[cid].resize(keypoint_map[cid].size());
    int fid = 0;
    for (auto ip_it = keypoint_map[cid].begin(); ip_it != keypoint_map[cid].end();
         ip_it++) {
      auto& dist_ip = ip_it->first;  // alias
      keypoint_map[cid][dist_ip] = fid;
      keypoint_vec[cid][fid] = dist_ip;
      fid++;
    }
  }

  // If feature A in image I matches feather B in image J, which
  // matches feature C in image K, then (A, B, C) belong together in
  // a track, and will have a single triangulated xyz. Build such a track.

  openMVG::matching::PairWiseMatches match_map;
  for (auto it = matches.begin(); it != matches.end(); it++) {
    std::pair<int, int> const& index_pair = it->first;     // alias

    int left_index = index_pair.first;
    int right_index = index_pair.second;

    dense_map::MATCH_PAIR const& match_pair = it->second;  // alias
    std::vector<dense_map::InterestPoint> const& left_ip_vec = match_pair.first;
    std::vector<dense_map::InterestPoint> const& right_ip_vec = match_pair.second;

    std::vector<openMVG::matching::IndMatch> mvg_matches;

    for (size_t ip_it = 0; ip_it < left_ip_vec.size(); ip_it++) {
      auto dist_left_ip  = std::make_pair(left_ip_vec[ip_it].x,  left_ip_vec[ip_it].y);
      auto dist_right_ip = std::make_pair(right_ip_vec[ip_it].x, right_ip_vec[ip_it].y);

      int left_id = keypoint_map[left_index][dist_left_ip];
      int right_id = keypoint_map[right_index][dist_right_ip];
      mvg_matches.push_back(openMVG::matching::IndMatch(left_id, right_id));
    }
    match_map[index_pair] = mvg_matches;
  }

  if (verbose) {
    for (auto it = matches.begin(); it != matches.end(); it++) {
      std::pair<int, int> index_pair = it->first;
      dense_map::MATCH_PAIR const& match_pair = it->second;

      int left_index = index_pair.first;
      int right_index = index_pair.second;

      std::string left_image = image_files[left_index];
      std::string right_image = image_files[right_index];

      std::string left_stem = boost::filesystem::path(left_image).stem().string();
      std::string right_stem = boost::filesystem::path(right_image).stem().string();

      std::string match_file = left_stem + "__" + right_stem + ".match";

      std::cout << "Writing: " << left_image << ' ' << right_image << ' '
                << match_file << std::endl;
      dense_map::writeMatchFile(match_file, match_pair.first, match_pair.second);
    }
  }

  // De-allocate data not needed anymore and take up a lot of RAM
  matches.clear(); matches = MATCH_MAP();
  keypoint_map.clear(); keypoint_map.shrink_to_fit();
  cid_to_keypoint_map.clear(); cid_to_keypoint_map.shrink_to_fit();

  {
    // Build tracks
    // De-allocate these as soon as not needed to save memory
    openMVG::tracks::TracksBuilder trackBuilder;
    trackBuilder.Build(match_map);  // Build:  Efficient fusion of correspondences
    trackBuilder.Filter();          // Filter: Remove tracks that have conflict
    // trackBuilder.ExportToStream(std::cout);
    // Export tracks as a map (each entry is a sequence of imageId and featureIndex):
    //  {TrackIndex => {(imageIndex, featureIndex), ... ,(imageIndex, featureIndex)}
    openMVG::tracks::STLMAPTracks map_tracks;
    trackBuilder.ExportToSTL(map_tracks);
    match_map = openMVG::matching::PairWiseMatches();  // wipe this, no longer needed
    trackBuilder = openMVG::tracks::TracksBuilder();   // wipe it

    if (map_tracks.empty())
      LOG(FATAL) << "No tracks left after filtering. Perhaps images are too dis-similar?\n";

    // Populate the filtered tracks
    size_t num_elems = map_tracks.size();
    pid_to_cid_fid.resize(num_elems);
    size_t curr_id = 0;
    for (auto itr = map_tracks.begin(); itr != map_tracks.end(); itr++) {
      for (auto itr2 = (itr->second).begin(); itr2 != (itr->second).end(); itr2++) {
        pid_to_cid_fid[curr_id][itr2->first] = itr2->second;
      }
      curr_id++;
    }
  }

  return;
}

}  // end namespace dense_map


#include "basic.h"

#if defined(__GNUC__)
#pragma GCC diagnostic push
#if __GNUC_PREREQ(9, 0)
#pragma GCC diagnostic ignored "-Wdeprecated-copy"
#endif
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>
#include <boost/log/trivial.hpp>

#if defined(__GNUC__)
#pragma GCC diagnostic pop
#endif

#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable : 4554)
#pragma warning(disable : 26450)
#pragma warning(disable : 26451)
#pragma warning(disable : 26454)
#pragma warning(disable : 26495)
#pragma warning(disable : 26812)
#elif defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif

#include <unsupported/Eigen/CXX11/Tensor>

#if defined(_MSC_VER)
#pragma warning(pop)
#elif defined(__GNUC__)
#pragma GCC diagnostic pop
#endif

#include <OpenSimplexNoise.h>

namespace ba = boost::accumulators;

namespace WorldEngine
{

void AntiAlias(boost::multi_array<float, 2>& mapData, size_t steps)
{
   typedef Eigen::Tensor<float, 2>    Tensor2D;
   typedef Eigen::TensorMap<Tensor2D> Tensor2DMap;

   static const float w = 1.0f / 11.0f;

   Tensor2D kernel(3, 3);
   kernel.setValues({{w, w, w}, {w, w, w}, {w, w, w}});

   const uint32_t width  = static_cast<uint32_t>(mapData.shape()[1]);
   const uint32_t height = static_cast<uint32_t>(mapData.shape()[0]);

   Tensor2DMap map(mapData.data(), width, height);

   Tensor2D mapPart = map * (2.0f / 11.0f);

   auto AntiAliasStep = [&width,
                         &height,
                         &mapPart = std::as_const(mapPart),
                         &kernel  = std::as_const(kernel)](Tensor2DMap& map) {
      // Specify first and second dimension for convolution
      static const Eigen::array<int, 2> dimensions({0, 1});

      // Create a circular boundary
      const Eigen::array<std::pair<int, int>, 2> paddings(
         {std::make_pair(1, 1), std::make_pair(1, 1)});
      Tensor2D paddedMap = map.pad(paddings);

      for (uint32_t x = 0; x <= width + 1; x++)
      {
         paddedMap(x, 0)          = paddedMap(x, height);
         paddedMap(x, height + 1) = paddedMap(x, 1);
      }
      for (uint32_t y = 0; y <= height + 1; y++)
      {
         paddedMap(0, y)         = paddedMap(width, y);
         paddedMap(width + 1, y) = paddedMap(1, y);
      }

      map = paddedMap.convolve(kernel, dimensions);

      map += mapPart;
   };

   for (size_t i = 0; i < steps; i++)
   {
      AntiAliasStep(map);
   }
}

template<typename T>
boost::multi_array<uint32_t, 2>
CountNeighbors(const boost::multi_array<T, 2>& mask, int32_t radius)
{
   const int32_t width  = static_cast<int32_t>(mask.shape()[1]);
   const int32_t height = static_cast<int32_t>(mask.shape()[0]);

   boost::multi_array<uint32_t, 2> neighbors(boost::extents[height][width]);

   std::fill(neighbors.data(), neighbors.data() + neighbors.num_elements(), 0u);

   for (int32_t y = 0; y < height; y++)
   {
      for (int32_t x = 0; x < width; x++)
      {
         for (int32_t ny = y - radius; ny <= y + radius; ny++)
         {
            if (0 <= ny && ny < height)
            {
               for (int32_t nx = x - radius; nx <= x + radius; nx++)
               {
                  if ((nx != x || ny != y) && 0 <= nx && nx < width)
                  {
                     if (mask[ny][nx])
                     {
                        neighbors[y][x]++;
                     }
                  }
               }
            }
         }
      }
   }

   return neighbors;
}
template boost::multi_array<uint32_t, 2>
CountNeighbors<bool>(const boost::multi_array<bool, 2>& mask, int32_t radius);
template boost::multi_array<uint32_t, 2>
CountNeighbors<float>(const boost::multi_array<float, 2>& mask, int32_t radius);

float FindThresholdF(const boost::multi_array<float, 2>& mapData,
                     float                               landPercentage,
                     const OceanArrayType*               ocean)
{
   typedef ba::accumulator_set<float, ba::stats<ba::tag::p_square_quantile>>
      accumulator_t;

   const uint32_t width    = static_cast<uint32_t>(mapData.shape()[1]);
   const uint32_t height   = static_cast<uint32_t>(mapData.shape()[0]);
   float          quantile = 1.0f - landPercentage;

   accumulator_t accumulator(ba::quantile_probability = quantile);

   if (ocean == nullptr || ocean->size() != mapData.size())
   {
      BOOST_LOG_TRIVIAL(trace) << "Calculating threshold (" << landPercentage
                               << ") without ocean data...";

      for (uint32_t y = 0; y < height; y++)
      {
         for (uint32_t x = 0; x < width; x++)
         {
            accumulator(mapData[y][x]);
         }
      }
   }
   else
   {
      BOOST_LOG_TRIVIAL(trace) << "Calculating threshold (" << landPercentage
                               << ") with ocean data...";

      for (uint32_t y = 0; y < height; y++)
      {
         for (uint32_t x = 0; x < width; x++)
         {
            if (!(*ocean)[y][x])
            {
               accumulator(mapData[y][x]);
            }
         }
      }
   }

   float threshold = ba::p_square_quantile(accumulator);

   BOOST_LOG_TRIVIAL(trace) << "Threshold: " << threshold;

   return threshold;
}

double Noise(const OpenSimplexNoise::Noise& noise,
             double                         x,
             double                         y,
             uint32_t                       octaves)
{
   static const double persistence = 0.5;
   static const double lacunarity  = 2.0;

   double freq  = 1.0;
   double amp   = 1.0;
   double max   = 1.0;
   double total = noise.eval(x, y);

   for (uint32_t i = 1; i < octaves; i++)
   {
      freq *= lacunarity;
      amp *= persistence;
      max += amp;
      total += noise.eval(x * freq, y * freq) * amp;
   }

   return total / max;
}

} // namespace WorldEngine


/*
 *  Copyright 2011-2014 Maxim Milakov
 *
 *  Licensed under the Apache License, Version 2.0 (the "License");
 *  you may not use this file except in compliance with the License.
 *  You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 *  Unless required by applicable law or agreed to in writing, software
 *  distributed under the License is distributed on an "AS IS" BASIS,
 *  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *  See the License for the specific language governing permissions and
 *  limitations under the License.
 */

#include "distort_2d_data_sampler_transformer.h"

#include "neural_network_exception.h"
#include "data_transformer_util.h"

#include <opencv2/core/core.hpp>
#include <boost/format.hpp>
#include <cstring>

namespace nnforge
{
	distort_2d_data_sampler_transformer::distort_2d_data_sampler_transformer(const std::vector<distort_2d_data_sampler_param>& params)
		: params(params)
	{
	}

	distort_2d_data_sampler_transformer::distort_2d_data_sampler_transformer(
		const std::vector<float>& rotation_angle_in_degrees_list,
		const std::vector<float>& scale_list,
		const std::vector<float>& shift_right_x_list,
		const std::vector<float>& shift_down_y_list)
	{
		for(std::vector<float>::const_iterator it1 = rotation_angle_in_degrees_list.begin(); it1 != rotation_angle_in_degrees_list.end(); ++it1)
			for(std::vector<float>::const_iterator it2 = scale_list.begin(); it2 != scale_list.end(); ++it2)
				for(std::vector<float>::const_iterator it3 = shift_right_x_list.begin(); it3 != shift_right_x_list.end(); ++it3)
					for(std::vector<float>::const_iterator it4 = shift_down_y_list.begin(); it4 != shift_down_y_list.end(); ++it4)
					{
						distort_2d_data_sampler_param new_item;
						new_item.rotation_angle_in_degrees = *it1;
						new_item.scale = *it2;
						new_item.shift_right_x = *it3;
						new_item.shift_down_y = *it4;
						params.push_back(new_item);
					}
	}

	distort_2d_data_sampler_transformer::~distort_2d_data_sampler_transformer()
	{
	}

	void distort_2d_data_sampler_transformer::transform(
		const void * data,
		void * data_transformed,
		neuron_data_type::input_type type,
		const layer_configuration_specific& original_config,
		unsigned int sample_id)
	{
		if (type != neuron_data_type::type_byte)
			throw neural_network_exception("distort_2d_data_sampler_transformer is implemented for data stored as bytes only");

		if (original_config.dimension_sizes.size() != 2)
			throw neural_network_exception((boost::format("distort_2d_data_sampler_transformer is processing 2d data only, data is passed with number of dimensions %1%") % original_config.dimension_sizes.size()).str());

		float rotation_angle = params[sample_id].rotation_angle_in_degrees;
		float scale = params[sample_id].scale;
		float shift_x = params[sample_id].shift_right_x;
		float shift_y = params[sample_id].shift_down_y;

		unsigned int neuron_count_per_feature_map = original_config.get_neuron_count_per_feature_map();
		for(unsigned int feature_map_id = 0; feature_map_id < original_config.feature_map_count; ++feature_map_id)
		{
			cv::Mat1b src_image(static_cast<int>(original_config.dimension_sizes[1]), static_cast<int>(original_config.dimension_sizes[0]), const_cast<unsigned char *>(static_cast<const unsigned char *>(data)) + (neuron_count_per_feature_map * feature_map_id));
			cv::Mat1b image(static_cast<int>(original_config.dimension_sizes[1]), static_cast<int>(original_config.dimension_sizes[0]), static_cast<unsigned char *>(data_transformed) + (neuron_count_per_feature_map * feature_map_id));
			memcpy(
				((unsigned char *)data_transformed) + neuron_count_per_feature_map * feature_map_id,
				((unsigned char *)data) + neuron_count_per_feature_map * feature_map_id,
				neuron_count_per_feature_map * neuron_data_type::get_input_size(type));

			data_transformer_util::rotate_scale_shift(
				image,
				cv::Point2f(static_cast<float>(image.cols) * 0.5F, static_cast<float>(image.rows) * 0.5F),
				rotation_angle,
				scale,
				shift_x,
				shift_y);
		}
	}

	bool distort_2d_data_sampler_transformer::is_in_place() const
	{
		return false;
	}

	unsigned int distort_2d_data_sampler_transformer::get_sample_count() const
	{
		return static_cast<unsigned int>(params.size());
	}

	bool distort_2d_data_sampler_transformer::is_deterministic() const
	{
		return true;
	}
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_PREDICATES_FUNCTIONS_IS_NOT_LESS_HPP_INCLUDED
#define BOOST_SIMD_PREDICATES_FUNCTIONS_IS_NOT_LESS_HPP_INCLUDED
#include <boost/simd/include/functor.hpp>
#include <boost/dispatch/include/functor.hpp>

namespace boost { namespace simd { namespace tag
  {
   /*!
     @brief is_not_less generic tag

     Represents the is_not_less function in generic contexts.

     @par Models:
        Hierarchy
   **/
    struct is_not_less_ : ext::elementwise_<is_not_less_>
    {
      /// @brief Parent hierarchy
      typedef ext::elementwise_<is_not_less_> parent;
      template<class... Args>
      static BOOST_FORCEINLINE BOOST_AUTO_DECLTYPE dispatch(Args&&... args)
      BOOST_AUTO_DECLTYPE_BODY( dispatching_is_not_less_( ext::adl_helper(), static_cast<Args&&>(args)... ) )
    };}
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::is_not_less_, Site> dispatching_is_not_less_(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::is_not_less_, Site>();
   }
   template<class... Args>
   struct impl_is_not_less_;
  }
  /*!
    TODO Put description here

    @par Semantic:

    @code
    logical<T> r = is_not_less(a0,a1);
    @endcode

    is similar to:

    @code
    logical<T> r = !(a0 < a1);
    @endcode

    @par Note:

    Due to existence of nan, this is not equivalent to @c is_greater_equal(a0, a1)
    for floating types

    @par Alias:

    @c is_nlt

    @param a0

    @param a1

    @return a logical value
  **/
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::is_not_less_, is_not_less, 2)
  BOOST_DISPATCH_FUNCTION_IMPLEMENTATION(tag::is_not_less_, is_nlt, 2)
} }

#endif


//            Copyright Daniel Trebbien 2010.
// Distributed under the Boost Software License, Version 1.0.
//   (See accompanying file LICENSE_1_0.txt or the copy at
//         http://www.boost.org/LICENSE_1_0.txt)

#ifndef BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP
#define BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP 1

#include <cassert>
#include <set>
#include <vector>
#include <boost/concept_check.hpp>
#include <boost/concept/assert.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/buffer_concepts.hpp>
#include <boost/graph/exception.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/named_function_params.hpp>
#include <boost/graph/detail/d_ary_heap.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/typeof/typeof.hpp>

namespace boost {

  namespace detail {

    /**
     * \brief Performs a phase of the Stoer-Wagner min-cut algorithm
     *
     * Performs a phase of the Stoer-Wagner min-cut algorithm.
     *
     * As described by Stoer & Wagner (1997), a phase is simply a maximum adjacency search
     * (also called a maximum cardinality search), which results in the selection of two vertices
     * \em s and \em t, and, as a side product, a minimum s-t cut of
     * the input graph. Here, the input graph is basically \p g, but some vertices are virtually
     * assigned to others as a way of viewing \p g as a graph with some sets of
     * vertices merged together.
     *
     * This implementation is a translation of pseudocode by Professor Uri Zwick,
     * School of Computer Science, Tel Aviv University.
     *
     * \pre \p g is a connected, undirected graph
     * \param[in] g the input graph
     * \param[in] assignments a read/write property map from each vertex to the vertex that it is assigned to
     * \param[in] assignedVertices a list of vertices that are assigned to others
     * \param[in] weights a readable property map from each edge to its weight (a non-negative value)
     * \param[out] pq a keyed, updatable max-priority queue
     * \returns a tuple (\em s, \em t, \em w) of the "s" and "t"
     *     of the minimum s-t cut and the cut weight \em w
     *     of the minimum s-t cut.
     * \see http://www.cs.tau.ac.il/~zwick/grad-algo-08/gmc.pdf
     *
     * \author Daniel Trebbien
     * \date 2010-09-11
     */
    template <class UndirectedGraph, class VertexAssignmentMap, class WeightMap, class KeyedUpdatablePriorityQueue>
    boost::tuple<typename boost::graph_traits<UndirectedGraph>::vertex_descriptor, typename boost::graph_traits<UndirectedGraph>::vertex_descriptor, typename boost::property_traits<WeightMap>::value_type>
    stoer_wagner_phase(const UndirectedGraph& g, VertexAssignmentMap assignments, const std::set<typename boost::graph_traits<UndirectedGraph>::vertex_descriptor>& assignedVertices, WeightMap weights, KeyedUpdatablePriorityQueue& pq) {
      typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;

      assert(pq.empty());
      typename KeyedUpdatablePriorityQueue::key_map keys = pq.keys();

      BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
        if (v == get(assignments, v)) { // foreach u \in V do
          put(keys, v, weight_type(0));

          pq.push(v);
        }
      }

      assert(pq.size() >= 2);

      vertex_descriptor s, t;
      weight_type w;
      while (!pq.empty()) { // while PQ \neq {} do
        const vertex_descriptor u = pq.top(); // u = extractmax(PQ)
        w = get(keys, u);
        pq.pop();

        s = t; t = u;

        BGL_FORALL_OUTEDGES_T(u, e, g, UndirectedGraph) { // foreach (u, v) \in E do
          const vertex_descriptor v = get(assignments, target(e, g));

          if (pq.contains(v)) { // if v \in PQ then
            put(keys, v, get(keys, v) + get(weights, e)); // increasekey(PQ, v, wA(v) + w(u, v))
            pq.update(v);
          }
        }

        typename std::set<vertex_descriptor>::const_iterator assignedVertexIt, assignedVertexEnd = assignedVertices.end();
        for (assignedVertexIt = assignedVertices.begin(); assignedVertexIt != assignedVertexEnd; ++assignedVertexIt) {
          const vertex_descriptor uPrime = *assignedVertexIt;

          if (get(assignments, uPrime) == u) {
            BGL_FORALL_OUTEDGES_T(uPrime, e, g, UndirectedGraph) { // foreach (u, v) \in E do
              const vertex_descriptor v = get(assignments, target(e, g));

              if (pq.contains(v)) { // if v \in PQ then
                put(keys, v, get(keys, v) + get(weights, e)); // increasekey(PQ, v, wA(v) + w(u, v))
                pq.update(v);
              }
            }
          }
        }
      }

      return boost::make_tuple(s, t, w);
    }

    /**
     * \brief Computes a min-cut of the input graph
     *
     * Computes a min-cut of the input graph using the Stoer-Wagner algorithm.
     *
     * \pre \p g is a connected, undirected graph
     * \pre pq.empty()
     * \param[in] g the input graph
     * \param[in] weights a readable property map from each edge to its weight (a non-negative value)
     * \param[out] parities a writable property map from each vertex to a bool type object for
     *     distinguishing the two vertex sets of the min-cut
     * \param[out] assignments a read/write property map from each vertex to a \c vertex_descriptor object. This
     *     map serves as work space, and no particular meaning should be derived from property values
     *     after completion of the algorithm.
     * \param[out] pq a keyed, updatable max-priority queue
     * \returns the cut weight of the min-cut
     * \see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.114.6687&rep=rep1&type=pdf
     * \see http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.31.614&rep=rep1&type=pdf
     *
     * \author Daniel Trebbien
     * \date 2010-09-11
     */
    template <class UndirectedGraph, class WeightMap, class ParityMap, class VertexAssignmentMap, class KeyedUpdatablePriorityQueue>
    typename boost::property_traits<WeightMap>::value_type
    stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, ParityMap parities, VertexAssignmentMap assignments, KeyedUpdatablePriorityQueue& pq) {
      BOOST_CONCEPT_ASSERT((boost::IncidenceGraphConcept<UndirectedGraph>));
      BOOST_CONCEPT_ASSERT((boost::VertexListGraphConcept<UndirectedGraph>));
      typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
      typedef typename boost::graph_traits<UndirectedGraph>::vertices_size_type vertices_size_type;
      typedef typename boost::graph_traits<UndirectedGraph>::edge_descriptor edge_descriptor;
      BOOST_CONCEPT_ASSERT((boost::Convertible<typename boost::graph_traits<UndirectedGraph>::directed_category, boost::undirected_tag>));
      BOOST_CONCEPT_ASSERT((boost::ReadablePropertyMapConcept<WeightMap, edge_descriptor>));
      typedef typename boost::property_traits<WeightMap>::value_type weight_type;
      BOOST_CONCEPT_ASSERT((boost::WritablePropertyMapConcept<ParityMap, vertex_descriptor>));
      typedef typename boost::property_traits<ParityMap>::value_type parity_type;
      BOOST_CONCEPT_ASSERT((boost::ReadWritePropertyMapConcept<VertexAssignmentMap, vertex_descriptor>));
      BOOST_CONCEPT_ASSERT((boost::Convertible<vertex_descriptor, typename boost::property_traits<VertexAssignmentMap>::value_type>));
      BOOST_CONCEPT_ASSERT((boost::KeyedUpdatableQueueConcept<KeyedUpdatablePriorityQueue>));

      vertices_size_type n = num_vertices(g);
      if (n < 2)
        throw boost::bad_graph("the input graph must have at least two vertices.");
      else if (!pq.empty())
        throw std::invalid_argument("the max-priority queue must be empty initially.");

      std::set<vertex_descriptor> assignedVertices;

      // initialize `assignments` (all vertices are initially assigned to themselves)
      BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
        put(assignments, v, v);
      }

      vertex_descriptor s, t;
      weight_type bestW;

      boost::tie(s, t, bestW) = boost::detail::stoer_wagner_phase(g, assignments, assignedVertices, weights, pq);
      assert(s != t);
      BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
        put(parities, v, parity_type(v == t ? 1 : 0));
      }
      put(assignments, t, s);
      assignedVertices.insert(t);
      --n;

      for (; n >= 2; --n) {
        weight_type w;
        boost::tie(s, t, w) = boost::detail::stoer_wagner_phase(g, assignments, assignedVertices, weights, pq);
        assert(s != t);

        if (w < bestW) {
          BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
            put(parities, v, parity_type(get(assignments, v) == t ? 1 : 0));

            if (get(assignments, v) == t) // all vertices that were assigned to t are now assigned to s
              put(assignments, v, s);
          }

          bestW = w;
        } else {
          BGL_FORALL_VERTICES_T(v, g, UndirectedGraph) {
            if (get(assignments, v) == t) // all vertices that were assigned to t are now assigned to s
              put(assignments, v, s);
          }
        }
        put(assignments, t, s);
        assignedVertices.insert(t);
      }

      assert(pq.empty());

      return bestW;
    }

  } // end `namespace detail` within `namespace boost`

  template <class UndirectedGraph, class WeightMap, class P, class T, class R>
  inline typename boost::property_traits<WeightMap>::value_type
  stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights, const boost::bgl_named_params
& params) {
    typedef typename boost::graph_traits<UndirectedGraph>::vertex_descriptor vertex_descriptor;
    typedef typename std::vector<vertex_descriptor>::size_type heap_container_size_type;
    typedef typename boost::property_traits<WeightMap>::value_type weight_type;

    typedef boost::bgl_named_params
 params_type;
    BOOST_GRAPH_DECLARE_CONVERTED_PARAMETERS(params_type, params)

    BOOST_AUTO(pq, (boost::detail::make_priority_queue_from_arg_pack_gen<boost::graph::keywords::tag::max_priority_queue, weight_type, vertex_descriptor, std::greater<weight_type> >(choose_param(get_param(params, boost::distance_zero_t()), weight_type(0)))(g, arg_pack)));

    return boost::detail::stoer_wagner_min_cut(g,
        weights,
        choose_param(get_param(params, boost::parity_map_t()), boost::dummy_property_map()),
        boost::detail::make_property_map_from_arg_pack_gen<boost::graph::keywords::tag::vertex_assignment_map, vertex_descriptor>(vertex_descriptor())(g, arg_pack),
        pq
      );
  }

  template <class UndirectedGraph, class WeightMap>
  inline typename boost::property_traits<WeightMap>::value_type
  stoer_wagner_min_cut(const UndirectedGraph& g, WeightMap weights) {
    return boost::stoer_wagner_min_cut(g, weights, boost::vertex_index_map(get(boost::vertex_index, g)));
  }

} // end `namespace boost`

#include <boost/graph/iteration_macros_undef.hpp>

#endif // !BOOST_GRAPH_STOER_WAGNER_MIN_CUT_HPP


/*===================================================================

MSI applications for interactive analysis in MITK (M2aia)

Copyright (c) Jonas Cordes.

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or https://www.github.com/jtfcordes/m2aia for details.

===================================================================*/

#include <Poco/SHA1Engine.h>
#include <algorithm>
#include <boost/algorithm/hex.hpp>
#include <boost/progress.hpp>
#include <boost/uuid/uuid_generators.hpp>
#include <boost/uuid/uuid_io.hpp>
#include <future>
#include <itksys/SystemTools.hxx>
#include <m2ImzMLEngine.h>
#include <m2ImzMLImageIO.h>
#include <m2ImzMLTemplate.h>
#include <m2ImzMLXMLParser.h>
#include <m2Timer.h>
#include <mitkIOMimeTypes.h>
#include <mitkIOUtil.h>
#include <mitkImagePixelReadAccessor.h>
#include <mitkImagePixelWriteAccessor.h>
#include <mitkLocaleSwitch.h>
#include <mitkProgressBar.h>
#include <numeric>
#include <signal/m2PeakDetection.h>
#include <signal/m2Pooling.h>
#include <thread>

template <class ConversionType, class ItFirst, class ItLast, class OStreamType>
void writeData(ItFirst itFirst, ItLast itLast, OStreamType &os)
{
  std::for_each(itFirst,
                itLast,
                [&os](const auto &v)
                {
                  const ConversionType c(v);
                  os.write((char *)(&c), sizeof(ConversionType));
                });
}

namespace m2
{
  ImzMLImageIO::ImzMLImageIO() : AbstractFileIO(mitk::Image::GetStaticNameOfClass(), IMZML_MIMETYPE(), "imzML Image")
  {
    AbstractFileWriter::SetRanking(10);
    AbstractFileReader::SetRanking(10);
    this->RegisterService();
  }

  mitk::IFileIO::ConfidenceLevel ImzMLImageIO::GetWriterConfidenceLevel() const
  {
    if (AbstractFileIO::GetWriterConfidenceLevel() == Unsupported)
      return Unsupported;
    const auto *input = static_cast<const m2::ImzMLSpectrumImage *>(this->GetInput());
    if (input)
      return Supported;
    else
      return Unsupported;
  }

  std::string ImzMLImageIO::GetIBDOutputPath() const
  {
    auto outFile = GetImzMLOutputPath();
    std::string ibdOutPath = itksys::SystemTools::GetParentDirectory(outFile) + "/" +
                             itksys::SystemTools::GetFilenameWithoutExtension(outFile) + ".ibd";
    return itksys::SystemTools::ConvertToOutputPath(ibdOutPath);
  }

  std::string ImzMLImageIO::GetImzMLOutputPath() const
  {
    auto outFile = this->GetOutputLocation();
    return itksys::SystemTools::ConvertToOutputPath(outFile);
  }

  /*if (any(input->GetExportMode() & ImzMLFormatType::ContinuousCentroid))
  {
    std::vector<double> is;
    auto mzIt = std::cbegin(mzs); // mzs contains already masked mzs
    for (const auto &i : maskIndices)
    {
      double mz = *mzIt++;
      if (input->GetMassPickingTolerance() != 0.0)
      {
        auto iLower = std::lower_bound(mzs.begin(), mzs.end(), mz - input->GetMassPickingTolerance() * 10e-6);
        auto iUpper = std::upper_bound(mzs.begin(), mzs.end(), mz + input->GetMassPickingTolerance() * 10e-6);
        auto s = std::next(std::begin(ints), std::distance(mzs.begin(), iLower));
        auto e = std::next(s, std::distance(iLower, iUpper));
        double val = 0;
        switch (input->GetIonImageGrabStrategy())
        {
        case m2::IonImageGrabStrategyType::Sum:
          val = std::accumulate(s, e, double(0));
          break;
        case m2::IonImageGrabStrategyType::Mean:
          val = std::accumulate(s, e, double(0)) / double(std::distance(s, e));
          break;
        case m2::IonImageGrabStrategyType::Maximum:
          val = *std::max_element(s, e);
          break;
        case m2::IonImageGrabStrategyType::Median:
        {
          const unsigned int _N = std::distance(s, e);
          double median = 0;
          if ((_N % 2) == 0)
          {
            std::nth_element(s, s + _N * 0.5, e);
            std::nth_element(s, s + _N * 0.5 + 1, e);
            median = 0.5 * (*std::next(s, _N * 0.5) + *std::next(s, _N * 0.5 + 1));
          }
          else
          {
            std::nth_element(s, s + ((_N + 1) * 0.5), e);
            median = 0.5 * (*std::next(s, _N * 0.5) + *std::next(s, _N * 0.5 + 1));
          }
          val = median;
          break;
        }
        }
        is.push_back(val);
      }
      else
      {
        is.push_back(ints[i + input->GetConstantOffset()]);
      }
    }
    ints = std::move(is);
  }
  */

  void ImzMLImageIO::WriteContinuousProfile(m2::ImzMLSpectrumImage::SourceListType &sourceList) const
  {
    const auto *input = static_cast<const m2::ImzMLSpectrumImage *>(this->GetInput());

    MITK_INFO << "ImzMLImageSource list size " << sourceList.size();
    std::vector<float> mzs;
    std::vector<float> ints;

    // Output file stream for ibd
    std::ofstream b(GetIBDOutputPath(), std::ofstream::binary | std::ofstream::app);

    unsigned sourceId = 0;
    unsigned long long offset = 16;
    unsigned long long offsetDelta = 0;
    // b.seekp(offset);

    // write mzs
    {
      auto &source = sourceList.front();
      // write mass axis
      input->GetSpectrum(0, mzs, ints, sourceId);

      source.m_Spectra[0].mzOffset = 16;
      source.m_Spectra[0].mzLength = mzs.size();

      switch (input->GetXOutputType())
      {
        case m2::NumericType::Float:
          writeData<float>(std::begin(mzs), std::end(mzs), b);
          offsetDelta = source.m_Spectra[0].mzLength * sizeof(float);
          break;
        case m2::NumericType::Double:
          writeData<double>(std::begin(mzs), std::end(mzs), b);
          offsetDelta = source.m_Spectra[0].mzLength * sizeof(double);
          break;
      }
      offset += offsetDelta;
    }

    for (auto &source : sourceList)
    {
      ;
      for (size_t id = 0; id < source.m_Spectra.size(); ++id)
      {
        auto &s = source.m_Spectra[id];
        // update mz axis info
        s.mzLength = sourceList.front().m_Spectra[0].mzLength;
        s.mzOffset = sourceList.front().m_Spectra[0].mzOffset;

        // write ints
        {
          input->GetSpectrum(id, mzs, ints, sourceId);

          s.intOffset = offset;
          s.intLength = ints.size();

          switch (input->GetYOutputType())
          {
            case m2::NumericType::Float:
              writeData<float>(std::begin(ints), std::end(ints), b);
              offsetDelta = s.intLength * sizeof(float);
              break;
            case m2::NumericType::Double:
              writeData<double>(std::begin(ints), std::end(ints), b);
              offsetDelta = s.intLength * sizeof(double);
              break;
          }

          offset += offsetDelta;
        }
        b.flush();
      }
      ++sourceId;
    }
  }
  void ImzMLImageIO::WriteContinuousCentroid(m2::ImzMLSpectrumImage::SourceListType &sourceList) const
  {
    const auto *input = static_cast<const m2::ImzMLSpectrumImage *>(this->GetInput());
    auto massIndicesMask = input->GetPeaks();
    if (massIndicesMask.empty())
      mitkThrow() << "No mass mask indices provided!";

    std::vector<float> mzs, mzsMasked;
    std::vector<float> ints, intsMasked;

    // Output file stream for ibd
    std::ofstream b(GetIBDOutputPath(), std::ofstream::binary | std::ofstream::app);

    unsigned sourceId = 0;
    unsigned long long offset = 16;
    unsigned long long offsetDelta = 0;
    b.seekp(offset);
    // write mzs
    {
      auto &source = sourceList.front();
      // write mass axis
      input->GetSpectrum(0, mzs, ints, sourceId);
      for (auto i : massIndicesMask)
      {
        mzsMasked.push_back(mzs[i.GetIndex()]);
      }
      mzs = mzsMasked;

      // update source spectra meta data to its actual values
      source.m_Spectra[0].mzOffset = 16;
      source.m_Spectra[0].mzLength = mzs.size();

      switch (input->GetXOutputType())
      {
        case m2::NumericType::Float:
          writeData<float>(std::begin(mzs), std::end(mzs), b);
          offsetDelta = source.m_Spectra[0].mzLength * sizeof(float);
          break;
        case m2::NumericType::Double:
          writeData<double>(std::begin(mzs), std::end(mzs), b);
          offsetDelta = source.m_Spectra[0].mzLength * sizeof(double);
          break;
      }
      offset += offsetDelta;
    }

    for (auto &source : sourceList)
    {
      for (size_t id = 0; id < source.m_Spectra.size(); ++id)
      {
        auto &s = source.m_Spectra[id];
        // update mz axis info
        s.mzLength = sourceList.front().m_Spectra[0].mzLength;
        s.mzOffset = sourceList.front().m_Spectra[0].mzOffset;

        // write ints
        {
          input->GetSpectrum(id, mzs, ints, sourceId);

          for (auto p : massIndicesMask)
          {
            auto i = p.GetIndex();
            double val = 0;
            if (input->GetTolerance() == 0)
            {
              val = ints[i];
            }
            else
            {
              auto tol = input->ApplyTolerance(mzs[i]) ;
              auto subRes = m2::Signal::Subrange(mzs, mzs[i] - tol, mzs[i] + tol);
              auto s = std::next(std::begin(ints), subRes.first);
              auto e = std::next(s, subRes.second);

              val = Signal::RangePooling<double>(s, e, input->GetRangePoolingStrategy());
            }
            intsMasked.push_back(val);
          }

          ints = std::move(intsMasked);
          intsMasked.clear();

          s.intOffset = offset;
          s.intLength = ints.size();

          switch (input->GetYOutputType())
          {
            case m2::NumericType::Float:
              writeData<float>(std::begin(ints), std::end(ints), b);
              offsetDelta = s.intLength * sizeof(float);
              break;
            case m2::NumericType::Double:
              writeData<double>(std::begin(ints), std::end(ints), b);
              offsetDelta = s.intLength * sizeof(double);
              break;
          }

          offset += offsetDelta;
        }
        b.flush();
      }
      ++sourceId;
    }
  }

  void ImzMLImageIO::WriteProcessedProfile(m2::ImzMLSpectrumImage::SourceListType & /*sourceList*/) const
  {
    mitkThrow() << "Not implemented";
  }

  void ImzMLImageIO::WriteProcessedCentroid(m2::ImzMLSpectrumImage::SourceListType &sourceList) const
  {
    const auto *input = static_cast<const m2::ImzMLSpectrumImage *>(this->GetInput());

    // Output file stream for ibd
    std::ofstream b(GetIBDOutputPath(), std::ofstream::binary | std::ofstream::app);

    // unsigned sourceId = 0;
    unsigned long long offset = 16;
    unsigned long long offsetDelta = 0;
    b.seekp(offset);
    // write mzs
    {
      using namespace std;
      vector<float> xs, ys, mzs, ints;

      // std::vector<unsigned int> xs_index;
      unsigned int sourceId = 0;
      for (auto &source : sourceList)
      {
        MITK_INFO << "Write binary data ...";
        boost::progress_display show_progress(source.m_Spectra.size());

        for (unsigned int spectrumId = 0; spectrumId < source.m_Spectra.size(); ++spectrumId)
        {
          const auto &peaks = source.m_Spectra[spectrumId].peaks;
          xs.reserve(peaks.size());
          ys.reserve(peaks.size());
          // update source spectra meta data to its actual values
          source.m_Spectra[spectrumId].mzOffset = offset;
          source.m_Spectra[spectrumId].mzLength = peaks.size();
          xs.clear();
          ys.clear();

          input->GetSpectrum(spectrumId, mzs, ints, sourceId);

          for (const auto &p : peaks)
          {
            xs.push_back(p.GetX());
            if (input->GetTolerance() == 0)
            {
              ys.push_back(p.GetY());
            }
            else
            {
              const auto tol = input->ApplyTolerance(mzs[p.GetIndex()]);
              const auto subRes = m2::Signal::Subrange(mzs, mzs[p.GetIndex()] - tol, mzs[p.GetIndex()] + tol);
              const auto s = next(begin(ints), subRes.first);
              const auto e = next(s, subRes.second);

              ys.push_back(Signal::RangePooling<double>(s, e, input->GetRangePoolingStrategy()));
            }
          }

          switch (input->GetXOutputType())
          {
            case m2::NumericType::Float:
              writeData<float>(begin(xs), end(xs), b);
              offsetDelta = xs.size() * sizeof(float);
              break;
            case m2::NumericType::Double:
              writeData<double>(begin(xs), end(xs), b);
              offsetDelta = xs.size() * sizeof(double);
              break;
          }
          offset += offsetDelta;
          source.m_Spectra[spectrumId].intOffset = offset;
          source.m_Spectra[spectrumId].intLength = peaks.size();

          switch (input->GetYOutputType())
          {
            case m2::NumericType::Float:
              writeData<float>(begin(ys), end(ys), b);
              offsetDelta = ys.size() * sizeof(float);
              break;
            case m2::NumericType::Double:
              writeData<double>(begin(ys), end(ys), b);
              offsetDelta = ys.size() * sizeof(double);
              break;
          }
          offset += offsetDelta;
          b.flush();
          ++show_progress;
        }
        ++sourceId;
      }
    } // namespace m2

    MITK_INFO << "File-Size " << offset;
  }

  void ImzMLImageIO::Write()
  {
    ValidateOutputLocation();

    const auto *input = static_cast<const m2::ImzMLSpectrumImage *>(this->GetInput());
    input->SaveModeOn();

    std::string uuidString;
    {
      // Write UUID string to ibd
      std::ofstream b(GetIBDOutputPath(), std::ofstream::binary);
      boost::uuids::basic_random_generator<boost::mt19937> gen;
      boost::uuids::uuid u = gen();
      uuidString = boost::uuids::to_string(u);
      b.seekp(0);
      b.write((char *)(u.data), u.static_size());
      b.flush();
      MITK_INFO << u.data;
    }

    try
    {
      MITK_INFO << "Start writing ImzML!";
      //// define working vectors
      std::vector<double> mzs;
      std::vector<double> ints;

      using m2::SpectrumFormatType;

      // copy the whole source meta data container; spectra information is
      // updated based on the save mode.
      // mz and ints meta data is manipuulated to write a correct imzML xml structure
      // copy of sources is discared after writing
      m2::ImzMLSpectrumImage::SourceListType sourceCopy(input->GetImzMLSpectrumImageSourceList());
      std::string sha1;
      switch (input->GetExportMode())
      {
        case SpectrumFormatType::ContinuousProfile:
          this->WriteContinuousProfile(sourceCopy);
          break;
        case SpectrumFormatType::ProcessedCentroid:
          this->WriteProcessedCentroid(sourceCopy);
          break;
        case SpectrumFormatType::ContinuousCentroid:
          this->WriteContinuousCentroid(sourceCopy);
          break;
        case SpectrumFormatType::ProcessedProfile:
          mitkThrow() << "ProcessedProfile export type is not supported!";
          break;
        default:
          break;
      }

      Poco::SHA1Engine generator;
      std::ifstream bf(GetIBDOutputPath(), std::fstream::binary);
      char *c = new char[2048];
      unsigned long long l = 0;
      while (bf)
      {
        bf.read(c, 2048);
        generator.update(c, bf.gcount());
        l += bf.gcount();
      }

      MITK_INFO << "bytes " << l;

      std::map<std::string, std::string> context;

      switch (input->GetExportMode())
      {
        case SpectrumFormatType::None:
          mitkThrow() << "SpectrumFormatType::None type is not supported!";
          break;
        case SpectrumFormatType::ContinuousCentroid:
          context["spectrumtype"] = "centroid spectrum";
          context["mode"] = "continuous";
          break;
        case SpectrumFormatType::ContinuousProfile:
          context["spectrumtype"] = "profile spectrum";
          context["mode"] = "continuous";
          break;
        case SpectrumFormatType::ProcessedCentroid:
          context["spectrumtype"] = "centroid spectrum";
          context["mode"] = "processed";
          break;
        case SpectrumFormatType::ProcessedProfile:
          context["spectrumtype"] = "profile spectrum";
          context["mode"] = "processed";
          break;
      }
      std::string sha1string = Poco::SHA1Engine::digestToHex(generator.digest());
      MITK_INFO << "[ibd SHA1] " << sha1string << "\n";
      MITK_INFO << "[uuid] " << uuidString << "\n";
      // context["mode"] = "continuous";
      context["uuid"] = uuidString;
      context["sha1sum"] = sha1string;
      unsigned mzBytes = 0;
      unsigned intBytes = 0;
      switch (input->GetXOutputType())
      {
        case m2::NumericType::Double:
          context["mz_data_type"] = "64-bit float";
          mzBytes = 8;
          break;
        case m2::NumericType::Float:
          context["mz_data_type"] = "32-bit float";
          mzBytes = 4;
          break;
      }

      switch (input->GetYOutputType())
      {
        case m2::NumericType::Double:
          context["int_data_type"] = "64-bit float";
          intBytes = 8;
          break;
        case m2::NumericType::Float:
          context["int_data_type"] = "32-bit float";
          intBytes = 4;
          break;
      }

      context["mz_data_type_code"] = m2::Template::TextToCodeMap[context["mz_data_type"]];
      context["int_data_type_code"] = m2::Template::TextToCodeMap[context["int_data_type"]];
      context["mz_compression"] = "no compression";
      context["int_compression"] = "no compression";

      context["mode_code"] = m2::Template::TextToCodeMap[context["mode"]];
      context["spectrumtype_code"] = m2::Template::TextToCodeMap[context["spectrumtype"]];

      context["size_x"] = std::to_string(input->GetDimensions()[0]);
      context["size_y"] = std::to_string(input->GetDimensions()[1]);
      context["size_z"] = std::to_string(input->GetDimensions()[2]);

      context["max dimension x"] =
        std::to_string(unsigned(input->GetDimensions()[0] * input->GetGeometry()->GetSpacing()[0] * 1e3));
      context["max dimension y"] =
        std::to_string(unsigned(input->GetDimensions()[1] * input->GetGeometry()->GetSpacing()[1] * 1e3));
      context["max dimension z"] =
        std::to_string(unsigned(input->GetDimensions()[2] * input->GetGeometry()->GetSpacing()[2] * 1e3));

      context["pixel size x"] = std::to_string(unsigned(input->GetGeometry()->GetSpacing()[0] * 1e3));
      context["pixel size y"] = std::to_string(unsigned(input->GetGeometry()->GetSpacing()[1] * 1e3));
      context["pixel size z"] = std::to_string(unsigned(input->GetGeometry()->GetSpacing()[2] * 1e3));

      context["origin x"] = std::to_string(input->GetGeometry()->GetOrigin()[0] * 1e3);
      context["origin y"] = std::to_string(input->GetGeometry()->GetOrigin()[1] * 1e3);
      context["origin z"] = std::to_string(input->GetGeometry()->GetOrigin()[2] * 1e3);

      context["run_id"] = std::to_string(0);
      const auto &sources = input->GetImzMLSpectrumImageSourceList();
      auto N = std::accumulate(
        std::begin(sources), std::end(sources), unsigned(0), [](auto s, auto v) { return s + v.m_Spectra.size(); });
      context["num_spectra"] = std::to_string(N);

      context["int_compression_code"] = m2::Template::TextToCodeMap[context["int_compression"]];
      context["mz_compression_code"] = m2::Template::TextToCodeMap[context["mz_compression"]];

      // Output file stream for imzML
      std::ofstream f(GetImzMLOutputPath(), std::ofstream::binary);

      std::string view = m2::Template::IMZML_TEMPLATE_START;
      f << m2::TemplateEngine::render(view, context);

      view = m2::Template::IMZML_SPECTRUM_TEMPLATE;
      unsigned long id = 0;
      for (const auto &source : sourceCopy)
      {
        MITK_INFO << "Write imzML data ...";
        boost::progress_display show_progress(source.m_Spectra.size());
        for (auto &s : source.m_Spectra)
        {
          auto x = s.index[0] + source.m_Offset[0] + 1; // start by 1
          auto y = s.index[1] + source.m_Offset[1] + 1; // start by 1
          auto z = s.index[2] + source.m_Offset[2] + 1; // start by 1

          context = {{"index", std::to_string(id++)},
                     {"x", std::to_string(x)},
                     {"y", std::to_string(y)},
                     {"z", std::to_string(z)},
                     {"mz_len", std::to_string(s.mzLength)},
                     {"mz_enc_len", std::to_string(s.mzLength * mzBytes)},
                     {"mz_offset", std::to_string(s.mzOffset)},
                     {"int_len", std::to_string(s.intLength)},
                     {"int_enc_len", std::to_string(s.intLength * intBytes)},
                     {"int_offset", std::to_string(s.intOffset)}};

          auto nonConst_input = const_cast<m2::ImzMLSpectrumImage *>(input);
          mitk::ImagePixelReadAccessor<m2::NormImagePixelType> nacc(nonConst_input->GetNormalizationImage());
          if (nacc.GetPixelByIndex(s.index + source.m_Offset) != 1)
          {
            context["tic"] = std::to_string(nacc.GetPixelByIndex(s.index + source.m_Offset));
          }
          f << m2::TemplateEngine::render(view, context);
          f.flush();
          ++show_progress;
        }
      }

      f << m2::Template::IMZML_TEMPLATE_END;
      f.close();
    }
    catch (std::exception &e)
    {
      input->SaveModeOff();
      MITK_ERROR << "Writing ImzML Faild! " + std::string(e.what());
    }

    input->SaveModeOff();
  } // namespace mitk

  mitk::IFileIO::ConfidenceLevel ImzMLImageIO::GetReaderConfidenceLevel() const
  {
    if (AbstractFileIO::GetReaderConfidenceLevel() == Unsupported)
      return Unsupported;
    return Supported;
  }

  std::vector<mitk::BaseData::Pointer> ImzMLImageIO::DoRead()
  {
    MITK_INFO << "Start reading imzML...";

    std::string mzGroupId, intGroupId;
    auto object = m2::ImzMLSpectrumImage::New();

    auto pathWithoutExtension = this->GetInputLocation();
    itksys::SystemTools::ReplaceString(pathWithoutExtension, ".imzML", "");

    if (!itksys::SystemTools::FileExists(pathWithoutExtension + ".ibd"))
      mitkThrow() << "No such file " << pathWithoutExtension;

    m2::ImzMLSpectrumImage::ImzMLImageSource source;
    source.m_ImzMLDataPath = this->GetInputLocation();
    source.m_BinaryDataPath = pathWithoutExtension + ".ibd";
    object->GetImzMLSpectrumImageSourceList().emplace_back(source);

    object->SetImportMode(SpectrumFormatType::None);
    {
      m2::Timer t("Parsing imzML");
      m2::ImzMLXMLParser::FastReadMetaData(object);
      m2::ImzMLXMLParser::SlowReadMetaData(object);
    }

    {
      m2::Timer t("Initialize placeholder images and spectra");
      object->InitializeGeometry();
    }
    {
      m2::Timer t("Load external data");
      EvaluateSpectrumFormatType(object);
      LoadAssociatedData(object);
    }

    return {object.GetPointer()};
  }

  void ImzMLImageIO::EvaluateSpectrumFormatType(m2::SpectrumImageBase *object)
  {
    if (object->GetProperty("continuous") && object->GetProperty("profile spectrum"))
      object->SetImportMode(m2::SpectrumFormatType::ContinuousProfile);
    else if (object->GetProperty("processed") && object->GetProperty("profile spectrum"))
      object->SetImportMode(m2::SpectrumFormatType::ProcessedProfile);
    else if (object->GetProperty("continuous") && object->GetProperty("centroid spectrum"))
      object->SetImportMode(m2::SpectrumFormatType::ContinuousCentroid);
    else if (object->GetProperty("processed") && object->GetProperty("centroid spectrum"))
      object->SetImportMode(m2::SpectrumFormatType::ProcessedCentroid);

    if (!object->GetProperty("processed") && !object->GetProperty("continuous"))
    {
      MITK_ERROR << "Set the continous (IMS:1000030) or processed (IMS:1000031) property in the ImzML "
                    "fileContent element.";
      MITK_ERROR << "Fallback to continous (MS:1000030)";
      object->SetImportMode(m2::SpectrumFormatType::ContinuousProfile);
    }

    if (!object->GetProperty("centroid spectrum") && !object->GetProperty("profile spectrum"))
    {
      MITK_ERROR << "Set the profile spectrum (MS:1000127) or centroid spectrum (MS:1000128) property in the ImzML "
                    "fileContent element.";
      MITK_ERROR << "Fallback to profile spectrum (MS:1000127)";
      object->SetImportMode(m2::SpectrumFormatType::ContinuousProfile);
    }
  }

  void ImzMLImageIO::LoadAssociatedData(m2::ImzMLSpectrumImage *object)
  {
    auto pathWithoutExtension = this->GetInputLocation();
    itksys::SystemTools::ReplaceString(pathWithoutExtension, ".imzML", "");

    auto source = object->GetImzMLSpectrumImageSource();

    if (itksys::SystemTools::FileExists(pathWithoutExtension + ".nrrd"))
    {
      source.m_MaskDataPath = pathWithoutExtension + ".nrrd";
      auto data = mitk::IOUtil::Load(source.m_MaskDataPath).at(0);
      object->GetImageArtifacts()["mask"] = dynamic_cast<mitk::Image *>(data.GetPointer());
      object->UseExternalMaskOn();
    }

    if (itksys::SystemTools::FileExists(pathWithoutExtension + ".mps"))
    {
      source.m_PointsDataPath = pathWithoutExtension + ".mps";
      auto data = mitk::IOUtil::Load(source.m_PointsDataPath).at(0);
      object->GetImageArtifacts()["references"] = dynamic_cast<mitk::PointSet *>(data.GetPointer());
    }
  }

  ImzMLImageIO *ImzMLImageIO::IOClone() const { return new ImzMLImageIO(*this); }
} // namespace m2