#include "process/ConverterProcess.h"

#include "datastructures/DynamicMortonIndex.h"
#include "io/Cesium3DTilesPersistence.h"
#include "io/PNTSWriter.h"
#include "io/PointsPersistence.h"
#include "io/TileSetWriter.h"
#include "math/AABB.h"
#include "pointcloud/Tileset.h"
#include "tiling/OctreeAlgorithms.h"
#include "util/Transformation.h"
#include "util/stuff.h"
#include <terminal/TerminalUI.h>
#include <terminal/stdout_helper.h>
#include <threading/TaskSystem.h>

#include <array>
#include <boost/format.hpp>
#include <boost/scope_exit.hpp>
#include <experimental/filesystem>
#include <fstream>
#include <iostream>
#include <memory>
#include <optional>
#include <queue>
#include <thread>

#include <rapidjson/document.h>
#include <rapidjson/error/en.h>
#include <rapidjson/filereadstream.h>

namespace rj = rapidjson;

constexpr float spacing_correction_factor = 2;

// function that parses a node name and returns the corresponding Morton index
// for that node
using NodeNameToMortonIndex_t =
  std::function<tl::expected<DynamicMortonIndex, std::string>(const std::string&)>;

struct Properties
{
  AABB root_bounds;
  float root_spacing;
  bool points_have_offset;
  NodeNameToMortonIndex_t morton_index_parser;
};

enum class DeleteSource
{
  No,
  Yes
};

static tl::expected<Properties, std::string>
parse_properties_json(const std::string& properties_json_path)
{
  auto fp = fopen(properties_json_path.c_str(), "r");
  if (!fp) {
    const auto error_msg =
      boost::format("Can't open properties.json file @ \"%1%\"") % properties_json_path;
    return tl::make_unexpected(error_msg.str());
  }

  BOOST_SCOPE_EXIT(&fp) { fclose(fp); }
  BOOST_SCOPE_EXIT_END

  char buf[65536];
  rj::FileReadStream stream{ fp, buf, sizeof(buf) };

  rj::Document document;
  if (document.ParseStream(stream).HasParseError()) {
    const auto parse_error_msg = rapidjson::GetParseError_En(document.GetParseError());
    const auto error_msg =
      boost::format("Can't parse properties.json file [%1%]") % parse_error_msg;
    return tl::make_unexpected(error_msg.str());
  }

  Properties props;

  props.root_spacing =
    static_cast<float>(document["source_properties"]["root_spacing"].GetDouble());

  const auto& bounds_member = document["source_properties"]["bounds"];
  const auto& bounds_min = bounds_member["min"];
  const auto& bounds_max = bounds_member["max"];

  props.root_bounds =
    AABB{ { bounds_min[0].GetDouble(), bounds_min[1].GetDouble(), bounds_min[2].GetDouble() },
          { bounds_max[0].GetDouble(), bounds_max[1].GetDouble(), bounds_max[2].GetDouble() } };

  props.morton_index_parser = [](const std::string& name) {
    return DynamicMortonIndex::parse_string(name, MortonIndexNamingConvention::Potree);
  };

  props.points_have_offset = true;

  return props;
}

static tl::expected<Properties, std::string>
parse_ept_json(const std::string& ept_json_path)
{
  auto fp = fopen(ept_json_path.c_str(), "r");
  if (!fp) {
    const auto error_msg = boost::format("Can't open ept.json file @ \"%1%\"") % ept_json_path;
    return tl::make_unexpected(error_msg.str());
  }

  BOOST_SCOPE_EXIT(&fp) { fclose(fp); }
  BOOST_SCOPE_EXIT_END

  char buf[65536];
  rj::FileReadStream stream{ fp, buf, sizeof(buf) };

  rj::Document document;
  if (document.ParseStream(stream).HasParseError()) {
    const auto parse_error_msg = rapidjson::GetParseError_En(document.GetParseError());
    const auto error_msg = boost::format("Can't parse ept.json file [%1%]") % parse_error_msg;
    return tl::make_unexpected(error_msg.str());
  }

  Properties props;

  const auto& bounds_member = document["bounds"];
  props.root_bounds = AABB{
    { bounds_member[0].GetDouble(), bounds_member[1].GetDouble(), bounds_member[2].GetDouble() },
    { bounds_member[3].GetDouble(), bounds_member[4].GetDouble(), bounds_member[5].GetDouble() }
  };

  const auto span = document["span"].GetInt64();
  const auto bounds_extent = props.root_bounds.extent();

  props.root_spacing = bounds_extent.x / span;

  props.morton_index_parser = [](const std::string& name) {
    return DynamicMortonIndex::parse_string(name, MortonIndexNamingConvention::Entwine);
  };

  props.points_have_offset = false;

  return props;
}

static tl::expected<Properties, std::string>
parse_cloud_js(const std::string& cloud_js_path)
{
  auto fp = fopen(cloud_js_path.c_str(), "r");
  if (!fp) {
    const auto error_msg = boost::format("Can't open cloud.js file @ \"%1%\"") % cloud_js_path;
    return tl::make_unexpected(error_msg.str());
  }

  BOOST_SCOPE_EXIT(&fp) { fclose(fp); }
  BOOST_SCOPE_EXIT_END

  char buf[65536];
  rj::FileReadStream stream{ fp, buf, sizeof(buf) };

  rj::Document document;
  if (document.ParseStream(stream).HasParseError()) {
    const auto parse_error_msg = rapidjson::GetParseError_En(document.GetParseError());
    const auto error_msg = boost::format("Can't parse cloud.js file [%1%]") % parse_error_msg;
    return tl::make_unexpected(error_msg.str());
  }

  Properties props;

  props.root_spacing = static_cast<float>(document["spacing"].GetDouble());

  const auto& bounds_member = document["boundingBox"];

  props.root_bounds = AABB{ { bounds_member["lx"].GetDouble(),
                              bounds_member["ly"].GetDouble(),
                              bounds_member["lz"].GetDouble() },
                            { bounds_member["ux"].GetDouble(),
                              bounds_member["uy"].GetDouble(),
                              bounds_member["uz"].GetDouble() } };

  props.morton_index_parser = [](const std::string& name) {
    return DynamicMortonIndex::parse_string(name, MortonIndexNamingConvention::Potree);
  };

  props.points_have_offset = true;

  return props;
}

static tl::expected<Properties, std::string>
parse_properties(const std::string& source_folder)
{
  const auto properties_json_path = source_folder + "/properties.json";
  if (fs::exists(properties_json_path))
    return parse_properties_json(properties_json_path);

  const auto ept_json_path = source_folder + "/ept.json";
  if (fs::exists(ept_json_path))
    return parse_ept_json(ept_json_path);

  const auto cloud_js_path = source_folder + "/cloud.js";
  if (fs::exists(cloud_js_path))
    return parse_cloud_js(cloud_js_path);

  return tl::make_unexpected(
    std::string{ "Source folder does not contain \"properties.json\" or \"ept.json\" "
                 "file! "
                 "Converter process only supports source folders that contain the result "
                 "of a "
                 "'tiler' process invocation, an Entwine 'build' process invocation or a "
                 "potree-converter invocation!" });
}

static std::unique_ptr<SRSTransformHelper>
get_transformation_helper(const std::optional<std::string>& sourceProjection)
{
  if (!sourceProjection) {
    util::write_log("Source projection not specified, skipping point transformation");
    return std::make_unique<IdentityTransform>();
  }

  if (std::strcmp(sourceProjection->c_str(), "+proj=longlat +datum=WGS84 +no_defs") == 0) {
    util::write_log("Source projection is already WGS84, skipping point transformation");
    return std::make_unique<IdentityTransform>();
  }

  try {
    return std::make_unique<Proj4Transform>(*sourceProjection);
  } catch (const std::runtime_error& err) {
    util::write_log(concat("Error while setting up coordinate transformation: ",
                           err.what(),
                           "\nSkipping point transformation"));
  }
  return std::make_unique<IdentityTransform>();
}

static std::optional<PointsPersistence>
get_persistence_for_file(const fs::path& file_path,
                         const std::string& source_folder,
                         const PointAttributes& attributes,
                         float spacing_at_root)
{
  const auto extension = file_path.extension();

  // TODO Support for schema conversions in the ConverterProcess

  if (extension == ".bin") {
    return std::make_optional<PointsPersistence>(
      BinaryPersistence{ source_folder, attributes, attributes, Compressed::No });
  }
  if (extension == ".binz") {
    return std::make_optional<PointsPersistence>(
      BinaryPersistence{ source_folder, attributes, attributes, Compressed::Yes });
  }
  if (extension == ".las") {
    return std::make_optional<PointsPersistence>(
      LASPersistence{ source_folder, attributes, attributes, Compressed::No });
  }
  if (extension == ".laz") {
    return std::make_optional<PointsPersistence>(
      LASPersistence{ source_folder, attributes, attributes, Compressed::Yes });
  }
  if (extension == ".pnts") {
    return std::make_optional<PointsPersistence>(Cesium3DTilesPersistence{
      source_folder, attributes, attributes, RGBMapping::None, spacing_at_root, {} });
  }

  return std::nullopt;
}

static bool
is_valid_file_extension(const std::string& extension)
{
  static const std::unordered_set<std::string> s_valid_extensions = {
    ".las", ".laz", ".bin", ".binz", ".pnts"
  };
  return s_valid_extensions.find(extension) != s_valid_extensions.end();
}

struct OctreeNode
{
  OctreeNode()
    : parent(nullptr)
    , level(-1)
  {}

  OctreeNode const* parent;
  std::array<std::unique_ptr<OctreeNode>, 8> children;

  int32_t level;
  std::string name;
  fs::path filepath;
  AABB bounds;
  float spacing;
};

static std::vector<fs::path>
find_all_octree_node_files(const std::string& source_folder,
                           std::optional<uint32_t> max_depth,
                           NodeNameToMortonIndex_t parse_morton_index)
{
  const auto all_files = get_all_files_in_directory(source_folder, Recursive::Yes);
  std::vector<fs::path> filtered_files;
  filtered_files.reserve(all_files.size());
  if (max_depth) {
    std::copy_if(
      all_files.begin(),
      all_files.end(),
      std::back_inserter(filtered_files),
      [=](const auto& file) {
        return is_valid_file_extension(file.extension().string()) &&
               parse_morton_index(file.stem().string())
                 .map([=](const auto& morton_index) { return morton_index.depth() <= *max_depth; })
                 .value_or(false);
      });
  } else {
    std::copy_if(
      all_files.begin(), all_files.end(), std::back_inserter(filtered_files), [](const auto& file) {
        return is_valid_file_extension(file.extension().string());
      });
  }

  return filtered_files;
}

static std::unique_ptr<OctreeNode>
generate_tree(const std::vector<fs::path>& node_files,
              const AABB& root_bounds,
              float root_spacing,
              NodeNameToMortonIndex_t morton_index_parser)
{
  auto root_node = std::make_unique<OctreeNode>();

  const auto find_node_from_name =
    [&](const auto& name) -> tl::expected<std::pair<OctreeNode*, DynamicMortonIndex>, std::string> {
    return morton_index_parser(name).map(
      [&](const auto& current_node_morton_index) -> std::pair<OctreeNode*, DynamicMortonIndex> {
        auto current_node = root_node.get();
        for (auto octant : current_node_morton_index) {
          auto& child_node = current_node->children[octant];
          if (!child_node) {
            child_node = std::make_unique<OctreeNode>();
            child_node->parent = current_node;
            child_node->level = current_node->level + 1;
          }
          current_node = child_node.get();
        }
        return std::make_pair(current_node, current_node_morton_index);
      });
  };

  for (auto& file : node_files) {
    const auto& node_name = file.filename().stem().string();
    find_node_from_name(node_name)
      .map([&](const auto& node_and_morton_index) {
        const auto node_for_file = node_and_morton_index.first;
        const auto& morton_index = node_and_morton_index.second;

        node_for_file->name = node_name;
        node_for_file->filepath = file;
        node_for_file->bounds = get_bounds_from_morton_index(morton_index, root_bounds);
        node_for_file->spacing = root_spacing / std::pow(2, node_for_file->level + 1);
      })
      .or_else([&](const auto& error_msg) {
        std::cerr << "Couldn't process node \"" << node_name << "\" [" << error_msg << "]"
                  << std::endl;
      });
  }

  return root_node;
}

static std::vector<OctreeNode const*>
get_children_at_level(const OctreeNode& root_node, uint32_t level)
{
  // level 0 == root_node
  std::vector<OctreeNode const*> children;
  std::queue<OctreeNode const*> to_process;
  to_process.push(&root_node);

  while (!to_process.empty()) {
    const auto current_node = to_process.front();
    to_process.pop();

    const auto level_diff = current_node->level - root_node.level;
    if (level_diff == static_cast<int32_t>(level)) {
      children.push_back(current_node);
    } else {
      for (size_t idx = 0; idx < 8; ++idx) {
        if (current_node->children[idx] != nullptr) {
          to_process.push(current_node->children[idx].get());
        }
      }
    }
  }

  return children;
}

static std::vector<OctreeNode const*>
split_tree_into_subtrees(const OctreeNode& root_node, uint32_t max_levels_per_subtree)
{
  std::vector<OctreeNode const*> subtrees;
  std::queue<OctreeNode const*> next_root_nodes;
  next_root_nodes.push(&root_node);

  while (!next_root_nodes.empty()) {
    const auto current_root_node = next_root_nodes.front();
    next_root_nodes.pop();
    subtrees.push_back(current_root_node);

    // Go down max_levels_per_subtree and find all the children of
    // current_root_node at that level
    const auto children_at_max_level =
      get_children_at_level(*current_root_node, max_levels_per_subtree);
    for (auto child : children_at_max_level) {
      next_root_nodes.push(child);
    }
  }

  return subtrees;
}

static Tileset
create_tileset_for_leaf_node(const OctreeNode& node, const SRSTransformHelper& transformation);

static Tileset
create_tileset_for_interior_node(const OctreeNode& node,
                                 const SRSTransformHelper& transformation,
                                 uint32_t max_level)
{
  Tileset tileset;
  tileset.url = node.name + ".json";
  tileset.geometricError = node.spacing * spacing_correction_factor;
  tileset.boundingVolume = boundingVolumeFromAABB(node.bounds, transformation);
  tileset.content_url = node.name + ".pnts";

  for (auto& child_node : node.children) {
    if (!child_node)
      continue;

    const auto child_has_children =
      std::any_of(child_node->children.begin(),
                  child_node->children.end(),
                  [](const auto& child_of_child) { return child_of_child != nullptr; });

    if (max_level == 1 && child_has_children) {
      tileset.children.push_back(create_tileset_for_leaf_node(*child_node, transformation));
    } else {
      tileset.children.push_back(
        create_tileset_for_interior_node(*child_node, transformation, max_level - 1));
    }
  }

  return tileset;
}

static Tileset
create_tileset_for_leaf_node(const OctreeNode& node, const SRSTransformHelper& transformation)
{
  Tileset tileset;

  tileset.url = node.name + ".json";
  tileset.geometricError = node.spacing * spacing_correction_factor;
  tileset.boundingVolume = boundingVolumeFromAABB(node.bounds, transformation);
  tileset.content_url = node.name + ".json";

  return tileset;
}

static void
write_tileset_json_for_subtree(const OctreeNode& subtree_root,
                               uint32_t depth_of_subtree,
                               const SRSTransformHelper& transformation,
                               const std::string& out_folder)
{
  const auto tileset_for_subtree =
    create_tileset_for_interior_node(subtree_root, transformation, depth_of_subtree);

  const auto tileset_path = out_folder + "/" + tileset_for_subtree.url;
  writeTilesetJSON(tileset_path, tileset_for_subtree);
}

static std::string
get_pnts_file_path(const fs::path& las_file_path, const std::string& output_folder)
{
  const auto name_stem = las_file_path.stem();
  return concat(output_folder, "/", name_stem.generic_string(), ".pnts");
}

static void
convert_to_pnts_file(const std::string& source_folder,
                     const fs::path& input_file,
                     const std::string& output_folder,
                     const PointAttributes& attributes,
                     float spacing_at_root,
                     const SRSTransformHelper& transformation,
                     DeleteSource delete_source)
{
  auto persistence =
    get_persistence_for_file(input_file, source_folder, attributes, spacing_at_root);
  if (!persistence) {
    util::write_log(concat("Could not read source file \"",
                           input_file.filename().string(),
                           "\": Unrecognized format!\n"));
    return;
  }

  const auto out_file_name = get_pnts_file_path(input_file, output_folder);
  PNTSWriter writer{ out_file_name, attributes, RGBMapping::None };

  const auto node_name = input_file.filename().stem().string();

  PointBuffer source_data;
  persistence->retrieve_points(node_name, source_data);
  transformation.transformPositionsTo(TargetSRS::CesiumWorld,
                                      gsl::make_span(source_data.positions()));
  auto local_offset_to_world = setOriginToSmallestPoint(source_data.positions());

  std::vector<PointBuffer::PointReference> point_references;
  point_references.reserve(source_data.count());
  std::copy(std::begin(source_data), std::end(source_data), std::back_inserter(point_references));

  writer.write_points(point_references);
  writer.flush(local_offset_to_world);

  if (delete_source == DeleteSource::Yes) {
    util::write_log("Deleting source file!");
    std::error_code ec;
    if (!fs::remove(input_file, ec)) {
      std::cerr << ec.message() << std::endl;
    }
  }
}

static void
convert_to_las_file(const std::string& source_folder,
                    const fs::path& input_file,
                    const std::string& output_folder,
                    const PointAttributes& attributes,
                    const SRSTransformHelper& transformation,
                    const AABB& root_bounds,
                    float spacing_at_root,
                    NodeNameToMortonIndex_t morton_index_parser,
                    Compressed compressed,
                    DeleteSource delete_source)
{
  auto persistence =
    get_persistence_for_file(input_file, source_folder, attributes, spacing_at_root);
  if (!persistence) {
    util::write_log(concat("Could not read source file \"",
                           input_file.filename().string(),
                           "\": Unrecognized format!\n"));
    return;
  }

  LASPersistence las_persistence{ output_folder, attributes, attributes, compressed };

  const auto node_name = input_file.filename().stem().string();

  morton_index_parser(node_name)
    .map([&](const auto& node_morton_index) {
      const auto node_bounds = get_bounds_from_morton_index(node_morton_index, root_bounds);

      PointBuffer source_data;
      persistence->retrieve_points(node_name, source_data);

      las_persistence.persist_points(
        std::begin(source_data), std::end(source_data), node_bounds, node_name);

      if (delete_source == DeleteSource::Yes) {
        std::error_code ec;
        if (!fs::remove(input_file, ec)) {
          std::cerr << ec.message() << std::endl;
        }
      }
    })
    .or_else([&](const auto& error_msg) { std::cerr << error_msg << std::endl; });
}

static void
convert_to_3dtiles_impl(const ConverterArguments& args,
                        const Properties& properties,
                        ProgressReporter& progress_reporter)
{
  const auto transformation = get_transformation_helper(args.source_projection);
  // Gather source files and create a tree structure in memory
  const auto node_files =
    find_all_octree_node_files(args.source_folder, args.max_depth, properties.morton_index_parser);

  progress_reporter.register_progress_counter<size_t>(progress::CONVERTING, node_files.size());

  TaskSystem task_system;
  task_system.run();

  std::vector<async::Awaitable<void>> futures;

  futures.push_back(task_system.push([&]() {
    const auto octree_root = generate_tree(
      node_files, properties.root_bounds, properties.root_spacing, properties.morton_index_parser);

    // Split tree into subtrees
    const auto subtrees =
      split_tree_into_subtrees(*octree_root, 3); // 3 levels: 8³ = 512 max nodes per subtree

    progress_reporter.register_progress_counter<size_t>(progress::GENERATING_TILESETS,
                                                        subtrees.size());

    // Generate a tileset.json file for each subtree
    // Need 2 methods: One for a full tileset with all content information,
    // children etc., and one for a leaf node, only containing references to the
    // next tileset.json and bounds
    for (auto subtree : subtrees) {
      write_tileset_json_for_subtree(*subtree, 3, *transformation, args.output_folder);

      progress_reporter.increment_progress<size_t>(progress::GENERATING_TILESETS, 1);
    }
  }));

  // Convert LAS/LAZ files into .pnts files
  for (auto& node_file : node_files) {
    futures.push_back(task_system.push([&]() {
      convert_to_pnts_file(args.source_folder,
                           node_file,
                           args.output_folder,
                           args.output_attributes,
                           properties.root_spacing,
                           *transformation,
                           (args.delete_source_files) ? DeleteSource::Yes : DeleteSource::No);

      progress_reporter.increment_progress<size_t>(progress::CONVERTING, 1);
    }));
  }

  async::all(std::move(futures)).await();

  task_system.stop_and_join();
}

static void
convert_to_3dtiles(const ConverterArguments& args, ProgressReporter& progress_reporter)
{
  // Look for a description file in the source folder and extract properties
  // from it This can be either a properties.json file (generated by the Tiler
  // process) or an ept.json file (generated by an Entwine conversion)
  parse_properties(args.source_folder)
    .map([&](const Properties& properties) {
      convert_to_3dtiles_impl(args, properties, progress_reporter);
    })
    .or_else([](const auto& error_msg) {
      std::cerr << error_msg;
      std::exit(1);
    });
}

static void
convert_to_las_impl(const ConverterArguments& args,
                    const Properties& properties,
                    ProgressReporter& progress_reporter,
                    Compressed compressed)
{
  const auto transformation = get_transformation_helper(args.source_projection);
  // Gather source files and create a tree structure in memory
  const auto node_files =
    find_all_octree_node_files(args.source_folder, args.max_depth, properties.morton_index_parser);

  util::write_log(concat("Converting ", node_files.size(), " files\n"));

  progress_reporter.register_progress_counter<size_t>(progress::CONVERTING, node_files.size());

  TaskSystem task_system;
  task_system.run();

  std::vector<async::Awaitable<void>> futures;
  futures.reserve(node_files.size());

  // Convert .bin files into .laz files
  for (auto& node_file : node_files) {
    futures.push_back(task_system.push([&]() {
      convert_to_las_file(args.source_folder,
                          node_file,
                          args.output_folder,
                          args.output_attributes,
                          *transformation,
                          properties.root_bounds,
                          properties.root_spacing,
                          properties.morton_index_parser,
                          compressed,
                          (args.delete_source_files) ? DeleteSource::Yes : DeleteSource::No);

      progress_reporter.increment_progress<size_t>(progress::CONVERTING, 1);
    }));
  }

  async::all(std::move(futures)).await();

  task_system.stop_and_join();
}

static void
convert_to_las(const ConverterArguments& args,
               ProgressReporter& progress_reporter,
               Compressed compressed)
{
  parse_properties(args.source_folder)
    .map([&](const Properties& properties) {
      convert_to_las_impl(args, properties, progress_reporter, compressed);
    })
    .or_else([](const auto& error_msg) {
      std::cerr << error_msg;
      std::exit(1);
    });
}

static void
prepare_conversion(const std::string& source_folder, const std::string& target_folder)
{
  if (!fs::exists(source_folder)) {
    util::write_log(concat("Source folder \"", source_folder, "\" does not exist!"));
    std::exit(EXIT_FAILURE);
  }

  if (target_folder == source_folder)
    return;

  // TODO Add option to clear target folder!
  if (fs::exists(target_folder)) {
    fs::remove_all(target_folder);
  }

  if (!fs::create_directories(target_folder)) {
    util::write_log(concat("Could not create output folder \"", source_folder, "\"!"));
    std::exit(EXIT_FAILURE);
  }
}

void
run_conversion(const ConverterArguments& args)
{

  UIState ui_state;
  TerminalUI ui{ &ui_state };

  std::atomic_bool draw_ui = true;
  std::thread ui_thread{ [&ui, &draw_ui]() {
    while (draw_ui) {
      ui.redraw();
      std::this_thread::sleep_for(std::chrono::milliseconds{ 50 });
    }
  } };

  prepare_conversion(args.source_folder, args.output_folder);
  switch (args.output_format) {
    case OutputFormat::CZM_3DTILES:
      convert_to_3dtiles(args, ui_state.get_progress_reporter());
      break;
    case OutputFormat::LAS:
      convert_to_las(args, ui_state.get_progress_reporter(), Compressed::No);
      break;
    case OutputFormat::LAZ:
      convert_to_las(args, ui_state.get_progress_reporter(), Compressed::Yes);
      break;
    default:
      break;
  }

  draw_ui = false;
  ui_thread.join();
}


/**
 * @file KinematicsState.hpp
 * @author Brahayam Ponton (brahayam.ponton@tuebingen.mpg.de)
 * @license License BSD-3-Clause
 * @copyright Copyright (c) 2019, New York University and Max Planck Gesellschaft.
 * @date 2019-10-08
 */

#pragma once

#include <array>
#include <vector>
#include <memory>
#include <iostream>
#include <Eigen/Eigen>

#include <momentumopt/setting/Definitions.hpp>

namespace momentumopt {

  /*! container for robot position variables */
  class RobotPosture
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
	  RobotPosture(int num_joints)
        : num_joints_(num_joints),
          base_position_(Eigen::Vector3d::Zero()),
          joint_positions_(Eigen::VectorXd(num_joints).setZero()),
		  base_orientation_(Eigen::Quaternion<double>::Identity())
      {}
	  ~RobotPosture(){}

      Eigen::Vector3d& basePosition() { return base_position_; }
      Eigen::VectorXd& jointPositions() { return joint_positions_; }
      Eigen::Quaternion<double>& baseOrientation() { return base_orientation_; }

      const Eigen::Vector3d& basePosition() const { return base_position_; }
      const Eigen::VectorXd& jointPositions() const { return joint_positions_; }
      const Eigen::Quaternion<double>& baseOrientation() const { return base_orientation_; }
      const Eigen::Vector4d pyBaseOrientation() const { return Eigen::Vector4d(base_orientation_.x(), base_orientation_.y(), base_orientation_.z(), base_orientation_.w()); }

      void basePosition(const Eigen::Vector3d& base_position) { base_position_ = base_position; }
      void jointPositions(const Eigen::VectorXd& joint_positions) { joint_positions_ = joint_positions; }
      void baseOrientation(const Eigen::Quaternion<double>& base_orientation) { base_orientation_ = base_orientation; }
      void pyBaseOrientation(const Eigen::Vector4d base_orientation) { base_orientation_ = Eigen::Quaternion<double>(base_orientation[3], base_orientation[0], base_orientation[1], base_orientation[2]); }

      Eigen::VectorXd generalizedJointPositions() const
      {
        Eigen::VectorXd generalized_joint_positions(num_joints_+7);
        generalized_joint_positions.head(3) = base_position_;
        generalized_joint_positions.tail(num_joints_) = joint_positions_;
        generalized_joint_positions.segment<4>(3) = Eigen::Vector4d(base_orientation_.x(), base_orientation_.y(), base_orientation_.z(), base_orientation_.w());
        return generalized_joint_positions;
      }

	  std::string toString() const;
	  friend std::ostream& operator<<(std::ostream &os, const RobotPosture& obj) { return os << obj.toString(); }

    private:
      int num_joints_;
      Eigen::Vector3d base_position_;
      Eigen::VectorXd joint_positions_;
      Eigen::Quaternion<double> base_orientation_;
  };

  /*! container for robot velocity variables */
  class RobotVelocity
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
	  RobotVelocity(int num_joints)
        : num_joints_(num_joints),
		  generalized_joint_velocities_(Eigen::VectorXd(num_joints+6).setZero())
      {}
	  ~RobotVelocity(){}

      Eigen::VectorXd& generalizedJointVelocities() { return generalized_joint_velocities_; }
      Eigen::Ref<Eigen::Vector3d> baseLinearVelocity() { return generalized_joint_velocities_.segment<3>(0); }
      Eigen::Ref<Eigen::Vector3d> baseAngularVelocity() { return generalized_joint_velocities_.segment<3>(3); }
      Eigen::Ref<Eigen::VectorXd> jointVelocities() { return generalized_joint_velocities_.tail(num_joints_); }

      const Eigen::VectorXd& generalizedJointVelocities() const { return generalized_joint_velocities_; }
      const Eigen::Ref<const Eigen::Vector3d> baseLinearVelocity() const { return generalized_joint_velocities_.segment<3>(0); }
      const Eigen::Ref<const Eigen::Vector3d> baseAngularVelocity() const { return generalized_joint_velocities_.segment<3>(3); }
      const Eigen::Ref<const Eigen::VectorXd> jointVelocities() const { return generalized_joint_velocities_.tail(num_joints_); }

      void generalizedJointVelocities(const Eigen::VectorXd& generalized_joint_velocities) { generalized_joint_velocities_ = generalized_joint_velocities; }
      void jointVelocities(const Eigen::Ref<const Eigen::VectorXd> joint_velocities) { generalized_joint_velocities_.tail(num_joints_) = joint_velocities; }
      void baseLinearVelocity(const Eigen::Ref< const Eigen::Vector3d> base_linear_velocity) { generalized_joint_velocities_.segment<3>(0) = base_linear_velocity; }
      void baseAngularVelocity(const Eigen::Ref<const Eigen::Vector3d> base_angular_velocity) { generalized_joint_velocities_.segment<3>(3) = base_angular_velocity; }

	  std::string toString() const;
	  friend std::ostream& operator<<(std::ostream &os, const RobotVelocity& obj) { return os << obj.toString(); }

    private:
      int num_joints_;
      Eigen::VectorXd generalized_joint_velocities_;
  };

  /*! container for robot acceleration variables */
  class RobotAcceleration
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
	  RobotAcceleration(int num_joints)
        : num_joints_(num_joints),
          generalized_joint_accelerations_(Eigen::VectorXd(num_joints+6).setZero())
      {}
	  ~RobotAcceleration(){}

      Eigen::VectorXd& generalizedJointAccelerations() { return generalized_joint_accelerations_; }
      Eigen::Ref<Eigen::Vector3d> baseLinearAcceleration() { return generalized_joint_accelerations_.segment<3>(0); }
      Eigen::Ref<Eigen::Vector3d> baseAngularAcceleration() { return generalized_joint_accelerations_.segment<3>(3); }
      Eigen::Ref<Eigen::VectorXd> jointAccelerations() { return generalized_joint_accelerations_.tail(num_joints_); }

      const Eigen::VectorXd& generalizedJointAccelerations() const { return generalized_joint_accelerations_; }
      const Eigen::Ref<const Eigen::Vector3d> baseLinearAcceleration() const { return generalized_joint_accelerations_.segment<3>(0); }
      const Eigen::Ref<const Eigen::Vector3d> baseAngularAcceleration() const { return generalized_joint_accelerations_.segment<3>(3); }
      const Eigen::Ref<const Eigen::VectorXd> jointAccelerations() const { return generalized_joint_accelerations_.tail(num_joints_); }

      void generalizedJointAccelerations(const Eigen::VectorXd& generalized_joint_accelerations) { generalized_joint_accelerations_ = generalized_joint_accelerations; }
      void jointAccelerations(const Eigen::Ref<const Eigen::VectorXd> joint_accelerations) { generalized_joint_accelerations_.tail(num_joints_) = joint_accelerations; }
      void baseLinearAcceleration(const Eigen::Ref< const Eigen::Vector3d> base_linear_acceleration) { generalized_joint_accelerations_.segment<3>(0) = base_linear_acceleration; }
      void baseAngularAcceleration(const Eigen::Ref<const Eigen::Vector3d> base_angular_acceleration) { generalized_joint_accelerations_.segment<3>(3) = base_angular_acceleration; }

	  std::string toString() const;
	  friend std::ostream& operator<<(std::ostream &os, const RobotAcceleration& obj) { return os << obj.toString(); }

    private:
      int num_joints_;
      Eigen::VectorXd generalized_joint_accelerations_;
  };

  /**
   * This class is a container for all variables required to define a
   * kinematics state: joint posture, joint velocities and accelerations,
   * endeffector poses and momentum.
   */
  class KinematicsState
  {
    public:
      KinematicsState(int num_joints);
      ~KinematicsState(){}

      // center of mass and momentum
	  Eigen::Vector3d& centerOfMass() { return com_; }
	  Eigen::Vector3d& linearMomentum() { return lmom_; }
	  Eigen::Vector3d& angularMomentum() { return amom_; }

	  const Eigen::Vector3d& centerOfMass() const { return com_; }
	  const Eigen::Vector3d& linearMomentum() const { return lmom_; }
	  const Eigen::Vector3d& angularMomentum() const { return amom_; }

	  void centerOfMass(const Eigen::Vector3d& com) { com_ = com; }
	  void linearMomentum(const Eigen::Vector3d& lmom) { lmom_ = lmom; }
	  void angularMomentum(const Eigen::Vector3d& amom) { amom_ = amom; }

      // robot joint variables
	  RobotPosture& robotPosture() { return robot_posture_; }
	  RobotVelocity& robotVelocity() { return robot_velocity_; }
	  RobotAcceleration& robotAcceleration() { return robot_acceleration_; }

	  const RobotPosture& robotPosture() const { return robot_posture_; }
	  const RobotVelocity& robotVelocity() const { return robot_velocity_; }
	  const RobotAcceleration& robotAcceleration() const { return robot_acceleration_; }

	  void robotPosture(const RobotPosture& robot_posture) { robot_posture_ = robot_posture; }
	  void robotVelocity(const RobotVelocity& robot_velocity) { robot_velocity_ = robot_velocity; }
	  void robotAcceleration(const RobotAcceleration& robot_acceleration) { robot_acceleration_ = robot_acceleration; }

      // endeffector poses
      Eigen::Vector3d& endeffectorPosition(int eff_id) { return eff_positions_[eff_id]; }
      Eigen::Vector3d& endeffectorVelocity(int eff_id) { return eff_velocities_[eff_id]; }
      Eigen::Vector3d& endeffectorAcceleration(int eff_id) { return eff_accelerations_[eff_id]; }
      Eigen::Quaternion<double>& endeffectorOrientation(int eff_id) { return eff_orientations_[eff_id]; }

      const Eigen::Vector3d& endeffectorPosition(int eff_id) const { return eff_positions_[eff_id]; }
      const Eigen::Vector3d& endeffectorVelocity(int eff_id) const { return eff_velocities_[eff_id]; }
      const Eigen::Vector3d& endeffectorAcceleration(int eff_id) const { return eff_accelerations_[eff_id]; }
      const Eigen::Quaternion<double>& endeffectorOrientation(int eff_id) const { return eff_orientations_[eff_id]; }

      const std::vector<Eigen::Vector3d>& pyEndeffectorPositions() const { return eff_positions_; }
      void pyEndeffectorPositions(const std::vector<Eigen::Vector3d>& eff_positions) { eff_positions_ = eff_positions; }

	  // Helper functions
	  std::string toString() const;
	  const int& numJoints() const { return num_joints_; }
	  friend std::ostream& operator<<(std::ostream &os, const KinematicsState& obj) { return os << obj.toString(); }

    private:
	  RobotPosture robot_posture_;
	  RobotVelocity robot_velocity_;
	  RobotAcceleration robot_acceleration_;

      int num_joints_;
      Eigen::Vector3d com_, lmom_, amom_;
      std::vector<Eigen::Quaternion<double>> eff_orientations_;
      std::vector<Eigen::Vector3d> eff_positions_, eff_velocities_, eff_accelerations_;
  };

  /**
   * This class is a container for a sequence of kinematic states
   */
  class KinematicsSequence
  {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    public:
	  KinematicsSequence(){}
	  ~KinematicsSequence(){}

	  void clean() { kinematics_sequence_.clear(); }
	  void resize(int num_timesteps, int num_joints);
	  int size() const { return kinematics_sequence_.size(); }

	  const std::vector<KinematicsState>& kinematicsSequence() const { return kinematics_sequence_; }
	  void kinematicsSequence(const std::vector<KinematicsState>& kinematics_sequence) { kinematics_sequence_ = kinematics_sequence; }

	  KinematicsState& kinematicsState(int time_id) { return kinematics_sequence_[time_id]; }
	  const KinematicsState& kinematicsState(int time_id) const { return kinematics_sequence_[time_id]; }

	  std::string toString() const;
  	  friend std::ostream& operator<<(std::ostream &os, const KinematicsSequence& obj) { return os << obj.toString(); }

    private:
      std::vector<KinematicsState> kinematics_sequence_;
  };

}


///////////////////////////////////////////////////////////////////////////////
// Authors: Chester Holtz, Devon Merrill, James (Ting-Chou) Lin, Connie (Yen-Yi) Wu
//          (respective Ph.D. advisors: Chung-Kuan Cheng, Andrew B. Kahng, Steven Swanson).
//
// BSD 3-Clause License
//
// Copyright (c) 2018, The Regents of the University of California
// 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 the copyright holder 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 HOLDER 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.
///////////////////////////////////////////////////////////////////////////////

#define BOOST_NO_AUTO_PTR

#include <functional>
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <ctype.h>
#include <map>
#include <vector>
#include <unordered_set>
#include <algorithm>
#include <string>
#include <numeric>
#include <cmath>
#include <unistd.h>
#include <ctime>
#include <ratio>
#include <chrono>

// boost version 1.69.0
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/geometry.hpp>
#include <boost/assign.hpp>
#include <boost/geometry.hpp>
#include <boost/geometry/io/io.hpp>
#include <boost/geometry/algorithms/area.hpp>
#include <boost/geometry/geometries/point.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/random.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_01.hpp>
#include <boost/foreach.hpp>
#include <boost/geometry/index/rtree.hpp>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>

#include "readFiles.hpp"
//#include "readScl.h"
#include "time.h"
//#include "taskflow/taskflow.hpp"

void random_initial_placement(int rotate_flag);
void set_boundaries();
void initialize_params(std::pair <double,double> &wl_normalization,
                       std::pair <double,double> &area_normalization,
                       std::pair <double,double> &routability_normalization,
                       map<int, vector<Pin> > &netToCell,
                       int rotate_flag);
void validate_move(Node &node, double rx, double ry);
double cost(
            std::pair <double,double> &wl_normalization,
            std::pair <double,double> &area_normalization,
            std::pair <double,double> &routability_normalization,
            map<int, vector<Pin> > &netToCell,
            int temp_debug = 0);
double cost_partial(vector < Node *> &nodes,
                    std::pair <double,double> &wl_normalization,
                    std::pair <double,double> &area_normalization,
                    std::pair <double,double> &routability_normalization,
                    map<int, vector<Pin> > &netToCell);
double cell_overlap();
double wirelength(map<int, vector<Pin> > &netToCell);
double cell_overlap_partial(vector < Node* > &nodes);
double wirelength_partial(vector < Node* > &nodes, map<int, vector<Pin> > &netToCell);
double rudy(map<int, vector<Pin> > &netToCell);
float annealer(int outer_loop_iter,
                          int inner_loop_iter,
                          double eps,
                          double t_0,bool var,
                          map<int, vector<Pin> > &netToCell,
                          string initial_pl,
                          int rotate_flag);
float multistart();
double initialize_temperature(vector< int > &accept_history,
                              double &Temperature,
                              std::pair <double,double> &wl_normalization,
                              std::pair <double,double> &area_normalization,
                              std::pair <double,double> &routability_normalization,
                              map<int, vector<Pin> > &netToCell,
                              int rotate_flag);
void update_Temperature(double& Temperature);
double initiate_move(double current_cost,
                     vector< int > &accept_history,
                     double & Temperature,
                     std::pair <double,double> &wl_normalization,
                     std::pair <double,double> &area_normalization,
                     std::pair <double,double> &routability_normalization,
                     map<int, vector<Pin> > &netToCell,
                     int rotate_flag);
bool check_move(double prevCost,
                double newCost,
                vector< int > &accept_history,
                double &Temperature);
void project_soln();
void random_placement(int xmin, int xmax, int ymin, int ymax, Node &n, int rotate_flag);
void gen_report(map<string, vector<double> > &report,
                vector< double > &accept_ratio_history,
                std::pair <double,double> &wl_normalization,
                std::pair <double,double> &area_normalization,
                std::pair <double,double> &routability_normalization,
                map<int, vector<Pin> > &netToCell);
void update_accept_history(vector< int > &accept_history, vector< double > &accept_ratio_history, float &accept_ratio);
void update_rtree(int idx);
vector < Node > ::iterator random_node();

struct boundaries {
  double minX, maxX, minY, maxY = 0.0;
};

int fact(int n);


// external
#include <boost/dynamic_bitset.hpp>
#include <boost/range/adaptor/filtered.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/algorithm_ext/push_back.hpp>
#include <boost/range/irange.hpp>

// internal
#include "poac/core/resolver/resolve.hpp"
#include "poac/core/resolver/sat.hpp"
#include "poac/util/net.hpp"
#include "poac/util/semver/semver.hpp"
#include "poac/util/verbosity.hpp"

namespace poac::core::resolver::resolve {

// A ∨ B ∨ C
// A ∨ ¬B ∨ ¬C
// ¬A ∨ B ∨ ¬C
// ¬A ∨ ¬B ∨ C
// ¬A ∨ ¬B ∨ ¬C
Vec<Vec<i32>>
multiple_versions_cnf(const Vec<i32>& clause) {
  return boost::irange(0, 1 << clause.size()) // number of combinations
       | boost::adaptors::transformed([&clause](const i32 i) {
           return boost::dynamic_bitset<>(to_binary_numbers(i, clause.size()));
         }) |
         boost::adaptors::filtered([](const boost::dynamic_bitset<>& bs) {
           return bs.count() != 1;
         }) |
         boost::adaptors::transformed(
             [&clause](const boost::dynamic_bitset<>& bs) -> Vec<i32> {
               return boost::irange(usize{0}, bs.size()) |
                      boost::adaptors::transformed([&clause, &bs](const i32 i) {
                        return bs[i] ? clause[i] * -1 : clause[i];
                      }) |
                      util::meta::containerized;
             }
         ) |
         util::meta::containerized;
}

Vec<Vec<i32>>
create_cnf(const DupDeps<WithDeps>& activated) {
  Vec<Vec<i32>> clauses;
  Vec<i32> already_added;

  DupDeps<WithDeps>::const_iterator first = std::cbegin(activated);
  DupDeps<WithDeps>::const_iterator last = std::cend(activated);
  for (i32 i = 0; i < static_cast<i32>(activated.size()); ++i) {
    if (util::meta::find(already_added, i)) {
      continue;
    }

    const auto name_lambda = [&](const auto& x) {
      return get_package(x) == get_package(activated[i]);
    };
    // No other packages with the same name as the package currently pointed to
    // exist
    if (const i64 count = std::count_if(first, last, name_lambda); count == 1) {
      Vec<i32> clause;
      clause.emplace_back(i + 1);
      clauses.emplace_back(clause);

      // index ⇒ deps
      if (!activated[i].second.has_value()) {
        clause[0] *= -1;
        for (const auto& [name, version] : activated[i].second.value()) {
          // It is guaranteed to exist
          clause.emplace_back(
              util::meta::index_of_if(
                  first, last,
                  [&n = name, &v = version](const auto& d) {
                    return get_package(d).name == n &&
                           get_package(d).version_rq == v;
                  }
              ) +
              1
          );
        }
        clauses.emplace_back(clause);
      }
    } else if (count > 1) {
      Vec<i32> clause;

      for (DupDeps<WithDeps>::const_iterator found = first; found != last;
           found = std::find_if(found, last, name_lambda)) {
        const i64 index = std::distance(first, found);
        clause.emplace_back(index + 1);
        already_added.emplace_back(index + 1);

        // index ⇒ deps
        if (!found->second.has_value()) {
          Vec<i32> new_clause;
          new_clause.emplace_back(index);
          for (const Package& package : found->second.value()) {
            // It is guaranteed to exist
            new_clause.emplace_back(
                util::meta::index_of_if(
                    first, last,
                    [&package](const auto& p) {
                      return get_package(p).name == package.name &&
                             get_package(p).version_rq == package.version_rq;
                    }
                ) +
                1
            );
          }
          clauses.emplace_back(new_clause);
        }
        ++found;
      }
      boost::range::push_back(clauses, multiple_versions_cnf(clause));
    }
  }
  return clauses;
}

[[nodiscard]] Result<UniqDeps<WithDeps>, String>
solve_sat(const DupDeps<WithDeps>& activated, const Vec<Vec<i32>>& clauses) {
  // deps.activated.size() == variables
  const Vec<i32> assignments = Try(sat::solve(clauses, activated.size()));
  UniqDeps<WithDeps> resolved_deps{};
  log::debug("SAT");
  for (i32 a : assignments) {
    log::debug("{} ", a);
    if (a > 0) {
      const auto& [package, deps] = activated[a - 1];
      resolved_deps.emplace(package, deps);
    }
  }
  log::debug(0);
  return Ok(resolved_deps);
}

[[nodiscard]] Result<UniqDeps<WithDeps>, String>
backtrack_loop(const DupDeps<WithDeps>& activated) {
  const Vec<Vec<i32>> clauses = create_cnf(activated);
  if (util::verbosity::is_verbose()) {
    for (const Vec<i32>& c : clauses) {
      for (i32 l : c) {
        const auto deps = activated[std::abs(l) - 1];
        const Package package = get_package(deps);
        log::debug("{}-{}: {}, ", package.name, package.version_rq, l);
      }
      log::debug("");
    }
  }
  return solve_sat(activated, clauses);
}

// Interval to multiple versions
// `>=0.1.2 and <3.4.0` -> { 2.4.0, 2.5.0 }
// name is boost/config, no boost-config
[[nodiscard]] Result<Vec<String>, String>
get_versions_satisfy_interval(const Package& package) {
  // TODO(ken-matsui): (`>1.2 and <=1.3.2` -> NG，`>1.2.0-alpha and <=1.3.2` ->
  // OK) `2.0.0` specific version or `>=0.1.2 and <3.4.0` version interval
  const semver::Interval i(package.version_rq);
  const Vec<String> satisfied_versions =
      Try(util::net::api::versions(package.name)) |
      boost::adaptors::filtered([&i](StringRef s) { return i.satisfies(s); }) |
      util::meta::containerized;

  if (satisfied_versions.empty()) {
    return Err(format(
        "`{}: {}` not found; seem dependencies are broken", package.name,
        package.version_rq
    ));
  }
  return Ok(satisfied_versions);
}

DupDeps<WithoutDeps>
gather_deps_of_deps(
    const UniqDeps<WithoutDeps>& deps_api_res, IntervalCache& interval_cache
) {
  DupDeps<WithoutDeps> cur_deps_deps;
  for (const auto& [name, version_rq] : deps_api_res) {
    const Package package{name, version_rq};

    // Check if node package is resolved dependency (by interval)
    const IntervalCache::iterator found_cache =
        boost::range::find_if(interval_cache, [&package](const Cache& cache) {
          return package == cache.package;
        });

    const Vec<String> dep_versions =
        found_cache != interval_cache.cend()
            ? found_cache->versions
            : get_versions_satisfy_interval(package).unwrap();
    if (found_cache == interval_cache.cend()) {
      // Cache interval and versions pair
      interval_cache.emplace(Cache{package, dep_versions});
    }
    for (const String& dep_version : dep_versions) {
      cur_deps_deps.emplace_back(Package{package.name, dep_version});
    }
  }
  return cur_deps_deps;
}

void
gather_deps(
    const Package& package, DupDeps<WithDeps>& new_deps,
    IntervalCache& interval_cache
) {
  // Check if root package resolved dependency (whether the specific version is
  // the same), and check circulating
  if (cache_exists(new_deps, package)) {
    return;
  }

  // Get dependencies of dependencies
  const UniqDeps<WithoutDeps> deps_api_res =
      util::net::api::deps(package.name, package.version_rq).unwrap();
  if (deps_api_res.empty()) {
    new_deps.emplace_back(package, None);
  } else {
    const DupDeps<WithoutDeps> deps_of_deps =
        gather_deps_of_deps(deps_api_res, interval_cache);

    // Store dependency and the dependency's dependencies.
    new_deps.emplace_back(package, deps_of_deps);

    // Gather dependencies of dependencies of dependencies.
    for (const Package& dep_package : deps_of_deps) {
      gather_deps(dep_package, new_deps, interval_cache);
    }
  }
}

[[nodiscard]] Result<DupDeps<WithDeps>, String>
gather_all_deps(const UniqDeps<WithoutDeps>& deps) {
  DupDeps<WithDeps> duplicate_deps;
  IntervalCache interval_cache;

  // Activate the root of dependencies
  for (const auto& [name, version_rq] : deps) {
    const Package package{name, version_rq};

    // Check whether the packages specified in poac.toml
    //   are already resolved which includes
    //   that package's dependencies and package's versions
    //   by checking whether package's interval is the same.
    if (cache_exists(interval_cache, package)) {
      continue;
    }

    // Get versions using interval
    // FIXME: versions API and deps API are received the almost same responses
    const Vec<String> versions = Try(get_versions_satisfy_interval(package));
    // Cache interval and versions pair
    interval_cache.emplace(Cache{package, versions});
    for (const String& version : versions) {
      gather_deps(
          Package{package.name, version}, duplicate_deps, interval_cache
      );
    }
  }
  return Ok(duplicate_deps);
}

} // namespace poac::core::resolver::resolve


#include <NIFTI.h>
#include <nifti1_io.h>
#include <COLOR_ui.h>
#include <TrackVis.h>
#include <VECTOR.h>
#include <cmath>
#include <regex>
#include <cstdlib>
#include "tclap/CmdLine.h"
#include <blitz/array.h>
using namespace std;

#include "colormaps.h"

NIFTI*                   niiDWI;
VECTOR<int>		         dim;
VECTOR<float>	         pixdim;

int                      SCHEME_version;
vector< VECTOR<float> >	 SCHEME_dirs;
vector<float>	         SCHEME_b;
vector<int>              SCHEME_idxB0;
vector<int>              SCHEME_idxDWI;
vector<float>	         SCHEME_shells_b;
vector< vector<int> >    SCHEME_shells_idx;

blitz::Array<float,3>    MAP;
VECTOR<int>		         VOXEL;
float                    MAP_min, MAP_min_view, MAP_max, MAP_max_view;
float 			         MAP_opacity = 0.5;
bool			         showPlane[3] = { true, true, true };
bool                     showAxes = true;
bool			         showConfig = true;
float				     LINE_width = 2.0;

NIFTI*                   niiPEAKS;
int				         PEAKS_n;
bool			         PEAKS_show = false;
float			         PEAKS_thr = 0.0;
bool			         PEAKS_doNormalize = false;
bool			         PEAKS_flip[3] = {false, false, false};
bool			         PEAKS_use_affine = false;
float                    PEAKS_affine[3][3];

TrackVis 		         TRK_file;
int				         TRK_skip;
int				         TRK_nTractsPlotted;
int*   			         TRK_nPoints;
float*			         TRK_coords;
float*			         TRK_colors;
float 			         TRK_crop = 1.0;
bool 			         TRK_crop_mode = true;
bool 			         TRK_show = false;
VECTOR<float> 	         TRK_offset;

bool 			         GLYPHS_show = false;
int                      GLYPHS_shell = 0;
bool			         GLYPHS_flip[3] = {false, false, false};
float	                 GLYPHS_b0_thr = 50.0;
bool			         GLYPHS_use_affine = false;
float                    GLYPHS_affine[3][3];

#include "OPENGL_callbacks.cxx"

/*----------------------------------------------------------------------------------------------------------------------------------*/
int main(int argc, char** argv)
{
    TCLAP::CmdLine cmd("This tool allows one to display in a common 3D space all the objects (DWI data, streamlines etc...) used by COMMIT in order to spot possible incosistencies between the conventions of COMMIT and the software that generated the data, e.g. flip in some axes in the DWI data or in the peaks, spatial shift in the streamlines, whether the affine transformation was already applied to the data etc..", ' ', "1.2");

    TCLAP::UnlabeledValueArg<string> argDWI(    "dwi","Filename of the DWI dataset [4D NIFTI]", true, "", "DWI", cmd );
    TCLAP::ValueArg<string>          argMAP(    "m", "map", "Background map [3D NIFTI]", false, "", "map", cmd );
    TCLAP::ValueArg<string>          argPEAKS(  "p", "peaks", "Main diffusion directions for the extra-axonal part in each voxel [4D NIFTI]", false, "", "peaks", cmd );
    TCLAP::ValueArg<string>          argTRK(    "f", "fibers", "Streamlines for the intra-axonal part [.TRK format]", false, "", "fibers", cmd );
    TCLAP::UnlabeledValueArg<string> argSCHEME( "scheme","Acquisition scheme [text]", true, "", "scheme", cmd );

    try	{ cmd.parse( argc, argv ); }
    catch (TCLAP::ArgException &e) { cerr << "error: " << e.error() << " for arg " << e.argId() << endl; }

    string DWI_filename( argDWI.getValue() );
    string SCHEME_filename( argSCHEME.getValue() );
    string PEAKS_filename( argPEAKS.getValue() );
    string TRK_filename( argTRK.getValue() );
    string MAP_filename( argMAP.getValue() );

    // ===================
    // Reading DWI dataset
    // ===================
    COLOR_msg( "-> Reading 'DWI' dataset:", "\n" );

    niiDWI = new NIFTI;
    niiDWI->open( DWI_filename, true );
    if ( !niiDWI->isValid() )
    {
        COLOR_error( "Unable to open file", "\t" );
        return EXIT_FAILURE;
    }
    dim.x = niiDWI->hdr->dim[1];
    dim.y = niiDWI->hdr->dim[2];
    dim.z = niiDWI->hdr->dim[3];
    pixdim.x = niiDWI->hdr->pixdim[1];
    pixdim.y = niiDWI->hdr->pixdim[2];
    pixdim.z = niiDWI->hdr->pixdim[3];
    printf( "\tdim    : %d x %d x %d x %d\n", dim.x, dim.y, dim.z, niiDWI->hdr->dim[4] );
    printf( "\tpixdim : %.4f x %.4f x %.4f\n", 	pixdim.x, pixdim.y, pixdim.z );
    printf( "\tqform  : %d\n", niiDWI->hdr->qform_code );
    mat44 DWI_qform = niiDWI->hdr->qto_xyz;
    if ( niiDWI->hdr->qform_code > 0 )
    {
        for(int i=0; i<3 ;i++)
        {
            printf( "\t\t| " );
            for(int j=0; j<4 ;j++)
                printf( "%9.4f ", DWI_qform.m[i][j] );
            printf( "|\n" );
        }
    }
    else
    {
        COLOR_warning( "This should never happen!", "\t\t" );
    }
    printf( "\tsform  : %d\n", niiDWI->hdr->sform_code );
    mat44 DWI_sform = niiDWI->hdr->sto_xyz;
    if ( niiDWI->hdr->sform_code > 0 )
    {
        for(int i=0; i<3 ;i++)
        {
            printf( "\t\t| " );
            for(int j=0; j<4 ;j++)
                printf( "%9.4f ", DWI_sform.m[i][j] );
            printf( "|\n" );
        }
    }

    // Read the affine matrix to rotate the vectors
    // NB: we need the inverse, but in this case inv=transpose
    if ( niiDWI->hdr->qform_code != 0 )
    {
        for(int i=0; i<3 ;i++)
        for(int j=0; j<3 ;j++)
            GLYPHS_affine[i][j] = DWI_qform.m[j][i];
    }
    else if ( niiDWI->hdr->sform_code != 0 )
    {
        for(int i=0; i<3 ;i++)
        for(int j=0; j<3 ;j++)
            GLYPHS_affine[i][j] = DWI_sform.m[j][i];
    }
    else {
        for(int i=0; i<3 ;i++)
        for(int j=0; j<3 ;j++)
            GLYPHS_affine[i][j] = 0;
        for(int i=0; i<3 ;i++)
            GLYPHS_affine[i][i] = 1;
    }

    mat33 tmp;
    for(int i=0; i<3 ;i++)
        for(int j=0; j<3 ;j++)
            tmp.m[i][j] = GLYPHS_affine[i][j];
    printf( "\tAffine used (%s):\n", nifti_mat33_determ(tmp)<0?"RADIOLOGICAL":"NEUROLOGICAL" );
    for(int i=0; i<3 ;i++)
    {
        printf( "\t\t| " );
        for(int j=0; j<3 ;j++)
            printf( "%9.4f ", GLYPHS_affine[i][j] );
        printf( "|\n" );
    }

    COLOR_msg( "   [OK]" );

    // ===================
    // Reading SCHEME file
    // ===================
    COLOR_msg( "-> Reading 'SCHEME' file:", "\n" );

    char line[1000];
    FILE* pFile = fopen( SCHEME_filename.c_str(), "rt" );

    // read the version
    // ----------------
    try
    {
        while( fgets(line, 1000, pFile) )
            if ( line[0]!='#' )
                break;

        std::regex reVersion("^VERSION: (.*)\\s*$");
        std::smatch reMatches;

        std::string str_line = string(line);
        if ( !std::regex_match(str_line, reMatches, reVersion) )
        {
            // no header found, assume standards BVECTOR format
            SCHEME_version = 0;
            fseek(pFile, -strlen(line), SEEK_CUR);
        }
        else
        {
            if( strcmp(reMatches[1].str().c_str(),"0")==0 || strcmp(reMatches[1].str().c_str(),"BVECTOR")==0 )
                SCHEME_version = 0;
            else if( strcmp(reMatches[1].str().c_str(),"1")==0 || strcmp(reMatches[1].str().c_str(),"STEJSKALTANNER")==0 )
                SCHEME_version = 1;
            else
                throw "Version not recognized";
        }
    }
    catch( const char* msg )
    {
        COLOR_error( msg, "\t" );
        return EXIT_FAILURE;
    }
    printf( "\tversion   : %s\n", SCHEME_version==0?"BVECTOR":"STEJSKALTANNER" );

    // read the data
    // -------------
    try
    {
        string      reFLOAT( "[-+]?[0-9]*\\.?[0-9]+(?:[eE][-+]?[0-9]+)?" );
        std::regex  reVERSION0( "^\\s*("+reFLOAT+")\\s+("+reFLOAT+")\\s+("+reFLOAT+")\\s+("+reFLOAT+")\\s*$" );
        std::regex  reVERSION1( "^\\s*("+reFLOAT+")\\s+("+reFLOAT+")\\s+("+reFLOAT+")\\s+("+reFLOAT+")\\s+("+reFLOAT+")\\s+("+reFLOAT+")\\s+("+reFLOAT+")\\s*$" );
        std::regex  reEMPTY( "^\\s*$" );
        std::smatch reMatches;
        int         Ns = 0;
        float       x, y, z, b, G, D, d;
        while( fgets(line, 1000, pFile) )
        {
            std::string str_line = string(line);
            if( std::regex_match(str_line, reMatches, reEMPTY) )
                continue;   // skip empty lines

            if( SCHEME_version == 0 )
            {
                if ( !std::regex_match(str_line, reMatches, reVERSION0) )
                    throw "Wrong row format";
                x = std::atof( reMatches[1].str().c_str() );
                y = std::atof( reMatches[2].str().c_str() );
                z = std::atof( reMatches[3].str().c_str() );
                b = std::atof( reMatches[4].str().c_str() ); // in mm^2/s
                VECTOR<float> tmp( x, y, z );
                tmp.Normalize();
                SCHEME_dirs.push_back( tmp );
                SCHEME_b.push_back( b );
            }
            else
            {
                if ( !std::regex_match(str_line, reMatches, reVERSION1) )
                    throw "Wrong row format";
                x = std::atof( reMatches[1].str().c_str() );
                y = std::atof( reMatches[2].str().c_str() );
                z = std::atof( reMatches[3].str().c_str() );
                G = std::atof( reMatches[4].str().c_str() );
                D = std::atof( reMatches[5].str().c_str() );
                d = std::atof( reMatches[6].str().c_str() );
                VECTOR<float> tmp( x, y, z );
                tmp.Normalize();
                SCHEME_dirs.push_back( tmp );
                b = std::pow( 267.513e6*G*d, 2 ) * (D-d/3.0) * 1e-6; // in mm^2/s
                SCHEME_b.push_back( b );
            }

            if ( b<5.0 )
            {
                SCHEME_idxB0.push_back( Ns );
            }
            else
            {
                SCHEME_idxDWI.push_back( Ns );
                if ( std::find(SCHEME_shells_b.begin(), SCHEME_shells_b.end(), b) == SCHEME_shells_b.end() )
                {
                    SCHEME_shells_b.push_back( b ) ;
                    vector<int> tmp;
                    SCHEME_shells_idx.push_back( tmp ) ;
                }
            }
            Ns++;
        }
    }
    catch( const char* msg )
    {
        COLOR_error( msg, "\t" );
        return EXIT_FAILURE;
    }
    fclose(pFile);

    printf( "\tgradients : %d\n", SCHEME_b.size() );
    if ( niiDWI->hdr->dim[4] != SCHEME_b.size() )
    {
        COLOR_error( "The scheme does not match the DWI dataset", "\t" );
        return EXIT_FAILURE;
    }

    // fill data structure about the SCHEME
    // ------------------------------------
    for(int i=0; i < SCHEME_b.size() ;i++)
    {
        if ( SCHEME_b[i] < 5 )
            continue;
        int s = std::find( SCHEME_shells_b.begin(), SCHEME_shells_b.end(), SCHEME_b[i] ) - SCHEME_shells_b.begin();
        SCHEME_shells_idx[s].push_back( i );
    }

    printf( "\tscheme    : %d b0 and %d shells (", SCHEME_idxB0.size(), SCHEME_shells_idx.size() );
    for(int i=0; i < SCHEME_shells_b.size() ;i++)
        printf( " [%d @ b=%.1f]", SCHEME_shells_idx[i].size(), SCHEME_shells_b[i] );
    printf( " )\n" );

    COLOR_msg( "   [OK]" );

    // =======================
    // Creating BACKGROUND map
    // =======================
    COLOR_msg( "-> Preparing 'BACKGROUND' map:", "\n" );
    MAP.resize(dim.x,dim.y,dim.z);
    if ( !MAP_filename.empty() )
    {
        printf( "\tdata   : reading from file\n" );
        NIFTI* niiMAP = new NIFTI;
        niiMAP->open( MAP_filename, true );
        if ( !niiMAP->isValid() )
        {
            COLOR_error( "Unable to open the file", "\t" );
            return EXIT_FAILURE;
        }

        printf( "\tdim    : %d x %d x %d x %d\n" , niiMAP->hdr->dim[1],    niiMAP->hdr->dim[2],    niiMAP->hdr->dim[3], niiMAP->hdr->dim[4] );
        printf( "\tpixdim : %.4f x %.4f x %.4f\n", niiMAP->hdr->pixdim[1], niiMAP->hdr->pixdim[2], niiMAP->hdr->pixdim[3] );

        if ( niiMAP->hdr->dim[1] != dim.x || niiMAP->hdr->dim[2] != dim.y || niiMAP->hdr->dim[3] != dim.z )
        {
            COLOR_error( "The DIMENSIONS do not match those of DWI images", "\t" );
            return EXIT_FAILURE;
        }
        if ( abs(niiMAP->hdr->pixdim[1]-pixdim.x) > 1e-4 || abs(niiMAP->hdr->pixdim[2]-pixdim.y) > 1e-4 || abs(niiMAP->hdr->pixdim[3]-pixdim.z) > 1e-4 )
        {
            COLOR_warning( "The VOXEL SIZE does not match that of DWI images", "\t" );
        }

        FLOAT32 MIN = 0;//(*niiMAP->img)(0,0,0);
        FLOAT32 MAX = MIN;

        for(int i=0; i<dim.x ;i++)
        for(int j=0; j<dim.y ;j++)
        for(int k=0; k<dim.z ;k++)
        {
            MAP(i,j,k) = (*niiMAP->img)(i,j,k);
            if ( MAP(i,j,k) > MAX )
                MAX = MAP(i,j,k);
            if ( MAP(i,j,k) < MIN )
                MIN = MAP(i,j,k);
        }
        if ( MAX - MIN <= 0 )
        {
            COLOR_error( "The dynamic range is zero", "\t" );
            return EXIT_FAILURE;
        }
        MAP_min	= MIN;
        MAP_min_view = 0;
        MAP_max	= MAP_max_view = MAX;

        printf( "\tvalues : [%.2e ... %.2e]\n", MAP_min, MAP_max );
        COLOR_msg( "   [OK]" );
    }
    else
    {
        printf( "\tdata   : " );

        if ( SCHEME_idxB0.size() > 0 )
        {
            printf( "taking first b0 image\n" );
            FLOAT32 MIN = (*niiDWI->img)(0,0,0,SCHEME_idxB0[0]);
            FLOAT32 MAX = MIN;

            for(int i=0; i<dim.x ;i++)
            for(int j=0; j<dim.y ;j++)
            for(int k=0; k<dim.z ;k++)
            {
                MAP(i,j,k) = (*niiDWI->img)(i,j,k,SCHEME_idxB0[0]);
                if ( MAP(i,j,k) > MAX )
                    MAX = MAP(i,j,k);
                if ( MAP(i,j,k) < MIN )
                    MIN = MAP(i,j,k);
            }
            if ( MAX - MIN <= 0 )
            {
                COLOR_error( "The dynamic range is zero", "\t" );
                return EXIT_FAILURE;
            }
            MAP_min	= MIN;
            MAP_min_view = 0;
            MAP_max	= MAP_max_view = MAX;
        }
        else
        {
            printf( "no b0 found\n" );
            MAP = 0;
            MAP_min	= MAP_min_view = 0;
            MAP_max	= MAP_max_view = 1;
        }
        printf( "\tvalues : [%.2e ... %.2e]\n", MAP_min, MAP_max );
        COLOR_msg( "   [OK]" );
    }

    // ==================
    // Reading PEAKS file
    // ==================
    COLOR_msg( "-> Reading 'PEAKS' dataset:", "\n" );

    if ( !PEAKS_filename.empty() )
    {
        niiPEAKS = new NIFTI;
        niiPEAKS->open( PEAKS_filename, true );
        if ( !niiPEAKS->isValid() )
        {
            COLOR_error( "Unable to open the file", "\t" );
            return false;
        }

        if ( niiPEAKS->hdr->dim[0] != 4 || niiPEAKS->hdr->dim[4]%3 != 0 )
        {
            COLOR_error( "The size must be (*,*,*,3*k)", "\t" );
            return EXIT_FAILURE;
        }
        PEAKS_n = niiPEAKS->hdr->dim[4]/3;

        printf( "\tdim     : %d x %d x %d (%d peaks per voxel)\n" , niiPEAKS->hdr->dim[1], niiPEAKS->hdr->dim[2], niiPEAKS->hdr->dim[3], PEAKS_n );
        printf( "\tpixdim  : %.4f x %.4f x %.4f\n", niiPEAKS->hdr->pixdim[1], niiPEAKS->hdr->pixdim[2], niiPEAKS->hdr->pixdim[3] );

        printf( "\tqform   : %d\n", niiPEAKS->hdr->qform_code );
        mat44 PEAKS_qform = niiPEAKS->hdr->qto_xyz;
        if ( niiPEAKS->hdr->qform_code > 0 )
        {
            for(int i=0; i<3 ;i++)
            {
                printf( "\t\t| " );
                for(int j=0; j<4 ;j++)
                    printf( "%9.4f ", PEAKS_qform.m[i][j] );
                printf( "|\n" );
            }
        }
        else
        {
            COLOR_warning( "This should never happen!", "\t\t" );
        }

        printf( "\tsform  : %d\n", niiPEAKS->hdr->sform_code );
        mat44 PEAKS_sform = niiPEAKS->hdr->sto_xyz;
        if ( niiPEAKS->hdr->sform_code > 0 )
        {
            for(int i=0; i<3 ;i++)
            {
                printf( "\t\t| " );
                for(int j=0; j<4 ;j++)
                    printf( "%9.4f ", PEAKS_sform.m[i][j] );
                printf( "|\n" );
            }
        }

        if ( niiPEAKS->hdr->dim[1] != dim.x || niiPEAKS->hdr->dim[2] != dim.y || niiPEAKS->hdr->dim[3] != dim.z )
        {
            COLOR_error( "The DIMENSIONS do not match those of DWI images", "\t" );
            return EXIT_FAILURE;
        }
        if ( abs(niiPEAKS->hdr->pixdim[1]-pixdim.x) > 1e-3 || abs(niiPEAKS->hdr->pixdim[2]-pixdim.y) > 1e-3 || abs(niiPEAKS->hdr->pixdim[3]-pixdim.z) > 1e-3 )
        {
            COLOR_warning( "The VOXEL SIZE does not match that of DWI images", "\t" );
        }
        if (
            niiPEAKS->hdr->sform_code != niiDWI->hdr->sform_code || niiPEAKS->hdr->qform_code != niiDWI->hdr->qform_code || niiPEAKS->hdr->pixdim[0] != niiDWI->hdr->pixdim[0] ||
            niiPEAKS->hdr->quatern_b != niiDWI->hdr->quatern_b || niiPEAKS->hdr->quatern_c != niiDWI->hdr->quatern_c || niiPEAKS->hdr->quatern_d != niiDWI->hdr->quatern_d ||
            niiPEAKS->hdr->qoffset_x != niiDWI->hdr->qoffset_x || niiPEAKS->hdr->qoffset_y != niiDWI->hdr->qoffset_y || niiPEAKS->hdr->qoffset_z != niiDWI->hdr->qoffset_z
        )
        {
            COLOR_warning( "The GEOMETRY does not match that of DWI images", "\t" );
        }

        // Read the affine matrix to rotate the vectors
        // NB: we need the inverse, but in this case inv=transpose
        if ( niiPEAKS->hdr->qform_code != 0 )
        {
            for(int i=0; i<3 ;i++)
            for(int j=0; j<3 ;j++)
                PEAKS_affine[i][j] = PEAKS_qform.m[j][i];
        }
        else if ( niiPEAKS->hdr->sform_code != 0 )
        {
            for(int i=0; i<3 ;i++)
            for(int j=0; j<3 ;j++)
                PEAKS_affine[i][j] = PEAKS_sform.m[j][i];
        }
        else {
            for(int i=0; i<3 ;i++)
            for(int j=0; j<3 ;j++)
                PEAKS_affine[i][j] = 0;
            for(int i=0; i<3 ;i++)
                PEAKS_affine[i][i] = 1;
        }

        printf( "\tAffine used :\n" );
        for(int i=0; i<3 ;i++)
        {
            printf( "\t\t| " );
            for(int j=0; j<3 ;j++)
                printf( "%9.4f ", PEAKS_affine[i][j] );
            printf( "|\n" );
        }

        COLOR_msg( "   [OK]" );
    }
    else {
        // no peaks are passed and won't be showed
        COLOR_msg( "   [no peaks specified]" );
        PEAKS_n = 0;
    }

    // ===================
    // Reading TRACTS file
    // ===================
    COLOR_msg( "-> Reading 'TRACTOGRAM':", "\n" );

    if ( !TRK_filename.empty() )
    {
        TRK_file = TrackVis();
        if ( !TRK_file.open( TRK_filename ) )
        {
            COLOR_error( "Unable to open the file", "\t" );
            return false;
        }

        printf("\tcount      : %d\n" , TRK_file.hdr.n_count );
        printf("\tdim        : %d x %d x %d\n" , TRK_file.hdr.dim[0], TRK_file.hdr.dim[1], TRK_file.hdr.dim[2] );
        printf("\tpixdim     : %.4f x %.4f x %.4f\n", TRK_file.hdr.voxel_size[0], TRK_file.hdr.voxel_size[1], TRK_file.hdr.voxel_size[2] );
        printf("\tscalars    : %d\n" , TRK_file.hdr.n_scalars );
        printf("\tproperties : %d\n" , TRK_file.hdr.n_properties );

        if ( TRK_file.hdr.dim[0] != dim.x || TRK_file.hdr.dim[1] != dim.y || TRK_file.hdr.dim[2] != dim.z ||
             abs(TRK_file.hdr.voxel_size[0]-pixdim.x) > 1e-4 || abs(TRK_file.hdr.voxel_size[1]-pixdim.y) > 1e-4 || abs(TRK_file.hdr.voxel_size[2]-pixdim.z) > 1e-4 )
        {
            COLOR_warning( "The GEOMETRY does not match those of DWI images", "\t" );
        }

        TRK_skip = ceil( TRK_file.hdr.n_count / 25000.0 );
        int N, n_s = TRK_file.hdr.n_scalars, n_p = TRK_file.hdr.n_properties;
        FILE* fp = TRK_file.getFilePtr();

        // count how many points I need to store in memory
        int TractsRead = 0, CoordsRead = 0;
        fseek(fp, 1000, SEEK_SET);
        for(int f=0; f < TRK_file.hdr.n_count ; f++)
        {
            fread( (char*)&N, 1, 4, fp );
            fseek( fp, N*(3+n_s)*4 + n_p*4, SEEK_CUR );
            if ( f%TRK_skip==0 )
            {
                TractsRead++;
                CoordsRead += N;
            }
        }
        printf("\tin memory  : %d (%d points)\n" , TractsRead, CoordsRead );

        // create data structure for drawing the tracts
        TRK_nTractsPlotted = TractsRead;
        TRK_nPoints = new int[TRK_nTractsPlotted];
        TRK_coords  = new float[3*CoordsRead];
        TRK_colors  = new float[3*CoordsRead];

        float* ptr  = TRK_coords;
        float* ptrc = TRK_colors;
        float norm;
        VECTOR<float> dir;
        TractsRead = 0;
        fseek(fp, 1000, SEEK_SET);
        for(int f=0; f < TRK_file.hdr.n_count ; f++)
        {
            if ( f%TRK_skip==0 )
            {
                fread( (char*)&N, 1, 4, fp );
                TRK_nPoints[TractsRead] = N;

                for(int i=0; i<N; i++)
                {
                    fread((char*)ptr, 1, 12, fp);
                    fseek( fp, n_s*4, SEEK_CUR );

                    // coordinates (later they will be scaled back to voxel size)
                    ptr[0] /= pixdim.x;
                    ptr[1] /= pixdim.y;
                    ptr[2] /= pixdim.z;

                    // colors
                    if ( i > 0 )
                    {
                        dir.x = *(ptr  ) - *(ptr-3);
                        dir.y = *(ptr+1) - *(ptr-2);
                        dir.z = *(ptr+2) - *(ptr-1);
                        norm = dir.norm();
                        ptrc[0] = abs( dir.x / norm );
                        ptrc[1] = abs( dir.y / norm );
                        ptrc[2] = abs( dir.z / norm );
                    }
                    else
                    {
                        ptrc[0] = 0;
                        ptrc[1] = 0;
                        ptrc[2] = 0;
                    }

                    ptr  += 3;
                    ptrc += 3;
                }
                fseek( fp, n_p*4, SEEK_CUR );
                TractsRead++;
            }
            else
            {
                fread( (char*)&N, 1, 4, fp );
                fseek( fp, N*(3+n_s)*4 + n_p*4, SEEK_CUR );
            }
        }

        COLOR_msg( "   [OK]" );
        printf( "\n\n" );
    }
    else
    {
        // no fibers are passed and won't be showed
        COLOR_msg( "   [no streamlines specified]" );
        TRK_nTractsPlotted = 0;
    }

    TRK_offset.x = 0;
    TRK_offset.y = 0;
    TRK_offset.z = 0;

    // ============
    // SETUP OpenGL
    // ============
    VOXEL.x = round( dim.x / 2.0 );
    VOXEL.y = round( dim.y / 2.0 );
    VOXEL.z = round( dim.z / 2.0 );
    OpenGL_init( argc, argv );

    return EXIT_SUCCESS;
}


//
// test/ibstream.hpp
//
// Copyright (c) Chris Glover, 2017-2018
//
// Distributed under the Boost Software License, Version 1.0. (See accompanying
// file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
//

#define BOOST_TEST_MODULE TestEncode
#include <boost/test/unit_test.hpp>

#include <boost/foreach.hpp>
#include <boost/radix/basic_codec.hpp>
#include <boost/radix/encode.hpp>
#include <boost/radix/encode_iterator.hpp>
#include <boost/radix/static_ibitstream_lsb.hpp>
#include <boost/radix/static_ibitstream_msb.hpp>
#include <vector>

#include "common.hpp"

template <std::size_t Bits>
class msb_codec
    : public boost::radix::basic_codec<
          boost::radix::bits::to_alphabet_size<Bits>::value> {
 public:
  msb_codec()
      : boost::radix::basic_codec<
            boost::radix::bits::to_alphabet_size<Bits>::value>(
            generate_alphabet(Bits)) {
  }
};

template <std::size_t Bits>
boost::radix::static_ibitstream_msb<Bits> get_segment_unpacker(
    msb_codec<Bits> const&) {
  return boost::radix::static_ibitstream_msb<Bits>();
}

template <std::size_t Bits>
class lsb_codec
    : public boost::radix::basic_codec<
          boost::radix::bits::to_alphabet_size<Bits>::value> {
 public:
  lsb_codec()
      : boost::radix::basic_codec<
            boost::radix::bits::to_alphabet_size<Bits>::value>(
            generate_alphabet(Bits)) {
  }
};

template <std::size_t Bits>
boost::radix::static_ibitstream_lsb<Bits> get_segment_unpacker(
    lsb_codec<Bits> const&) {
  return boost::radix::static_ibitstream_lsb<Bits>();
}

template <std::size_t Bits, typename DataGenerator, typename Encoder>
void test_encode(DataGenerator data_generator, Encoder encoder) {
  std::vector<char_type> alphabet = generate_alphabet(Bits);
  std::vector<bits_type> data     = data_generator(Bits);
  std::string result;
  boost::radix::encode(
      data.begin(), data.end(), std::back_inserter(result), encoder);
  BOOST_TEST(std::equal(
      alphabet.begin(), alphabet.end(), result.begin(), is_equal_unsigned()));
}

template <std::size_t Bits, typename DataGenerator, typename Encoder>
void test_encoder(DataGenerator data_generator, Encoder codec) {
  std::vector<char_type> alphabet = generate_alphabet(Bits);
  std::vector<bits_type> data     = data_generator(Bits);
  std::vector<char_type> result;
  result.resize(boost::radix::encoded_size(data.size(), codec));
  boost::radix::encoder<Encoder, char*> encoder =
      boost::radix::make_encoder(codec, result.data());
  encoder.append(data.begin(), data.end());
  encoder.resolve();
  result.resize(encoder.bytes_written());
  BOOST_TEST(std::equal(
      alphabet.begin(), alphabet.end(), result.begin(), is_equal_unsigned()));
}

template <std::size_t Bits, typename Encoder>
void test_encode_part_range(Encoder codec) {
  using namespace boost::radix::codec_traits;
  std::vector<bits_type> data = generate_random_bytes(1024, (1 << Bits) - 1);
  std::vector<char_type> full_result;
  boost::radix::encode(
      data.begin(), data.end(), std::back_inserter(full_result), codec);
  std::vector<char_type> partial_result;
  partial_result.resize(boost::radix::encoded_size(data.size(), codec));
  boost::radix::encoder<Encoder, char_type*> encoder =
      boost::radix::make_encoder(codec, partial_result.data());
  std::size_t segment                 = 1;
  std::vector<bits_type>::iterator it = data.begin();
  while(std::distance(it, data.end()) < packed_segment_size<Encoder>::value) {
    std::vector<bits_type>::iterator next =
        it + packed_segment_size<Encoder>::value - 1;
    encoder.append(it, next);
    it = next;

    // We also compare the result as we go because it should be able to
    // output partial results.
    if(encoder.bytes_written() ==
       (segment *
        boost::radix::codec_traits::packed_segment_size<Encoder>::value)) {
      ++segment;
      BOOST_TEST(std::equal(
          partial_result.begin(),
          partial_result.begin() + encoder.bytes_written(), full_result.begin(),
          is_equal_unsigned()));
    }
  }

  encoder.append(it, data.end());
  encoder.resolve();
  partial_result.resize(encoder.bytes_written());
  BOOST_TEST(full_result == partial_result);
}

template <std::size_t Bits, typename Encoder>
void test_encode_part_single(Encoder codec) {
  std::vector<bits_type> data = generate_random_bytes(1024, (1 << Bits) - 1);
  std::vector<char_type> full_result;
  boost::radix::encode(
      data.begin(), data.end(), std::back_inserter(full_result), codec);
  std::vector<char_type> partial_result;
  partial_result.resize(boost::radix::encoded_size(data.size(), codec));
  boost::radix::encoder<Encoder, char_type*> encoder =
      boost::radix::make_encoder(codec, partial_result.data());
  std::size_t segment = 1;
  BOOST_FOREACH(bits_type bits, data) {
    encoder.append(bits);

    // We also compare the result as we go because it should be able to
    // output partial results.
    if(encoder.bytes_written() ==
       (segment *
        boost::radix::codec_traits::packed_segment_size<Encoder>::value)) {
      ++segment;
      BOOST_TEST(std::equal(
          partial_result.begin(),
          partial_result.begin() + encoder.bytes_written(), full_result.begin(),
          is_equal_unsigned()));
    }
  }

  encoder.resolve();
  partial_result.resize(encoder.bytes_written());
  BOOST_TEST(full_result == partial_result);
}

template <std::size_t Bits, typename DataGenerator, typename Encoder>
void test_encode_iterator(DataGenerator data_generator, Encoder encoder) {
  std::vector<char_type> alphabet = generate_alphabet(Bits);
  std::vector<bits_type> data     = data_generator(Bits);
  std::string result;
  std::copy(
      data.begin(), data.end(),
      boost::radix::make_encode_iterator(encoder, std::back_inserter(result)));
  BOOST_TEST(std::equal(
      alphabet.begin(), alphabet.end(), result.begin(), is_equal_unsigned()));
}

template <std::size_t Bits>
void test_encode_msb() {
  test_encode<Bits>(generate_all_permutations_msb, msb_codec<Bits>());
}

template <std::size_t Bits>
void test_encode_lsb() {
  test_encode<Bits>(generate_all_permutations_lsb, lsb_codec<Bits>());
}

BOOST_AUTO_TEST_CASE(encode_one_bit_msb) {
  test_encode_msb<1>();
}

BOOST_AUTO_TEST_CASE(encode_two_bit_msb) {
  test_encode_msb<2>();
}

BOOST_AUTO_TEST_CASE(encode_three_bit_msb) {
  test_encode_msb<3>();
}

BOOST_AUTO_TEST_CASE(encode_four_bit_msb) {
  test_encode_msb<4>();
}

BOOST_AUTO_TEST_CASE(encode_five_bit_msb) {
  test_encode_msb<5>();
}

BOOST_AUTO_TEST_CASE(encode_six_bit_msb) {
  test_encode_msb<6>();
}

BOOST_AUTO_TEST_CASE(encode_one_bit_lsb) {
  test_encode_lsb<1>();
}

BOOST_AUTO_TEST_CASE(encode_two_bit_lsb) {
  test_encode_lsb<2>();
}

BOOST_AUTO_TEST_CASE(encode_three_bit_lsb) {
  test_encode_lsb<3>();
}

BOOST_AUTO_TEST_CASE(encode_four_bit_lsb) {
  test_encode_lsb<4>();
}

BOOST_AUTO_TEST_CASE(encode_five_bit_lsb) {
  test_encode_lsb<5>();
}

BOOST_AUTO_TEST_CASE(encode_six_bit_lsb) {
  test_encode_lsb<6>();
}

BOOST_AUTO_TEST_CASE(encode_seven_bit_msb) {
  test_encode_lsb<7>();
}

BOOST_AUTO_TEST_CASE(encode_seven_bit_lsb) {
  test_encode_lsb<7>();
}

BOOST_AUTO_TEST_CASE(encoder_one_bit_msb) {
  test_encoder<1>(generate_all_permutations_msb, msb_codec<1>());
}

BOOST_AUTO_TEST_CASE(encoder_two_bit_msb) {
  test_encoder<2>(generate_all_permutations_msb, msb_codec<2>());
}

BOOST_AUTO_TEST_CASE(encoder_three_bit_msb) {
  test_encoder<3>(generate_all_permutations_msb, msb_codec<3>());
}

BOOST_AUTO_TEST_CASE(encoder_four_bit_msb) {
  test_encoder<4>(generate_all_permutations_msb, msb_codec<4>());
}

BOOST_AUTO_TEST_CASE(encoder_five_bit_msb) {
  test_encoder<5>(generate_all_permutations_msb, msb_codec<5>());
}

BOOST_AUTO_TEST_CASE(encoder_six_bit_msb) {
  test_encoder<6>(generate_all_permutations_msb, msb_codec<6>());
}

BOOST_AUTO_TEST_CASE(encoder_seven_bit_msb) {
  test_encoder<7>(generate_all_permutations_msb, msb_codec<7>());
}

BOOST_AUTO_TEST_CASE(encode_iterator_one_bit_msb) {
  test_encode_iterator<1>(generate_all_permutations_msb, msb_codec<1>());
}

BOOST_AUTO_TEST_CASE(encode_iterator_two_bit_msb) {
  test_encode_iterator<2>(generate_all_permutations_msb, msb_codec<2>());
}

BOOST_AUTO_TEST_CASE(encode_iterator_three_bit_msb) {
  test_encode_iterator<3>(generate_all_permutations_msb, msb_codec<3>());
}

BOOST_AUTO_TEST_CASE(encode_iterator_four_bit_msb) {
  test_encode_iterator<4>(generate_all_permutations_msb, msb_codec<4>());
}

BOOST_AUTO_TEST_CASE(encode_iterator_five_bit_msb) {
  test_encode_iterator<5>(generate_all_permutations_msb, msb_codec<5>());
}

BOOST_AUTO_TEST_CASE(encode_iterator_six_bit_msb) {
  test_encode_iterator<6>(generate_all_permutations_msb, msb_codec<6>());
}

BOOST_AUTO_TEST_CASE(encode_iterator_seven_bit_msb) {
  test_encode_iterator<7>(generate_all_permutations_msb, msb_codec<7>());
}

BOOST_AUTO_TEST_CASE(encode_part_single_one_bit_msb) {
  test_encode_part_single<1>(msb_codec<1>());
}

BOOST_AUTO_TEST_CASE(encode_part_single_two_bit_msb) {
  test_encode_part_single<2>(msb_codec<2>());
}

BOOST_AUTO_TEST_CASE(encode_part_single_three_bit_msb) {
  test_encode_part_single<3>(msb_codec<3>());
}

BOOST_AUTO_TEST_CASE(encode_part_single_four_bit_msb) {
  test_encode_part_single<4>(msb_codec<4>());
}

BOOST_AUTO_TEST_CASE(encode_part_single_five_bit_msb) {
  test_encode_part_single<5>(msb_codec<5>());
}

BOOST_AUTO_TEST_CASE(encode_part_single_six_bit_msb) {
  test_encode_part_single<6>(msb_codec<6>());
}

BOOST_AUTO_TEST_CASE(encode_part_single_seven_bit_msb) {
  test_encode_part_single<7>(msb_codec<7>());
}

BOOST_AUTO_TEST_CASE(encode_part_range_one_bit_msb) {
  test_encode_part_range<1>(msb_codec<1>());
}

BOOST_AUTO_TEST_CASE(encode_part_range_two_bit_msb) {
  test_encode_part_range<2>(msb_codec<2>());
}

BOOST_AUTO_TEST_CASE(encode_part_range_three_bit_msb) {
  test_encode_part_range<3>(msb_codec<3>());
}

BOOST_AUTO_TEST_CASE(encode_part_range_four_bit_msb) {
  test_encode_part_range<4>(msb_codec<4>());
}

BOOST_AUTO_TEST_CASE(encode_part_range_five_bit_msb) {
  test_encode_part_range<5>(msb_codec<5>());
}

BOOST_AUTO_TEST_CASE(encode_part_range_six_bit_msb) {
  test_encode_part_range<6>(msb_codec<6>());
}

BOOST_AUTO_TEST_CASE(encode_part_range_seven_bit_msb) {
  test_encode_part_range<7>(msb_codec<7>());
}


#include <unordered_map>
#include <memory>
#include <iterator>
#include <Eigen/Core>
#include <Eigen/LU>
#include "./parser.hpp"
#include "./lattice.hpp"
#include "./site.hpp"
#include "./search_equiv_sites.hpp"
#include "./myhash.hpp"
#include "./myindex.hpp"
#include "./input.hpp"
#include "./metroconf.hpp"

using allclusters = std::vector<std::vector<std::vector<std::vector<int>>>>;

void dispInput(std::shared_ptr<Input> in){
	//  [INPUT]
	std::cout << "MAXBODY     : " << in->getDataByString("MAXBODY") << std::endl;
	std::cout << "TRUNCATION  : " << in->getDataByString("TRUNCATION") << std::endl;
	std::cout << "SPINCE      : " << in->getDataByString("SPINCE") << std::endl;
	std::cout << "SPINPOSCAR  : " << in->getDataByString("SPINPOSCAR") << std::endl;
	//  [OPTION]
	std::cout << "PAIRTRUNC   : " << in->getDataByString("PAIRTRUNC")  << std::endl;
	std::cout << "TRITRUNC    : " << in->getDataByString("TRITRUNC") << std::endl;
	std::cout << "QUADTRUNC   : " << in->getDataByString("QUADTRUNC") << std::endl;
	std::cout << "CLUSTEROUT  : " << in->getDataByString("CLUSTEROUT") << std::endl;
	std::cout << "CORRDUMP    : " << in->getDataByString("CORRDUMP") << std::endl;
}

void showResult(int is_corrdump, const std::shared_ptr<Input>& in){

	const ParseClusterOut parse_cluster_out("./cluster.out");

	if( is_corrdump>0 ){
		Conf2corr PoscarSpin("./poscar.spin", in, parse_cluster_out.getLabel(), parse_cluster_out.getCluster());

		for(int i=0; i<PoscarSpin.getSpins().size(); ++i ){
			bool isin = PoscarSpin.isInNthNearestNeighborPair(i);
			if( isin ) {std::cout << std::endl; return;}
		}

		for(const auto& corrs : PoscarSpin.getCorrelationFunctions() ) {
			for(const auto& corr : corrs){
				std::cout << corr << " ";
			}
			std::cout << std::endl;
		}
	} else {
		const ParseEcicar ecicar("./ecicar");
		std::shared_ptr<allclusters> pall_clusters_dummy(new allclusters());
		for(int i=0; i<parse_cluster_out.getCluster()->size(); ++i){
			for(const auto eci : ecicar.getEci() ){
				if( (i+1) == eci.first ) {
					pall_clusters_dummy->push_back((*(parse_cluster_out.getCluster()))[i]);
					break;
				}
			}
		}

		Metroconf PoscarSpin("./poscar.spin", in, parse_cluster_out.getLabel(), pall_clusters_dummy, ecicar.getEci(), nullptr, nullptr);
		PoscarSpin.setTotalEnergy();
		auto compositions = PoscarSpin.getCompositions();

		for(int i=0; i<PoscarSpin.getSpins().size(); ++i ){
			bool isin = PoscarSpin.isInNthNearestNeighborPair(i);
			if( isin ) {std::cout << std::endl; return;}
		}

		for(const auto c : compositions) std::cout << c << " ";
		std::cout << PoscarSpin.getTotalEnergy() << std::endl;
	}

}

int main(int argc, char* argv[]){
	int is_corrdump = -1;
	int is_read     = -1;

	int num_max_body = 0;
	double truncation = 0;
	double precision  = 0.00001;
	double pair_truncation = 0;
	double triplet_truncation = 0;
	double quadruplet_truncation = 0;
	std::shared_ptr<Input> in(new Input("getconf.ini"));

	in->setData("MAXBODY",      num_max_body, true);
	in->setData("TRUNCATION",   truncation, true);

	in->setData("PRECISION", precision);
	in->setData("PAIRTRUNC", pair_truncation);
	in->setData("TRITRUNC",  triplet_truncation);
	in->setData("QUADTRUNC", quadruplet_truncation);

	in->setData("CLUSTEROUT",  is_read);
	in->setData("CORRDUMP",    is_corrdump);

	if( pair_truncation == 0 ) pair_truncation = truncation;
	if( triplet_truncation == 0 ) triplet_truncation = truncation;
	if( quadruplet_truncation == 0 ) quadruplet_truncation = truncation;

	if( pair_truncation < triplet_truncation or triplet_truncation < quadruplet_truncation ) {
		std::cerr << "ERROR : TRITRUNC <= PAIRTRUNC(orTRUNATION) and QUADTRUNC <= TRITRUNC(orTRUNATION)" << std::endl;
		exit(1);
	}

	if( is_read>0 ){
		showResult(is_corrdump, in);
		return 1;
	}

	std::string filename_poscar_in = "poscar.spin";

	ParsePoscar poscar(filename_poscar_in.c_str());
	const auto atoms_unit = poscar.getAtoms();

	Eigen::Vector3d unit_x, unit_y, unit_z;
	unit_x << 1, 0, 0;
	unit_y << 0, 1, 0;
	unit_z << 0, 0, 1;

	double unit_length_x = (poscar.getLatticeBasis() * unit_x).norm();
	double unit_length_y = (poscar.getLatticeBasis() * unit_y).norm();
	double unit_length_z = (poscar.getLatticeBasis() * unit_z).norm();

	int N_unit = poscar.getAtoms().size();
	/* if use ceil, integer distance cannot be correctly handled. */
	int expand_x = std::ceil((pair_truncation*3.)/unit_length_x);
	int expand_y = std::ceil((pair_truncation*3.)/unit_length_y);
	int expand_z = std::ceil((pair_truncation*3.)/unit_length_z);

	poscar.expandPoscar(expand_x, expand_y, expand_z);

	const auto atoms = poscar.getAtoms();
	const int N = atoms.size();

	/*  setting lattice and site */
	std::vector<std::shared_ptr<Lattice>> vec_lattices;
	std::vector<std::shared_ptr<Site>> vec_sites;
	setSiteAndLattice(vec_lattices, vec_sites, poscar);

	std::ofstream cluster_info("cluster.info", std::ios::out );
	std::ofstream cluster_out( "cluster.out", std::ios::out );

	for( int i=0; i<atoms_unit.size(); ++i){
		cluster_out << atoms_unit[i].first << " " <<  atoms_unit[i].second.transpose() << std::endl;
	}
	cluster_out << "--" << std::endl;

	/*  set nbody = 1 */
	cluster_info << " -- point cluster" << std::endl;
	int num_index = 1;
	for( const auto& lattice : vec_lattices ){
		cluster_info << num_index << " : "  << "[" << lattice->getLatticeNum() << "] : 0 : 1 : 0 0 0 " << std::endl;
		cluster_out << num_index << " ";
		cluster_out << "1" << " ";
		cluster_out << N_unit << " ";
		cluster_out << N_unit << " ";
		for( int i=0; i<N_unit; ++i){
			cluster_out << vec_sites[i]->getSiteNum() << " ";
		}
		cluster_out << std::endl;
		++num_index;
	}

	/*  set nbody = 2 */
	cluster_info << " -- 2 body cluster" << std::endl;
	std::unordered_map<PairIndex , Eigen::Vector3d, hash_pairindex> index_pair_clusters;
	if( pair_truncation == 0 ) pair_truncation = truncation;
	setSiteReferencesAndPairIndex(vec_lattices, vec_sites, index_pair_clusters, pair_truncation, precision);

	std::vector<PairIndex> sorted_vec_pairindex;
	for(const auto& index_pair : index_pair_clusters){
		sorted_vec_pairindex.push_back(index_pair.first);
	}
	sort(sorted_vec_pairindex.begin(), sorted_vec_pairindex.end());

	cluster_info << " # [index] : distance : multiplicity : coordination " << std::endl;
	for( const auto& pairindex : sorted_vec_pairindex ){
		cluster_info << num_index << " : ";
		cluster_info << "[" << pairindex.lattice_index[0] << "," << pairindex.lattice_index[1]  << "] : ";
		const auto pair_itr = vec_sites[0]->getLinkedSiteIterator(pairindex.distance);
		cluster_info << pairindex.distance << " : " << std::distance(pair_itr.first, pair_itr.second) << " : ";
		cluster_info << index_pair_clusters[pairindex].transpose() << std::endl;

		for( const auto& site : vec_sites ) {
			const auto test_pair_itr = site->getLinkedSiteIterator(pairindex.distance);
			assert( std::distance(pair_itr.first, pair_itr.second) == std::distance(test_pair_itr.first, test_pair_itr.second) );
		}

		cluster_out << num_index << " ";
		cluster_out << "2" << " ";
		cluster_out << N_unit << " ";
		cluster_out << N_unit * std::distance(pair_itr.first, pair_itr.second) << " ";
		for(int i=0; i<N_unit; ++i){
			auto pair_itr = vec_sites[i]->getLinkedSiteIterator(pairindex.distance);
			while( pair_itr.first != pair_itr.second ){
				cluster_out << vec_sites[i]->getSiteNum() << " " << (*(pair_itr.first)).site->getSiteNum() << " ";
				++pair_itr.first;
			}
		}
		cluster_out << std::endl;
		++num_index;
	}
	if( num_max_body == 2 ){
		cluster_out.close();
		cluster_info.close();
		showResult(is_corrdump, in);
		return 1;
	}

	/*  set nbody = 3 */
	cluster_info << " -- 3 body cluster" << std::endl;
	std::vector<TripletIndex> keys_index_triplet_cluster;
	std::unordered_map<TripletIndex, std::vector<std::vector<Eigen::Vector3d>>, hash_tripletindex> index_triplet_cluster;
	setTripletIndex(vec_lattices, vec_sites, sorted_vec_pairindex, keys_index_triplet_cluster, index_triplet_cluster, triplet_truncation, precision);

	for(int i=2; i>=0; --i){
		stable_sort(keys_index_triplet_cluster.begin(), keys_index_triplet_cluster.end(), [i](const TripletIndex& lhs, const TripletIndex& rhs){
				return lhs.vec_pairindex[i] < rhs.vec_pairindex[i];
		});
	}

	for(const auto& tripletindex : keys_index_triplet_cluster){

		cluster_info << num_index << " : ";
		cluster_info << "[] : ";
		cluster_info << " [ ";
		cluster_info << tripletindex.vec_pairindex[0].distance << ",";
		cluster_info << tripletindex.vec_pairindex[1].distance << ",";
		cluster_info << tripletindex.vec_pairindex[2].distance;
		cluster_info << " ] : ";
		cluster_info << index_triplet_cluster[tripletindex].size() <<  " : ";

		cluster_info << index_triplet_cluster[tripletindex][0][0].transpose() << ",";
		cluster_info << index_triplet_cluster[tripletindex][0][1].transpose() << std::endl;

		cluster_out << num_index << " ";
		cluster_out << "3" << " ";
		cluster_out << N_unit << " ";
		cluster_out << N_unit*index_triplet_cluster[tripletindex].size() << " ";
		for(int i=0; i<N_unit; ++i){
			for(int j=0; j<index_triplet_cluster[tripletindex].size(); ++j){
				cluster_out  << vec_sites[i]->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_triplet_cluster[tripletindex][j][0])->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_triplet_cluster[tripletindex][j][1])->getSiteNum() << " ";
			}
		}
		cluster_out << std::endl;
		++num_index;
	}

	if( num_max_body == 3 ){
		cluster_out.close();
		cluster_info.close();
		showResult(is_corrdump, in);
		return 1;
	}

	// /*  set nbody = 4 */
	cluster_info << " -- 4 body cluster" << std::endl;
	std::vector<QuadrupletIndex> keys_index_quadruplet_cluster;
	std::unordered_map<QuadrupletIndex, std::vector<std::vector<Eigen::Vector3d>>, hash_quadrupletindex> index_quadruplet_cluster;
	setQuadrupletIndex(vec_lattices, vec_sites, sorted_vec_pairindex,  index_triplet_cluster, keys_index_quadruplet_cluster, index_quadruplet_cluster, quadruplet_truncation, precision);

	for(int i=3; i>=0; --i){
		for(int j=2; j>=0; --j){
		stable_sort(keys_index_quadruplet_cluster.begin(), keys_index_quadruplet_cluster.end(),
			[i,j](const QuadrupletIndex& lhs, const QuadrupletIndex& rhs){
				return lhs.vec_tripletindex[i].vec_pairindex[j] < rhs.vec_tripletindex[i].vec_pairindex[j];
			});
		}
	}

	for(const auto& key_index_quadruplet_cluster : keys_index_quadruplet_cluster){

		cluster_info << num_index << " : ";
		cluster_info << "[] : ";
		cluster_info << " [ ";
		for( const auto distance : key_index_quadruplet_cluster.getVecDistance() ){
			cluster_info << distance << ",";
		}
		cluster_info << " ] : ";
		cluster_info << index_quadruplet_cluster[key_index_quadruplet_cluster].size() <<  " : ";

		cluster_info << index_quadruplet_cluster[key_index_quadruplet_cluster][0][0].transpose() << ",";
		cluster_info << index_quadruplet_cluster[key_index_quadruplet_cluster][0][1].transpose() << ",";
		cluster_info << index_quadruplet_cluster[key_index_quadruplet_cluster][0][2].transpose() << std::endl;

		cluster_out << num_index << " ";
		cluster_out << "4" << " ";
		cluster_out << N_unit << " ";
		cluster_out << N_unit*index_quadruplet_cluster[key_index_quadruplet_cluster].size() << " ";
		for(int i=0; i<N_unit; ++i){
			for(int j=0; j<index_quadruplet_cluster[key_index_quadruplet_cluster].size(); ++j){
				cluster_out  << vec_sites[i]->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_quadruplet_cluster[key_index_quadruplet_cluster][j][0])->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_quadruplet_cluster[key_index_quadruplet_cluster][j][1])->getSiteNum() << " ";
				cluster_out  << vec_sites[i]->getLinkedSite(index_quadruplet_cluster[key_index_quadruplet_cluster][j][2])->getSiteNum() << " ";
			}
		}
		cluster_out << std::endl;
		++num_index;
	}
	cluster_out.close();
	cluster_info.close();

	showResult(is_corrdump, in);
	return 1;
}