#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>

#include <pcl/recognition/linemod.h>
#include <pcl/recognition/color_gradient_modality.h>
#include <pcl/recognition/surface_normal_modality.h>

#include <boost/any.hpp>
#include <boost/program_options.hpp>

#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <v4r/common/pcl_opencv.h>

//#include <png++/png.hpp>

using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloudXYZRGBA;

pcl::LINEMOD linemod;

double score_threshold = 0.8;
void printElapsedTimeAndNumberOfPoints (double t, int w, int h=1)
{
  print_info ("[done, "); print_value ("%g", t); print_info (" ms : ");
  print_value ("%d", w*h); print_info (" points]\n");
}

bool
loadCloud (const std::string & filename, PointCloudXYZRGBA & cloud)
{
  TicToc tt;
  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());

  tt.tic ();
  if (loadPCDFile (filename, cloud) < 0)
    return (false);

  printElapsedTimeAndNumberOfPoints (tt.toc (), cloud.width, cloud.height);

  print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());

  return (true);
}

std::vector<pcl::LINEMODDetection>
matchTemplates (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr & input, const pcl::LINEMOD & linemod)
{
  pcl::ColorGradientModality<pcl::PointXYZRGB> color_grad_mod;
  color_grad_mod.setInputCloud (input);
  color_grad_mod.processInputData ();

  pcl::SurfaceNormalModality<pcl::PointXYZRGB> surface_norm_mod;
  surface_norm_mod.setInputCloud (input);
  surface_norm_mod.processInputData ();

  std::vector<pcl::QuantizableModality*> modalities (2);
  modalities[0] = &color_grad_mod;
  modalities[1] = &surface_norm_mod;

  std::vector<pcl::LINEMODDetection> detections;
  linemod.matchTemplates (modalities, detections);

  return (detections);
}

void compute (const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr & input) {

  // Match the templates to the provided image
  std::vector<pcl::LINEMODDetection> detections = matchTemplates (input, linemod);

  // Output the position and score of the best match for each template
  for (size_t i = 0; i < detections.size (); ++i) {
    const LINEMODDetection & d = detections[i];
    printf ("%lu: %d %d %d %f\n", i, d.x, d.y, d.template_id, d.score);
  }

  cv::Mat image = v4r::ConvertPCLCloud2Image(*input);
  // Draw a green box around the object
  for (size_t i = 0; i < detections.size (); ++i)
  {
    const LINEMODDetection & d = detections[i];
    if (d.score < score_threshold)
      continue;

    const pcl::SparseQuantizedMultiModTemplate & tmplt = linemod.getTemplate (d.template_id);
    std::cout << "d.template_id " << d.template_id << std::endl;
    int w = tmplt.region.width;
    int h = tmplt.region.height;
    cv::rectangle(image,  cv::Point(d.x, d.y),
          cv::Point(d.x + w , d.y + h),
          cv::Scalar(0,255,0));;
  }
  cv::imshow("detection", image);
  cv::waitKey(0);

  /*// Visualization code for testing purposes (requires libpng++)

  image.write("output.png");
  */
}

namespace po = boost::program_options;
namespace fs = boost::filesystem;

int main (int argc, char** argv) {
    std::string pathStr = ".";
    typedef pcl::PointXYZRGB PointT;
    std::string mask = "";
    int valid_model = 0;
    std::string template_filename = "";
    std::string class_name = "";

    po::options_description desc("Single-View Object Instance Recognizer\n======================================\n**Allowed options");
    desc.add_options()
        ("help,h", "produce help message")
        ("template,t", po::value(&template_filename)->required() , "Template filename generated from saveTemapltes")
        ("class,c", po::value(&class_name) , "If present will only look into one class")
        ("path,p",po::value(&pathStr)->required(),"Path to operate on")

            ;

    po::variables_map vm;
    po::parsed_options parsed = po::command_line_parser(argc, argv).options(desc).allow_unregistered().run();
    po::store(parsed, vm);
    if (vm.count("help")) { std::cout << desc << std::endl; }
    try { po::notify(vm); }
    catch(std::exception& e) { std::cerr << "Error: " << e.what() << std::endl << std::endl << desc << std::endl;  }

    // Gather list of pcd
    std::vector<fs::path> filesPath;
    {
        auto it = fs::recursive_directory_iterator(pathStr);
        auto end = fs::recursive_directory_iterator();

        for(; it != end; ++it)
        {
            fs::path path = *it;
            if(fs::is_directory(path) && fs::exists(path / "frame.pcd"))
                filesPath.push_back(fs::canonical(path));
        }
    }
     std::cout << "Loading model: " << template_filename << std::endl;
    linemod.loadTemplates(template_filename.c_str());
    for (int i = 0; i < filesPath.size(); i++){

        std::string maskFilename = "mask_"+class_name+".png";
        std::cout << "Recognizing file " << filesPath[i].string() << std::endl;
        if(!class_name.empty()){
            if(!fs::is_directory(filesPath[i]) || !fs::exists(filesPath[i] / maskFilename)){
                std::cout << "Class " << mask << " is not present " << std::endl;
                continue;
            }
        }
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr input(new pcl::PointCloud<pcl::PointXYZRGB>);
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr whole_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);

        pcl::io::loadPCDFile((filesPath[i] / "frame.pcd").string(), *input);

        // Load the specified templates and match them to the provided input cloud
        compute (input);

    }

}


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <mc_rbdyn/RobotLoader.h>
#include <mc_rbdyn/configuration_io.h>
#include <mc_tasks/AddRemoveContactTask.h>
#include <mc_tasks/BSplineTrajectoryTask.h>
#include <mc_tasks/CoMTask.h>
#include <mc_tasks/ComplianceTask.h>
#include <mc_tasks/ExactCubicTrajectoryTask.h>
#include <mc_tasks/GazeTask.h>
#include <mc_tasks/MetaTaskLoader.h>
#include <mc_tasks/PositionBasedVisServoTask.h>
#include <mc_tasks/PostureTask.h>
#include <mc_tasks/RelativeEndEffectorTask.h>
#include <mc_tasks/SurfaceTransformTask.h>
#include <mc_tasks/VectorOrientationTask.h>

#include <boost/mpl/list.hpp>
#include <boost/test/unit_test.hpp>

#include "utils.h"

static bool configured = configureRobotLoader();
/* Create Robots with one robot and an environment for the purpose of the test */
static auto rm = mc_rbdyn::RobotLoader::get_robot_module("JVRC1");
static auto em =
    mc_rbdyn::RobotLoader::get_robot_module("env", std::string(mc_rtc::MC_ENV_DESCRIPTION_PATH), std::string("ground"));
static auto robots = mc_rbdyn::loadRobotAndEnv(*rm, *em);
static mc_solver::QPSolver solver(robots, 0.005);

template<typename T>
struct fail : public std::false_type
{
};

template<typename T>
struct TaskTester
{
  static_assert(fail<T>::value, "This should be specialized");
  mc_tasks::MetaTaskPtr make_ref()
  {
    return nullptr;
  }

  std::string json()
  {
    return "";
  }

  void check(const mc_tasks::MetaTaskPtr & /*ref*/, const mc_tasks::MetaTaskPtr & /*loaded*/) {}
};

template<>
struct TaskTester<mc_tasks::CoMTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto ret = std::make_shared<mc_tasks::CoMTask>(*robots, 0, stiffness, weight);
    ret->com(com);
    return ret;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "com");
    config.add("robotIndex", 0);
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("com", com);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::CoMTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::CoMTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
    BOOST_CHECK(ref->com().isApprox(loaded->com(), 1e-9));
  }

  Eigen::Vector3d com = Eigen::Vector3d::Random();
  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
};

#define AddRemoveContactTaskTester(T, typeStr)                                              \
  template<>                                                                                \
  struct TaskTester<T>                                                                      \
  {                                                                                \
    mc_tasks::MetaTaskPtr make_ref()                                                        \
    {                                                                                \
      auto ret = std::make_shared<T>(solver, contact, speed, stiffness, weight);            \
      return ret;                                                                           \
    }                                                                                \
                                                                                \
    std::string json()                                                                      \
    {                                                                                \
      mc_rtc::Configuration config;                                                         \
      config.add("type", typeStr);                                                          \
      config.add("contact", contact);                                                       \
      config.add("stiffness", stiffness);                                                   \
      config.add("weight", weight);                                                         \
      config.add("speed", speed);                                                           \
      auto ret = getTmpFile();                                                              \
      config.save(ret);                                                                     \
      return ret;                                                                           \
    }                                                                                \
                                                                                \
    void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p) \
    {                                                                                \
      auto ref = std::dynamic_pointer_cast<T>(ref_p);                                       \
      auto loaded = std::dynamic_pointer_cast<T>(loaded_p);                                 \
      BOOST_REQUIRE(ref);                                                                   \
      BOOST_REQUIRE(loaded);                                                                \
      BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);                       \
      BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);                             \
      BOOST_CHECK_CLOSE(ref->speed(), loaded->speed(), 1e-6);                               \
      auto ref_surf = ref->robotSurf;                                                       \
      auto loaded_surf = loaded->robotSurf;                                                 \
      BOOST_CHECK(ref_surf->type() == loaded_surf->type());                                 \
      BOOST_CHECK(ref_surf->name() == loaded_surf->name());                                 \
      BOOST_CHECK(ref->bodyId == loaded->bodyId);                                           \
      BOOST_CHECK(ref->robotBodyIndex == loaded->robotBodyIndex);                           \
      BOOST_CHECK(ref->targetTf == loaded->targetTf);                                       \
    }                                                                                \
                                                                                \
    mc_rbdyn::Contact contact = mc_rbdyn::Contact(*robots, "LeftFoot", "AllGround");        \
    double speed = fabs(rnd());                                                             \
    double stiffness = fabs(rnd());                                                         \
    double weight = fabs(rnd());                                                            \
  };
AddRemoveContactTaskTester(mc_tasks::AddContactTask, "addContact")
    AddRemoveContactTaskTester(mc_tasks::RemoveContactTask, "removeContact")

        template<>
        struct TaskTester<mc_tasks::force::ComplianceTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto t = std::shared_ptr<mc_tasks::force::ComplianceTask>(
        new mc_tasks::force::ComplianceTask(*robots, 0, "R_WRIST_Y_S", solver.dt(), dof, stiffness, weight, forceThresh,
                                            torqueThresh, forceGain, torqueGain));
    t->setTargetWrench(wrench);
    return t;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "compliance");
    config.add("robotIndex", 0);
    config.add("body", "R_WRIST_Y_S");
    config.add("dof", dof);
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("forceThresh", forceThresh);
    config.add("torqueThresh", torqueThresh);
    config.add("forceGain", forceGain);
    config.add("torqueGain", torqueGain);
    config.add("wrench", wrench);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::force::ComplianceTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::force::ComplianceTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK(ref->dof().isApprox(loaded->dof(), 1e-6));
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
    BOOST_CHECK_CLOSE(ref->forceThresh(), loaded->forceThresh(), 1e-6);
    BOOST_CHECK_CLOSE(ref->torqueThresh(), loaded->torqueThresh(), 1e-6);
    BOOST_CHECK_CLOSE(ref->forceGain().first, loaded->forceGain().first, 1e-6);
    BOOST_CHECK_CLOSE(ref->forceGain().second, loaded->forceGain().second, 1e-6);
    BOOST_CHECK_CLOSE(ref->torqueGain().first, loaded->torqueGain().first, 1e-6);
    BOOST_CHECK_CLOSE(ref->torqueGain().second, loaded->torqueGain().second, 1e-6);
    BOOST_CHECK(ref->getTargetWrench().vector().isApprox(loaded->getTargetWrench().vector()));
  }

  Eigen::Matrix6d dof = Eigen::Matrix6d::Random();
  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  double forceThresh = fabs(rnd());
  double torqueThresh = fabs(rnd());
  std::pair<double, double> forceGain = {fabs(rnd()), fabs(rnd())};
  std::pair<double, double> torqueGain = {fabs(rnd()), fabs(rnd())};
  sva::ForceVecd wrench = {Eigen::Vector6d::Random()};
};

template<>
struct TaskTester<mc_tasks::OrientationTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto t = std::make_shared<mc_tasks::OrientationTask>("R_WRIST_Y_S", *robots, 0, stiffness, weight);
    t->orientation(ori);
    return t;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "orientation");
    config.add("robotIndex", 0);
    config.add("body", "R_WRIST_Y_S");
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("orientation", ori);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::OrientationTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::OrientationTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
    BOOST_CHECK(ref->orientation().isApprox(loaded->orientation(), 1e-6));
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  Eigen::Matrix3d ori = Eigen::Matrix3d::Random();
};

template<>
struct TaskTester<mc_tasks::PositionTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto t = std::make_shared<mc_tasks::PositionTask>("R_WRIST_Y_S", *robots, 0, stiffness, weight);
    t->position(pos);
    return t;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "position");
    config.add("robotIndex", 0);
    config.add("body", "R_WRIST_Y_S");
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("position", pos);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::PositionTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::PositionTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
    BOOST_CHECK(ref->position().isApprox(loaded->position(), 1e-6));
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  Eigen::Vector3d pos = Eigen::Vector3d::Random();
};

template<>
struct TaskTester<mc_tasks::EndEffectorTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto t = std::make_shared<mc_tasks::EndEffectorTask>("R_WRIST_Y_S", *robots, 0, stiffness, weight);
    t->set_ef_pose({ori, pos});
    return t;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "body6d");
    config.add("robotIndex", 0);
    config.add("body", "R_WRIST_Y_S");
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("position", pos);
    config.add("orientation", ori);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::EndEffectorTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::EndEffectorTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->positionTask->stiffness(), loaded->positionTask->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->positionTask->weight(), loaded->positionTask->weight(), 1e-6);
    BOOST_CHECK(ref->positionTask->position().isApprox(loaded->positionTask->position(), 1e-6));
    BOOST_CHECK_CLOSE(ref->orientationTask->stiffness(), loaded->orientationTask->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->orientationTask->weight(), loaded->orientationTask->weight(), 1e-6);
    BOOST_CHECK(ref->orientationTask->orientation().isApprox(loaded->orientationTask->orientation(), 1e-6));
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  Eigen::Matrix3d ori = Eigen::Matrix3d::Random();
  Eigen::Vector3d pos = Eigen::Vector3d::Random();
};

template<>
struct TaskTester<mc_tasks::RelativeEndEffectorTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto t = std::make_shared<mc_tasks::RelativeEndEffectorTask>("R_WRIST_Y_S", *robots, 0, "L_WRIST_Y_S", stiffness,
                                                                 weight);
    t->set_ef_pose({ori, pos});
    return t;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "relBody6d");
    config.add("robotIndex", 0);
    config.add("body", "R_WRIST_Y_S");
    config.add("relBody", "L_WRIST_Y_S");
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("position", pos);
    config.add("orientation", ori);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::RelativeEndEffectorTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::RelativeEndEffectorTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->positionTask->stiffness(), loaded->positionTask->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->positionTask->weight(), loaded->positionTask->weight(), 1e-6);
    BOOST_CHECK_CLOSE(ref->orientationTask->stiffness(), loaded->orientationTask->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->orientationTask->weight(), loaded->orientationTask->weight(), 1e-6);
    BOOST_CHECK(ref->get_ef_pose().rotation().isApprox(loaded->get_ef_pose().rotation(), 1e-6));
    BOOST_CHECK(ref->get_ef_pose().translation().isApprox(loaded->get_ef_pose().translation(), 1e-6));
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  Eigen::Matrix3d ori = Eigen::Matrix3d::Random();
  Eigen::Vector3d pos = Eigen::Vector3d::Random();
};

template<>
struct TaskTester<mc_tasks::GazeTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    // If the last element of the point3d is Zero, this would cause a division
    // by zero. Checks that the constructor prevents it by throwing an
    // exception.
    BOOST_CHECK_THROW(std::make_shared<mc_tasks::GazeTask>("NECK_P_S", Eigen::Vector3d::Zero(), X_b_gaze, *robots, 0,
                                                           stiffness, weight),
                      std::logic_error);

    auto ret = std::make_shared<mc_tasks::GazeTask>("NECK_P_S", Eigen::Vector3d{0, 0, 1}, X_b_gaze, *robots, 0,
                                                    stiffness, weight);
    return ret;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "gaze");
    config.add("robotIndex", 0);
    config.add("body", "NECK_P_S");
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("X_b_gaze", X_b_gaze);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::GazeTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::GazeTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  sva::PTransformd X_b_gaze = random_pt();
};

template<>
struct TaskTester<mc_tasks::PositionBasedVisServoTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto ret = std::make_shared<mc_tasks::PositionBasedVisServoTask>("NECK_P_S", sva::PTransformd::Identity(), X_b_s,
                                                                     *robots, 0, stiffness, weight);
    return ret;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "pbvs");
    config.add("robotIndex", 0);
    config.add("surface", "LeftFoot");
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::PositionBasedVisServoTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::PositionBasedVisServoTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  sva::PTransformd X_b_s = random_pt();
};

template<>
struct TaskTester<mc_tasks::SurfaceTransformTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto ret = std::make_shared<mc_tasks::SurfaceTransformTask>("LeftFoot", *robots, 0, stiffness, weight);
    ret->target(target);
    return ret;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "surfaceTransform");
    config.add("robotIndex", 0);
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("target", target);
    config.add("surface", "LeftFoot");
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::SurfaceTransformTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::SurfaceTransformTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
    BOOST_CHECK(ref->surface() == loaded->surface());
    BOOST_CHECK(ref->target().rotation().isApprox(loaded->target().rotation(), 1e-6));
    BOOST_CHECK(ref->target().translation().isApprox(loaded->target().translation(), 1e-6));
  }

  sva::PTransformd target = random_pt();
  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
};

template<>
struct TaskTester<mc_tasks::VectorOrientationTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto ret = std::make_shared<mc_tasks::VectorOrientationTask>("R_WRIST_Y_S", bodyVector, targetVector, *robots, 0,
                                                                 stiffness, weight);
    return ret;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "vectorOrientation");
    config.add("body", "R_WRIST_Y_S");
    config.add("bodyVector", bodyVector);
    config.add("targetVector", targetVector);
    config.add("robotIndex", 0);
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::VectorOrientationTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::VectorOrientationTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
    BOOST_CHECK(ref->body() == loaded->body());
    BOOST_CHECK(ref->bodyVector().isApprox(loaded->bodyVector()));
  }

  Eigen::Vector3d bodyVector = Eigen::Vector3d::Random();
  Eigen::Vector3d targetVector = Eigen::Vector3d::Random();
  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
};

template<>
struct TaskTester<mc_tasks::PostureTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto ret = std::make_shared<mc_tasks::PostureTask>(solver, 0, stiffness, weight);
    return ret;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "posture");
    config.add("robotIndex", 0);
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::PostureTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::PostureTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
};

template<>
struct TaskTester<mc_tasks::BSplineTrajectoryTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto ret = std::make_shared<mc_tasks::BSplineTrajectoryTask>(*robots, 0, "LeftFoot", d, stiffness, weight, target);
    return ret;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "bspline_trajectory");
    config.add("robotIndex", 0);
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("duration", d);
    config.add("surface", "LeftFoot");
    config.add("target", target);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::BSplineTrajectoryTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::BSplineTrajectoryTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
    BOOST_CHECK_CLOSE(ref->duration(), loaded->duration(), 1e-6);
    BOOST_CHECK(ref->target().rotation().isApprox(loaded->target().rotation(), 1e-6));
    BOOST_CHECK(ref->target().translation().isApprox(loaded->target().translation(), 1e-6));
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  double d = fabs(rnd());
  sva::PTransformd target = random_pt();
};

template<>
struct TaskTester<mc_tasks::ExactCubicTrajectoryTask>
{
  mc_tasks::MetaTaskPtr make_ref()
  {
    auto ret =
        std::make_shared<mc_tasks::ExactCubicTrajectoryTask>(*robots, 0, "LeftFoot", d, stiffness, weight, target);
    return ret;
  }

  std::string json()
  {
    mc_rtc::Configuration config;
    config.add("type", "exact_cubic_trajectory");
    config.add("robotIndex", 0);
    config.add("stiffness", stiffness);
    config.add("weight", weight);
    config.add("duration", d);
    config.add("surface", "LeftFoot");
    config.add("target", target);
    auto ret = getTmpFile();
    config.save(ret);
    return ret;
  }

  void check(const mc_tasks::MetaTaskPtr & ref_p, const mc_tasks::MetaTaskPtr & loaded_p)
  {
    auto ref = std::dynamic_pointer_cast<mc_tasks::ExactCubicTrajectoryTask>(ref_p);
    auto loaded = std::dynamic_pointer_cast<mc_tasks::ExactCubicTrajectoryTask>(loaded_p);
    BOOST_REQUIRE(ref);
    BOOST_REQUIRE(loaded);
    BOOST_CHECK_CLOSE(ref->stiffness(), loaded->stiffness(), 1e-6);
    BOOST_CHECK_CLOSE(ref->weight(), loaded->weight(), 1e-6);
    BOOST_CHECK_CLOSE(ref->duration(), loaded->duration(), 1e-6);
    BOOST_CHECK(ref->target().rotation().isApprox(loaded->target().rotation(), 1e-6));
    BOOST_CHECK(ref->target().translation().isApprox(loaded->target().translation(), 1e-6));
  }

  double stiffness = fabs(rnd());
  double weight = fabs(rnd());
  double d = fabs(rnd());
  sva::PTransformd target = random_pt();
};

typedef boost::mpl::list<mc_tasks::CoMTask,
                         mc_tasks::AddContactTask,
                         mc_tasks::RemoveContactTask,
                         mc_tasks::force::ComplianceTask,
                         mc_tasks::OrientationTask,
                         mc_tasks::PositionTask,
                         mc_tasks::EndEffectorTask,
                         mc_tasks::RelativeEndEffectorTask,
                         mc_tasks::GazeTask,
                         mc_tasks::PositionBasedVisServoTask,
                         mc_tasks::SurfaceTransformTask,
                         mc_tasks::VectorOrientationTask,
                         mc_tasks::PostureTask,
                         mc_tasks::BSplineTrajectoryTask,
                         mc_tasks::ExactCubicTrajectoryTask>
    test_types;

BOOST_AUTO_TEST_CASE_TEMPLATE(TestMetaTaskLoader, T, test_types)
{
  auto tester = TaskTester<T>();
  auto ref = tester.make_ref();
  auto conf = tester.json();
  auto loaded = mc_tasks::MetaTaskLoader::load(solver, conf);
  tester.check(ref, loaded);
  bfs::remove(conf);
}


// Copyright (c) 2015-2020 The Bitcoin Core developers
// Copyright (c) 2013-2021 The Riecoin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <chain.h>
#include <chainparams.h>
#include <pow.h>
#include <test/util/setup_common.h>

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(pow_tests, BasicTestingSetup)

// TODO: rewrite the tests for PoW Version 1

/* Test calculation of next difficulty target with no constraints applying */
BOOST_AUTO_TEST_CASE(get_next_work)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    int64_t nLastRetargetTime = 1435639430; // Block #287712
    CBlockIndex pindexLast;
    pindexLast.nHeight = 287999;
    pindexLast.nTime = 1435676461;  // Block #287999
    pindexLast.nBits = 0x0205d900; // 1497
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, nLastRetargetTime, chainParams->GetConsensus()), 0x0205f200); // 1522
}

/* Test the constraint on the upper bound for next work */
BOOST_AUTO_TEST_CASE(get_next_work_pow_limit)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    int64_t nLastRetargetTime = 1577836800;
    CBlockIndex pindexLast;
    pindexLast.nHeight = 287;
    pindexLast.nTime = nLastRetargetTime + 288*150*2;
    pindexLast.nBits = 0x02013000; // 304
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, nLastRetargetTime, chainParams->GetConsensus()), 0x02013000); // 304
}

/* Test the constraint on the lower bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_lower_limit_actual)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    int64_t nLastRetargetTime = 1577836800;
    CBlockIndex pindexLast;
    pindexLast.nHeight = 1151; // Note that the bound is not applied for the first 3 adjustments
    pindexLast.nTime = nLastRetargetTime + 288*150/5; // Limited to >= 288*150/4
    pindexLast.nBits = 0x02064000; // 1600
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, nLastRetargetTime, chainParams->GetConsensus()), 0x02074a00); // 1866
}

/* Test the constraint on the upper bound for actual time taken */
BOOST_AUTO_TEST_CASE(get_next_work_upper_limit_actual)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    int64_t nLastRetargetTime = 1577836800;
    CBlockIndex pindexLast;
    pindexLast.nHeight = 1151; // Note that the bound is not applied for the 3 first adjustments
    pindexLast.nTime = nLastRetargetTime + 5*288*150; // Limited to <= 4*288*150
    pindexLast.nBits = 0x02064000; // 1600
    BOOST_CHECK_EQUAL(CalculateNextWorkRequired(&pindexLast, nLastRetargetTime, chainParams->GetConsensus()), 0x02055b00); // 1371
}

BOOST_AUTO_TEST_CASE(CheckProofOfWork_test_negative_target)
{
    const auto consensus = CreateChainParams(*m_node.args, CBaseChainParams::REGTEST)->GetConsensus();
    uint256 hash, offset;
    unsigned int nBits(arith_uint256{304}.GetCompact(true));
    hash.SetHex("0x0");
    offset.SetHex("0x65"); // 2^303 + 101 is prime
    BOOST_CHECK(!CheckProofOfWork(hash, nBits, offset, consensus));
}

BOOST_AUTO_TEST_CASE(CheckProofOfWork_test_overflow_target)
{
    const auto consensus = CreateChainParams(*m_node.args, CBaseChainParams::REGTEST)->GetConsensus();
    uint256 hash, offset;
    unsigned int nBits = ~0x00800000;
    hash.SetHex("0x0");
    offset.SetHex("0xaf"); // 2^264 + 175 is prime
    BOOST_CHECK(!CheckProofOfWork(hash, nBits, offset, consensus));
}

BOOST_AUTO_TEST_CASE(CheckProofOfWork_test_too_easy_target)
{
    const auto consensus = CreateChainParams(*m_node.args, CBaseChainParams::REGTEST)->GetConsensus();
    uint256 hash, offset;
    unsigned int nBits(33632000); // 303
    hash.SetHex("0x0");
    offset.SetHex("0x133"); // 2^302 + 307 is prime
    BOOST_CHECK(!CheckProofOfWork(hash, nBits, offset, consensus));
}

BOOST_AUTO_TEST_CASE(CheckProofOfWork_test_biger_hash_than_target)
{
    const auto consensus = CreateChainParams(*m_node.args, CBaseChainParams::MAIN)->GetConsensus();
    uint256 hash, offset;
    unsigned int nBits(33632256); // 304
    hash.SetHex("0x0");
    offset.SetHex("0x0b770d4f166f50f63d6001df19f113cf68f79133439a90dc59c99b22a69dd8c3"); // 2^303 + offset is a prime sextuplet, but offset >= 2^39
    BOOST_CHECK(!CheckProofOfWork(hash, nBits, offset, consensus));
}

BOOST_AUTO_TEST_CASE(CheckProofOfWork_test_zero_target)
{
    const auto consensus = CreateChainParams(*m_node.args, CBaseChainParams::REGTEST)->GetConsensus();
    uint256 hash, offset;
    unsigned int nBits;
    arith_uint256 hash_arith{0};
    nBits = hash_arith.GetCompact();
    hash = ArithToUint256(hash_arith);
    offset.SetHex("0xaf"); // 2^264 + 175 is prime
    BOOST_CHECK(!CheckProofOfWork(hash, nBits, offset, consensus));
}

BOOST_AUTO_TEST_CASE(GetBlockProofEquivalentTime_test)
{
    const auto chainParams = CreateChainParams(*m_node.args, CBaseChainParams::MAIN);
    std::vector<CBlockIndex> blocks(10000);
    for (int i = 0; i < 10000; i++) {
        blocks[i].pprev = i ? &blocks[i - 1] : nullptr;
        blocks[i].nHeight = i;
        blocks[i].nTime = 1577836800 + i * chainParams->GetConsensus().nPowTargetSpacing;
        blocks[i].nBits = 0x02013000;
        blocks[i].nChainWork = i ? blocks[i - 1].nChainWork + GetBlockProof(blocks[i - 1]) : arith_uint256(0);
    }

    for (int j = 0; j < 1000; j++) {
        CBlockIndex *p1 = &blocks[InsecureRandRange(10000)];
        CBlockIndex *p2 = &blocks[InsecureRandRange(10000)];
        CBlockIndex *p3 = &blocks[InsecureRandRange(10000)];

        int64_t tdiff = GetBlockProofEquivalentTime(*p1, *p2, *p3, chainParams->GetConsensus());
        BOOST_CHECK_EQUAL(tdiff, p1->GetBlockTime() - p2->GetBlockTime());
    }
}

void sanity_check_chainparams(const ArgsManager& args, std::string chainName)
{
    const auto chainParams = CreateChainParams(args, chainName);
    const auto consensus = chainParams->GetConsensus();

    // hash genesis is correct
    BOOST_CHECK_EQUAL(consensus.hashGenesisBlock, chainParams->GenesisBlock().GetHash());

    // target timespan is an even multiple of spacing
    BOOST_CHECK_EQUAL(consensus.nPowTargetTimespan % consensus.nPowTargetSpacing, 0);
}

BOOST_AUTO_TEST_CASE(ChainParams_MAIN_sanity)
{
    sanity_check_chainparams(*m_node.args, CBaseChainParams::MAIN);
}

BOOST_AUTO_TEST_CASE(ChainParams_REGTEST_sanity)
{
    sanity_check_chainparams(*m_node.args, CBaseChainParams::REGTEST);
}

BOOST_AUTO_TEST_CASE(ChainParams_TESTNET_sanity)
{
    sanity_check_chainparams(*m_node.args, CBaseChainParams::TESTNET);
}

BOOST_AUTO_TEST_CASE(ChainParams_SIGNET_sanity)
{
    sanity_check_chainparams(*m_node.args, CBaseChainParams::SIGNET);
}

BOOST_AUTO_TEST_SUITE_END()


#ifndef EFANNA_HASHING_INDEX_H_
#define EFANNA_HASHING_INDEX_H_

#include "algorithm/base_index.hpp"
#include <boost/dynamic_bitset.hpp>
#include <time.h>
//for Debug
#include <iostream>
#include <fstream>
#include <string>
#include <unordered_map>
#include <map>
#include <sstream>
#include <set>
//#define MAX_RADIUS 6

namespace efanna{
struct HASHINGIndexParams : public IndexParams
{
    HASHINGIndexParams(int codelen, int TableNum,int UpperBits, int HashRadius, char*& BaseCodeFile, char*& QueryCodeFile, int codelenShift = 0)
    {
      init_index_type = HASHING;
      ValueType len;
      len.int_val = codelen;
      extra_params.insert(std::make_pair("codelen",len));
      ValueType nTab;
      nTab.int_val = TableNum;
      extra_params.insert(std::make_pair("tablenum",nTab));
      ValueType upb;
      upb.int_val = UpperBits;
      extra_params.insert(std::make_pair("upbits",upb));
      ValueType radius;
      radius.int_val = HashRadius;
      extra_params.insert(std::make_pair("radius",radius));
      ValueType bcf;
      bcf.str_pt = BaseCodeFile;
      extra_params.insert(std::make_pair("bcfile",bcf));
      ValueType qcf;
      qcf.str_pt = QueryCodeFile;
      extra_params.insert(std::make_pair("qcfile",qcf));
      ValueType lenShift;
      lenShift.int_val = codelenShift;
      extra_params.insert(std::make_pair("lenshift",lenShift));
    }
};

template <typename DataType>
class HASHINGIndex : public InitIndex<DataType>
{
  public:

    typedef InitIndex<DataType> BaseClass;

    typedef std::vector<unsigned int> Codes;
    typedef std::unordered_map<unsigned int, std::vector<unsigned int> > HashBucket;
    typedef std::vector<HashBucket> HashTable;

    typedef std::vector<unsigned long> Codes64;
    typedef std::unordered_map<unsigned long, std::vector<unsigned int> > HashBucket64;
    typedef std::vector<HashBucket64> HashTable64;

    HASHINGIndex(const Matrix<DataType>& dataset, const Distance<DataType>* d, const IndexParams& params = HASHINGIndexParams(0,NULL,NULL)) :
      BaseClass(dataset,d,params)
    {
      std::cout<<"HASHING initial, max code length : 64" <<std::endl;

      ExtraParamsMap::const_iterator it = params_.extra_params.find("codelen");
      if(it != params_.extra_params.end()){
        codelength = (it->second).int_val;
        std::cout << "use  "<<codelength<< " bit code"<< std::endl;
      }
      else{
        std::cout << "error: no code length setting" << std::endl;
      }

      it = params_.extra_params.find("lenshift");
      if(it != params_.extra_params.end()){
        codelengthshift = (it->second).int_val;
        int actuallen = codelength - codelengthshift;
        if(actuallen > 0){
          std::cout << "Actually use  "<< actuallen<< " bit code"<< std::endl;
        }else{
          std::cout << "lenShift error: could not be larger than the code length!  "<<  std::endl;
        }
      }
      else{
        codelengthshift = 0;
      }

      it = params_.extra_params.find("tablenum");
      if(it != params_.extra_params.end()){
        tablenum = (it->second).int_val;
        std::cout << "use  "<<tablenum<< " hashtables"<< std::endl;
      }
      else{
        std::cout << "error: no table number setting" << std::endl;
      }

      it = params_.extra_params.find("upbits");
      if(it != params_.extra_params.end()){
        upbits = (it->second).int_val;
        std::cout << "use upper "<<upbits<< " bits as first level index of hashtable"<< std::endl;
        std::cout << "use lower "<<codelength - codelengthshift - upbits<< " bits as second level index of hashtable"<< std::endl;
      }
      else{
        std::cout << "error: no upper bits number setting" << std::endl;
      }

      if(upbits >= codelength-codelengthshift){
        std::cout << "upbits should be smaller than the actual codelength!" << std::endl;
        return;
      }

      int actuallen = codelength - codelengthshift;
      it = params_.extra_params.find("radius");
      if(it != params_.extra_params.end()){
        radius = (it->second).int_val;
        if(actuallen<=32){
          if(radius > 13){
            std::cout << "radius greater than 13 not supported yet!" << std::endl;
            radius = 13;
          }
        }else if(actuallen<=36){
          if(radius > 11){
            std::cout << "radius greater than 11 not supported yet!" << std::endl;
            radius = 11;
          }
        }else if(actuallen<=40){
          if(radius > 10){
            std::cout << "radius greater than 10 not supported yet!" << std::endl;
            radius = 10;
          }
        }else if(actuallen<=48){
          if(radius > 9){
            std::cout << "radius greater than 9 not supported yet!" << std::endl;
            radius = 9;
          }
        }else if(actuallen<=60){
          if(radius > 8){
            std::cout << "radius greater than 8 not supported yet!" << std::endl;
            radius = 8;
          }
        }else{ //actuallen<=64
          if(radius > 7){
            std::cout << "radius greater than 7 not supported yet!" << std::endl;
            radius = 7;
          }
        }
        std::cout << "search hamming radius "<<radius<< std::endl;
      }
      else{
        std::cout << "error: no radius number setting" << std::endl;
      }

      it = params_.extra_params.find("bcfile");
      if(it != params_.extra_params.end()){
        char* fpath = (it->second).str_pt;
        std::string str(fpath);
        std::cout << "Loading base code from " << str << std::endl;

        if (codelength <= 32 ){
          LoadCode32(fpath, BaseCode);
        }else if(codelength <= 64 ){
          LoadCode64(fpath, BaseCode64);
        }else{
          std::cout<<"code length not supported yet!"<<std::endl;
        }

        std::cout << "code length is "<<codelength<<std::endl;
      }
      else{
        std::cout << "error: no base code file" << std::endl;
      }

      it = params_.extra_params.find("qcfile");
      if(it != params_.extra_params.end()){
        char* fpath = (it->second).str_pt;
        std::string str(fpath);
        std::cout << "Loading query code from " << str << std::endl;

        if (codelength <= 32 ){
          LoadCode32(fpath, QueryCode);
        }else if(codelength <= 64 ){
          LoadCode64(fpath, QueryCode64);
        }else{
          std::cout<<"code length not supported yet!"<<std::endl;
        }

        std::cout << "code length is "<<codelength<<std::endl;
      }
      else{
        std::cout << "error: no query code file" << std::endl;
      }

    }

    /**
     * Builds the index
     */

    void LoadCode32(char* filename, std::vector<Codes>& baseAll){
      if (tablenum < 1){
        std::cout<<"Total hash table num error! "<<std::endl;
      }

      int actuallen = codelength-codelengthshift;

      unsigned int  maxValue = 1;
      for(int i=1;i<actuallen;i++){
        maxValue = maxValue << 1;
        maxValue ++;
      }

      std::stringstream ss;
      for(int j = 0; j < tablenum; j++){

        ss << filename << "_" << j+1 ;
        std::string codeFile;
        ss >> codeFile;
        ss.clear();

        std::ifstream in(codeFile.c_str(), std::ios::binary);
        if(!in.is_open()){std::cout<<"open file " << filename <<" error"<< std::endl;return;}

        int codeNum;
        in.read((char*)&codeNum,4);
        if (codeNum != 1){
          std::cout<<"Codefile  "<< j << " error!"<<std::endl;
        }

        in.read((char*)&codelength,4);
        //std::cout<<"codelength: "<<codelength<<std::endl;

        int num;
        in.read((char*)&num,4);
        //std::cout<<"ponit num: "<<num<<std::endl;

        Codes base;
        for(int i = 0; i < num; i++){
          unsigned int codetmp;
          in.read((char*)&codetmp,4);
          codetmp = codetmp >> codelengthshift;
          if (codetmp > maxValue){
            std::cout<<"codetmp: "<< codetmp <<std::endl;
            std::cout<<"codelengthshift: "<<codelengthshift<<std::endl;
            std::cout<<"codefile  "<< codeFile << " error! Exceed maximum value"<<std::endl;
            in.close();
            return;
          }
          base.push_back(codetmp);
        }
        baseAll.push_back(base);

        in.close();
      }
    }

    void LoadCode64(char* filename, std::vector<Codes64>& baseAll){
      if (tablenum < 1){
        std::cout<<"Total hash table num error! "<<std::endl;
      }

      int actuallen = codelength-codelengthshift;

      unsigned long  maxValue = 1;
      for(int i=1;i<actuallen;i++){
        maxValue = maxValue << 1;
        maxValue ++;
      }

      std::stringstream ss;
      for(int j = 0; j < tablenum; j++){

        ss << filename << "_" << j+1 ;
        std::string codeFile;
        ss >> codeFile;
        ss.clear();

        std::ifstream in(codeFile.c_str(), std::ios::binary);
        if(!in.is_open()){std::cout<<"open file " << filename <<" error"<< std::endl;return;}

        int codeNum;
        in.read((char*)&codeNum,4);
        if (codeNum != 1){
          std::cout<<"Codefile  "<< j << " error!"<<std::endl;
        }

        in.read((char*)&codelength,4);
        //std::cout<<"codelength: "<<codelength<<std::endl;

        int num;
        in.read((char*)&num,4);
        //std::cout<<"ponit num: "<<num<<std::endl;

        Codes64 base;
        for(int i = 0; i < num; i++){
          unsigned long codetmp;
          in.read((char*)&codetmp,8);
          codetmp = codetmp >> codelengthshift;
          if (codetmp > maxValue){
            std::cout<<"codetmp: "<< codetmp <<std::endl;
            std::cout<<"codelengthshift: "<<codelengthshift<<std::endl;
            std::cout<<"codefile  "<< codeFile << " error! Exceed maximum value"<<std::endl;
            in.close();
            return;
          }

          base.push_back(codetmp);
        }
        baseAll.push_back(base);

        in.close();
      }
    }

    void BuildHashTable32(int upbits, int lowbits, std::vector<Codes>& baseAll ,std::vector<HashTable>& tbAll){

      for(size_t h=0; h < baseAll.size(); h++){
        Codes& base = baseAll[h];

        HashTable tb;
        for(int i = 0; i < (1 << upbits); i++){
          HashBucket emptyBucket;
          tb.push_back(emptyBucket);
        }

        for(size_t i = 0; i < base.size(); i ++){
          unsigned int idx1 = base[i] >> lowbits;
          unsigned int idx2 = base[i] - (idx1 << lowbits);
          if(tb[idx1].find(idx2) != tb[idx1].end()){
            tb[idx1][idx2].push_back(i);
          }else{
            std::vector<unsigned int> v;
            v.push_back(i);
            tb[idx1].insert(make_pair(idx2,v));
          }
        }
        tbAll.push_back(tb);
      }
    }

    void generateMask32(){
      //i = 0 means the origin code
      HammingBallMask.push_back(0);
      HammingRadius.push_back(HammingBallMask.size());

      if(radius>0){
        //radius 1
        for(int i = 0; i < codelength; i++){
          unsigned int mask = 1 << i;
          HammingBallMask.push_back(mask);
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>1){
        //radius 2
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            unsigned int mask = (1<<i) | (1<<j);
            HammingBallMask.push_back(mask);
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>2){
        //radius 3
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              unsigned int mask = (1<<i) | (1<<j) | (1<<k);
              HammingBallMask.push_back(mask);
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>3){
        //radius 4
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a);
                HammingBallMask.push_back(mask);
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>4){
        //radius 5
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b);
                  HammingBallMask.push_back(mask);
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>5){
        //radius 6
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b)| (1<<c);
                    HammingBallMask.push_back(mask);
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>6){
        //radius 7
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b)| (1<<c)| (1<<d);
                      HammingBallMask.push_back(mask);
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>7){
        //radius 8
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b)| (1<<c)| (1<<d)| (1<<e);
                        HammingBallMask.push_back(mask);
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>8){
        //radius 9
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        for(int f = e+1; f < codelength; f++){
                          unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b)| (1<<c)| (1<<d)| (1<<e)| (1<<f);
                          HammingBallMask.push_back(mask);
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>9){
        //radius 10
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        for(int f = e+1; f < codelength; f++){
                          for(int g = f+1; g < codelength; g++){
                            unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b)| (1<<c)| (1<<d)| (1<<e)| (1<<f)| (1<<g);
                            HammingBallMask.push_back(mask);
                          }
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>10){
        //radius 11
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        for(int f = e+1; f < codelength; f++){
                          for(int g = f+1; g < codelength; g++){
                            for(int h = g+1; h < codelength; h++){
                              unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b)| (1<<c)| (1<<d)| (1<<e)| (1<<f)| (1<<g)| (1<<h);
                              HammingBallMask.push_back(mask);
                            }
                          }
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>11){
        //radius 12
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        for(int f = e+1; f < codelength; f++){
                          for(int g = f+1; g < codelength; g++){
                            for(int h = g+1; h < codelength; h++){
                              for(int l = h+1; h < codelength; l++){
                                unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b)| (1<<c)| (1<<d)| (1<<e)| (1<<f)| (1<<g)| (1<<h)| (1<<l);
                                HammingBallMask.push_back(mask);
                              }
                            }
                          }
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

      if(radius>12){
        //radius 13
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        for(int f = e+1; f < codelength; f++){
                          for(int g = f+1; g < codelength; g++){
                            for(int h = g+1; h < codelength; h++){
                              for(int l = h+1; h < codelength; l++){
                                for(int m = l+1; m < codelength; m++){
                                  unsigned int mask = (1<<i) | (1<<j) | (1<<k)| (1<<a)| (1<<b)| (1<<c)| (1<<d)| (1<<e)| (1<<f)| (1<<g)| (1<<h)| (1<<l)| (1<<m);
                                  HammingBallMask.push_back(mask);
                                }
                              }
                            }
                          }
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask.size());
      }

    }

    void BuildHashTable64(int upbits, int lowbits, std::vector<Codes64>& baseAll ,std::vector<HashTable64>& tbAll){

      for(size_t h=0; h < baseAll.size(); h++){
        Codes64& base = baseAll[h];

        HashTable64 tb;
        for(int i = 0; i < (1 << upbits); i++){
          HashBucket64 emptyBucket;
          tb.push_back(emptyBucket);
        }

        for(size_t i = 0; i < base.size(); i ++){
          unsigned int idx1 = base[i] >> lowbits;
          unsigned long idx2 = base[i] - ((unsigned long)idx1 << lowbits);
          if(tb[idx1].find(idx2) != tb[idx1].end()){
            tb[idx1][idx2].push_back(i);
          }else{
            std::vector<unsigned int> v;
            v.push_back(i);
            tb[idx1].insert(make_pair(idx2,v));
          }
        }
        tbAll.push_back(tb);
      }
    }

    void generateMask64(){
      //i = 0 means the origin code
      HammingBallMask64.push_back(0);
      HammingRadius.push_back(HammingBallMask64.size());

      unsigned long One = 1;
      if(radius>0){
        //radius 1
        for(int i = 0; i < codelength; i++){
          unsigned long mask = One << i;
          HammingBallMask64.push_back(mask);
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>1){
        //radius 2
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            unsigned long mask = (One<<i) | (One<<j);
            HammingBallMask64.push_back(mask);
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>2){
        //radius 3
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              unsigned long mask = (One<<i) | (One<<j) | (One<<k);
              HammingBallMask64.push_back(mask);
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>3){
        //radius 4
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                unsigned long mask = (One<<i) | (One<<j) | (One<<k)| (One<<a);
                HammingBallMask64.push_back(mask);
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>4){
        //radius 5
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  unsigned long mask = (One<<i) | (One<<j) | (One<<k)| (One<<a)| (One<<b);
                  HammingBallMask64.push_back(mask);
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>5){
        //radius 6
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    unsigned long mask = (One<<i) | (One<<j) | (One<<k)| (One<<a)| (One<<b)| (One<<c);
                    HammingBallMask64.push_back(mask);
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>6){
        //radius 7
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      unsigned long mask = (One<<i) | (One<<j) | (One<<k)| (One<<a)| (One<<b)| (One<<c)| (One<<d);
                      HammingBallMask64.push_back(mask);
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>7){
        //radius 8
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        unsigned long mask = (One<<i) | (One<<j) | (One<<k)| (One<<a)| (One<<b)| (One<<c)| (One<<d)| (One<<e);
                        HammingBallMask64.push_back(mask);
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>8){
        //radius 9
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        for(int f = e+1; f < codelength; f++){
                          unsigned long mask = (One<<i) | (One<<j) | (One<<k)| (One<<a)| (One<<b)| (One<<c)| (One<<d)| (One<<e)| (One<<f);
                          HammingBallMask64.push_back(mask);
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>9){
        //radius 10
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        for(int f = e+1; f < codelength; f++){
                          for(int g = f+1; g < codelength; g++){
                            unsigned long mask = (One<<i) | (One<<j) | (One<<k)| (One<<a)| (One<<b)| (One<<c)| (One<<d)| (One<<e)| (One<<f)| (One<<g);
                            HammingBallMask64.push_back(mask);
                          }
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

      if(radius>10){
        //radius 11
        for(int i = 0; i < codelength; i++){
          for(int j = i+1; j < codelength; j++){
            for(int k = j+1; k < codelength; k++){
              for(int a = k+1; a < codelength; a++){
                for(int b = a+1; b < codelength; b++){
                  for(int c = b+1; c < codelength; c++){
                    for(int d = c+1; d < codelength; d++){
                      for(int e = d+1; e < codelength; e++){
                        for(int f = e+1; f < codelength; f++){
                          for(int g = f+1; g < codelength; g++){
                            for(int h = g+1; h < codelength; h++){
                              unsigned long mask = (One<<i) | (One<<j) | (One<<k)| (One<<a)| (One<<b)| (One<<c)| (One<<d)| (One<<e)| (One<<f)| (One<<g)| (One<<h);
                              HammingBallMask64.push_back(mask);
                            }
                          }
                        }
                      }
                    }
                  }
                }
              }
            }
          }
        }
        HammingRadius.push_back(HammingBallMask64.size());
      }

    }

    void buildIndexImpl()
    {
      std::cout<<"HASHING building hashing table"<<std::endl;

      if (codelength <= 32 ){
        codelength = codelength - codelengthshift;
        BuildHashTable32(upbits, codelength-upbits, BaseCode ,htb);
        generateMask32();
      }else if(codelength <= 64 ){
        codelength = codelength - codelengthshift;
        BuildHashTable64(upbits, codelength-upbits, BaseCode64 ,htb64);
        generateMask64();
      }else{
        std::cout<<"code length not supported yet!"<<std::endl;
      }
    }

    void getNeighbors(size_t K, const Matrix<DataType>& query){

      if(gs.size() != features_.get_rows()){
        if (codelength <= 32 ){
          getNeighbors32(K,query);
        }else if(codelength <= 64 ){
          getNeighbors64(K,query);
        }else{
          std::cout<<"code length not supported yet!"<<std::endl;
        }
      }else{
        switch(SP.search_method){
        case 0:
          if (codelength <= 32 ){
            getNeighborsIEH32_kgraph(K, query);
          }else if(codelength <= 64 ){
            getNeighborsIEH64_kgraph(K, query);
          }else{
            std::cout<<"code length not supported yet!"<<std::endl;
          }
          break;
        case 1:
          if (codelength <= 32 ){
            getNeighborsIEH32_nnexp(K, query);
          }else if(codelength <= 64 ){
            getNeighborsIEH64_nnexp(K, query);
          }else{
            std::cout<<"code length not supported yet!"<<std::endl;
          }
          break;
        default:
          std::cout<<"no such searching method"<<std::endl;
        }
      }

    }

    void getNeighbors32(size_t K, const Matrix<DataType>& query){
      int lowbits = codelength - upbits;

      unsigned int MaxCheck=HammingRadius[radius];
      std::cout<<"maxcheck : "<<MaxCheck<<std::endl;

      boost::dynamic_bitset<> tbflag(features_.get_rows(), false);

      nn_results.clear();

      VisitBucketNum.clear();
      VisitBucketNum.resize(radius+2);

      for(size_t cur = 0; cur < query.get_rows(); cur++){

        std::vector<unsigned int> pool(SP.search_init_num);
        unsigned int p = 0;
        tbflag.reset();

        unsigned int j = 0;
        for(; j < MaxCheck; j++){
          for(unsigned int h=0; h < QueryCode.size(); h++){
            unsigned int searchcode = QueryCode[h][cur] ^ HammingBallMask[j];
            unsigned int idx1 = searchcode >> lowbits;
            unsigned int idx2 = searchcode - (idx1 << lowbits);

            HashBucket::iterator bucket= htb[h][idx1].find(idx2);
            if(bucket != htb[h][idx1].end()){
              std::vector<unsigned int> vp = bucket->second;
              for(size_t k = 0; k < vp.size() && p < (unsigned int)SP.search_init_num; k++){
                if(tbflag.test(vp[k]))continue;

                tbflag.set(vp[k]);
                pool[p++]=(vp[k]);
              }
              if(p >= (unsigned int)SP.search_init_num)  break;
            }
            if(p >= (unsigned int)SP.search_init_num)  break;
          }
          if(p >= (unsigned int)SP.search_init_num)  break;
        }

        if(p < (unsigned int)SP.search_init_num){
          VisitBucketNum[radius+1]++;
        }else{
          for(int r=0;r<=radius;r++){
            if(j<=HammingRadius[r]){
              VisitBucketNum[r]++;
              break;
            }
          }
        }

        if (p<K){
          int base_n = features_.get_rows();
          while(p < K){
            unsigned int nn = rand() % base_n;
            if(tbflag.test(nn)) continue;
            tbflag.set(nn);
            pool[p++] = (nn);
          }

        }

        std::vector<std::pair<float,size_t>> result;
        for(unsigned int i=0; i<p;i++){
          result.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(pool[i]), features_.get_cols()),pool[i]));
        }
        std::partial_sort(result.begin(), result.begin() + K, result.end());

        std::vector<int> res;
        for(unsigned int j = 0; j < K; j++) res.push_back(result[j].second);
        nn_results.push_back(res);

      }
      //std::cout<<"bad query number:  " << VisitBucketNum[radius+1] << std::endl;
    }

    void getNeighbors64(size_t K, const Matrix<DataType>& query){
      int lowbits = codelength - upbits;

      unsigned int MaxCheck=HammingRadius[radius];
      std::cout<<"maxcheck : "<<MaxCheck<<std::endl;

      boost::dynamic_bitset<> tbflag(features_.get_rows(), false);

      nn_results.clear();

      VisitBucketNum.clear();
      VisitBucketNum.resize(radius+2);

      for(size_t cur = 0; cur < query.get_rows(); cur++){

        std::vector<unsigned int> pool(SP.search_init_num);
        unsigned int p = 0;
        tbflag.reset();

        unsigned int j = 0;
        for(; j < MaxCheck; j++){
          for(unsigned int h=0; h < QueryCode64.size(); h++){
            unsigned long searchcode = QueryCode64[h][cur] ^ HammingBallMask64[j];
            unsigned int idx1 = searchcode >> lowbits;
            unsigned long idx2 = searchcode - (( unsigned long)idx1 << lowbits);

            HashBucket64::iterator bucket= htb64[h][idx1].find(idx2);
            if(bucket != htb64[h][idx1].end()){
              std::vector<unsigned int> vp = bucket->second;
              for(size_t k = 0; k < vp.size() && p < (unsigned int)SP.search_init_num; k++){
                if(tbflag.test(vp[k]))continue;

                tbflag.set(vp[k]);
                pool[p++]=(vp[k]);
              }
              if(p >= (unsigned int)SP.search_init_num)  break;
            }
            if(p >= (unsigned int)SP.search_init_num)  break;
          }
          if(p >= (unsigned int)SP.search_init_num)  break;
        }

        if(p < (unsigned int)SP.search_init_num){
          VisitBucketNum[radius+1]++;
        }else{
          for(int r=0;r<=radius;r++){
            if(j<=HammingRadius[r]){
              VisitBucketNum[r]++;
              break;
            }
          }
        }

        if (p<K){
          int base_n = features_.get_rows();
          while(p < K){
            unsigned int nn = rand() % base_n;
            if(tbflag.test(nn)) continue;
            tbflag.set(nn);
            pool[p++] = (nn);
          }

        }

        std::vector<std::pair<float,size_t>> result;
        for(unsigned int i=0; i<p;i++){
          result.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(pool[i]), features_.get_cols()),pool[i]));
        }
        std::partial_sort(result.begin(), result.begin() + K, result.end());

        std::vector<int> res;
        for(unsigned int j = 0; j < K; j++) res.push_back(result[j].second);
        nn_results.push_back(res);
      }
      //std::cout<<"bad query number:  " <<VisitBucketNum[radius+1]<< std::endl;
    }

    void getNeighborsIEH32_nnexp(size_t K, const Matrix<DataType>& query){
      int lowbits = codelength - upbits;

      unsigned int MaxCheck=HammingRadius[radius];
      std::cout<<"maxcheck : "<<MaxCheck<<std::endl;

      int resultSize = SP.extend_to;
      if (K > (unsigned)SP.extend_to)
        resultSize = K;

      boost::dynamic_bitset<> tbflag(features_.get_rows(), false);
      nn_results.clear();

      VisitBucketNum.clear();
      VisitBucketNum.resize(radius+2);

      for(size_t cur = 0; cur < query.get_rows(); cur++){
        tbflag.reset();

        std::vector<int> pool(SP.search_init_num);
        unsigned int p = 0;

        unsigned int j = 0;
        for(; j < MaxCheck; j++){
          for(size_t h=0; h < QueryCode.size(); h++){
            unsigned int searchcode = QueryCode[h][cur] ^ HammingBallMask[j];
            unsigned int idx1 = searchcode >> lowbits;
            unsigned int idx2 = searchcode - (idx1 << lowbits);

            HashBucket::iterator bucket= htb[h][idx1].find(idx2);
            if(bucket != htb[h][idx1].end()){
              std::vector<unsigned int> vp = bucket->second;
              for(size_t k = 0; k < vp.size() && p < (unsigned int)SP.search_init_num; k++){
                if(tbflag.test(vp[k]))continue;

                tbflag.set(vp[k]);
                pool[p++]=(vp[k]);
              }
              if(p >= (unsigned int)SP.search_init_num)  break;
            }
            if(p >= (unsigned int)SP.search_init_num)  break;
          }
          if(p >= (unsigned int)SP.search_init_num)  break;
        }

        if(p < (unsigned int)SP.search_init_num){
          VisitBucketNum[radius+1]++;
        }else{
          for(int r=0;r<=radius;r++){
            if(j<=HammingRadius[r]){
              VisitBucketNum[r]++;
              break;
            }
          }
        }

        int base_n = features_.get_rows();
        while(p < (unsigned int)SP.search_init_num){
          unsigned int nn = rand() % base_n;
          if(tbflag.test(nn)) continue;
          tbflag.set(nn);
          pool[p++] = (nn);
        }

        //sorting the pool
        std::vector<std::pair<float,size_t>> result;
        for(unsigned int i=0; i<pool.size();i++){
          result.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(pool[i]), features_.get_cols()),pool[i]));
        }
        std::partial_sort(result.begin(), result.begin() + resultSize, result.end());
        result.resize(resultSize);
        pool.clear();
        for(int j = 0; j < resultSize; j++)
          pool.push_back(result[j].second);

        //nn_exp
        boost::dynamic_bitset<> newflag(features_.get_rows(), true);
        newflag.set();

        int iter=0;
        std::vector<int> ids;
        while(iter++ < SP.search_epoches){
          //the heap is max heap
          ids.clear();
          for(unsigned j = 0; j < SP.extend_to ; j++){
            if(newflag.test( pool[j] )){
              newflag.reset(pool[j]);

              for(unsigned neighbor=0; neighbor < gs[pool[j]].size(); neighbor++){
                unsigned id = gs[pool[j]][neighbor];

                if(tbflag.test(id))continue;
                else tbflag.set(id);

                ids.push_back(id);
              }
            }
          }

          for(size_t j = 0; j < ids.size(); j++){
            result.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(ids[j]), features_.get_cols()),ids[j]));
          }
          std::partial_sort(result.begin(), result.begin() + resultSize, result.end());
          result.resize(resultSize);
          pool.clear();
          for(int j = 0; j < resultSize; j++)
            pool.push_back(result[j].second);
        }

        if(K<(unsigned)SP.extend_to)
          pool.resize(K);

        nn_results.push_back(pool);
      }
    }

    void getNeighborsIEH32_kgraph(size_t K, const Matrix<DataType>& query){
      int lowbits = codelength - upbits;

      unsigned int MaxCheck=HammingRadius[radius];
      std::cout<<"maxcheck : "<<MaxCheck<<std::endl;

      nn_results.clear();
      boost::dynamic_bitset<> tbflag(features_.get_rows(), false);

      bool bSorted = true;
      unsigned pool_size = SP.search_epoches * SP.extend_to;
      if (pool_size >= (unsigned)SP.search_init_num){
        SP.search_init_num = pool_size;
        bSorted = false;
      }

      VisitBucketNum.clear();
      VisitBucketNum.resize(radius+2);

      for(size_t cur = 0; cur < query.get_rows(); cur++){

        tbflag.reset();
        std::vector<unsigned int> pool(SP.search_init_num);
        unsigned int p = 0;

        unsigned int j = 0;
        for(; j < MaxCheck; j++){
          for(size_t h=0; h < QueryCode.size(); h++){
            unsigned int searchcode = QueryCode[h][cur] ^ HammingBallMask[j];
            unsigned int idx1 = searchcode >> lowbits;
            unsigned int idx2 = searchcode - (idx1 << lowbits);

            HashBucket::iterator bucket= htb[h][idx1].find(idx2);
            if(bucket != htb[h][idx1].end()){
              std::vector<unsigned int> vp = bucket->second;
              for(size_t k = 0; k < vp.size() && p < (unsigned int)SP.search_init_num; k++){
                if(tbflag.test(vp[k]))continue;

                tbflag.set(vp[k]);
                pool[p++]=(vp[k]);
              }
              if(p >= (unsigned int)SP.search_init_num)  break;
            }
            if(p >= (unsigned int)SP.search_init_num)  break;
          }
          if(p >= (unsigned int)SP.search_init_num)  break;
        }

        if(p < (unsigned int)SP.search_init_num){
          VisitBucketNum[radius+1]++;
        }else{
          for(int r=0;r<=radius;r++){
            if(j<=HammingRadius[r]){
              VisitBucketNum[r]++;
              break;
            }
          }
        }

        int base_n = features_.get_rows();
        while(p < (unsigned int)SP.search_init_num){
          unsigned int nn = rand() % base_n;
          if(tbflag.test(nn)) continue;
          tbflag.set(nn);
          pool[p++] = (nn);
        }

        std::vector<std::pair<float,size_t>> result;
        for(unsigned int i=0; i<pool.size();i++){
          result.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(pool[i]), features_.get_cols()),pool[i]));
        }
        if(bSorted){
          std::partial_sort(result.begin(), result.begin() + pool_size, result.end());
          result.resize(pool_size);
        }

        tbflag.reset();

        std::vector<Point> knn(K + SP.extend_to +1);
        std::vector<Point> results;

        for (unsigned iter = 0; iter < (unsigned)SP.search_epoches; iter++) {

          unsigned L = 0;
          for(unsigned j=0; j < SP.extend_to ; j++){
            if(!tbflag.test(result[iter*SP.extend_to+j].second)){
              tbflag.set(result[iter*SP.extend_to+j].second);
              knn[L].id = result[iter*SP.extend_to+j].second;
              knn[L].dist = result[iter*SP.extend_to+j].first;
              knn[L].flag = true;
              L++;
            }
          }
          if(~bSorted){
            std::sort(knn.begin(), knn.begin() + L);
          }

          unsigned int k =  0;
          while (k < L) {
            unsigned int nk = L;
            if (knn[k].flag) {
              knn[k].flag = false;
              unsigned n = knn[k].id;

              for(unsigned neighbor=0; neighbor < gs[n].size(); neighbor++){
                unsigned id = gs[n][neighbor];

                if(tbflag.test(id))continue;
                tbflag.set(id);

                float dist = distance_->compare(query.get_row(cur), features_.get_row(id), features_.get_cols());
                Point nn(id, dist);
                unsigned int r = InsertIntoKnn(&knn[0], L, nn);

                //if ( (r <= L) && (L + 1 < knn.size())) ++L;
                if ( L + 1 < knn.size()) ++L;
                if (r < nk) nk = r;
              }
            }
            if (nk <= k) k = nk;
            else ++k;
          }

          if (L > K) L = K;
          if (results.empty()) {
            results.reserve(K + 1);
            results.resize(L + 1);
            std::copy(knn.begin(), knn.begin() + L, results.begin());
          }
          else {
            for (unsigned int l = 0; l < L; ++l) {
              unsigned r = InsertIntoKnn(&results[0], results.size() - 1, knn[l]);
              if (r < results.size() /* inserted */ && results.size() < (K + 1)) {
                results.resize(results.size() + 1);
              }
            }
          }
        }

        std::vector<int> res;
        for(size_t i = 0; i < K && i < results.size();i++)
          res.push_back(results[i].id);
        nn_results.push_back(res);

      }
    }

    void getNeighborsIEH64_nnexp(size_t K, const Matrix<DataType>& query){

      int lowbits = codelength - upbits;

      unsigned int MaxCheck=HammingRadius[radius];
      std::cout<<"maxcheck : "<<MaxCheck<<std::endl;

      int resultSize = SP.extend_to;
      if (K > (unsigned)SP.extend_to)
        resultSize = K;

      boost::dynamic_bitset<> tbflag(features_.get_rows(), false);
      nn_results.clear();

      VisitBucketNum.clear();
      VisitBucketNum.resize(radius+2);

      for(size_t cur = 0; cur < query.get_rows(); cur++){

        std::vector<int> pool(SP.search_init_num);
        unsigned int p = 0;
        tbflag.reset();

        unsigned int j = 0;
        for(; j < MaxCheck; j++){
          for(unsigned int h=0; h < QueryCode64.size(); h++){
            unsigned long searchcode = QueryCode64[h][cur] ^ HammingBallMask64[j];
            unsigned int idx1 = searchcode >> lowbits;
            unsigned long idx2 = searchcode - (( unsigned long)idx1 << lowbits);

            HashBucket64::iterator bucket= htb64[h][idx1].find(idx2);
            if(bucket != htb64[h][idx1].end()){
              std::vector<unsigned int> vp = bucket->second;
              for(size_t k = 0; k < vp.size() && p < (unsigned int)SP.search_init_num; k++){
                if(tbflag.test(vp[k]))continue;

                tbflag.set(vp[k]);
                pool[p++]=(vp[k]);
              }
              if(p >= (unsigned int)SP.search_init_num)  break;
            }
            if(p >= (unsigned int)SP.search_init_num)  break;
          }
          if(p >= (unsigned int)SP.search_init_num)  break;
        }

        if(p < (unsigned int)SP.search_init_num){
          VisitBucketNum[radius+1]++;
        }else{
          for(int r=0;r<=radius;r++){
            if(j<=HammingRadius[r]){
              VisitBucketNum[r]++;
              break;
            }
          }
        }

        int base_n = features_.get_rows();
        while(p < (unsigned int)SP.search_init_num){
          unsigned int nn = rand() % base_n;
          if(tbflag.test(nn)) continue;
          tbflag.set(nn);
          pool[p++] = (nn);
        }

        //sorting the pool
        std::vector<std::pair<float,size_t>> result;
        for(unsigned int i=0; i<pool.size();i++){
          result.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(pool[i]), features_.get_cols()),pool[i]));
        }
        std::partial_sort(result.begin(), result.begin() + resultSize, result.end());
        result.resize(resultSize);
        pool.clear();
        for(int j = 0; j < resultSize; j++)
          pool.push_back(result[j].second);

        //nn_exp
        boost::dynamic_bitset<> newflag(features_.get_rows(), true);
        newflag.set();

        int iter=0;
        std::vector<int> ids;
        while(iter++ < SP.search_epoches){
          //the heap is max heap
          ids.clear();
          for(unsigned j = 0; j < SP.extend_to ; j++){
            if(newflag.test( pool[j] )){
              newflag.reset(pool[j]);

              for(unsigned neighbor=0; neighbor < gs[pool[j]].size(); neighbor++){
                unsigned id = gs[pool[j]][neighbor];

                if(tbflag.test(id))continue;
                else tbflag.set(id);

                ids.push_back(id);
              }
            }
          }

          for(size_t j = 0; j < ids.size(); j++){
            result.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(ids[j]), features_.get_cols()),ids[j]));
          }
          std::partial_sort(result.begin(), result.begin() + resultSize, result.end());
          result.resize(resultSize);
          pool.clear();
          for(int j = 0; j < resultSize; j++)
            pool.push_back(result[j].second);
        }

        if(K<(unsigned)SP.extend_to)
          pool.resize(K);

        nn_results.push_back(pool);
      }
    }

    void getNeighborsIEH64_kgraph(size_t K, const Matrix<DataType>& query){
      int lowbits = codelength - upbits;

      unsigned int MaxCheck=HammingRadius[radius];
      std::cout<<"maxcheck : "<<MaxCheck<<std::endl;

      nn_results.clear();
      boost::dynamic_bitset<> tbflag(features_.get_rows(), false);

      bool bSorted = true;
      unsigned pool_size = SP.search_epoches * SP.extend_to;
      if (pool_size >= (unsigned)SP.search_init_num){
        SP.search_init_num = pool_size;
        bSorted = false;
      }

      VisitBucketNum.clear();
      VisitBucketNum.resize(radius+2);

      for(size_t cur = 0; cur < query.get_rows(); cur++){

        std::vector<unsigned int> pool(SP.search_init_num);
        unsigned int p = 0;
        tbflag.reset();

        unsigned int j = 0;
        for(; j < MaxCheck; j++){
          for(unsigned int h=0; h < QueryCode64.size(); h++){
            unsigned long searchcode = QueryCode64[h][cur] ^ HammingBallMask64[j];
            unsigned int idx1 = searchcode >> lowbits;
            unsigned long idx2 = searchcode - (( unsigned long)idx1 << lowbits);

            HashBucket64::iterator bucket= htb64[h][idx1].find(idx2);
            if(bucket != htb64[h][idx1].end()){
              std::vector<unsigned int> vp = bucket->second;
              for(size_t k = 0; k < vp.size() && p < (unsigned int)SP.search_init_num; k++){
                if(tbflag.test(vp[k]))continue;

                tbflag.set(vp[k]);
                pool[p++]=(vp[k]);
              }
              if(p >= (unsigned int)SP.search_init_num)  break;
            }
            if(p >= (unsigned int)SP.search_init_num)  break;
          }
          if(p >= (unsigned int)SP.search_init_num)  break;
        }

        if(p < (unsigned int)SP.search_init_num){
          VisitBucketNum[radius+1]++;
        }else{
          for(int r=0;r<=radius;r++){
            if(j<=HammingRadius[r]){
              VisitBucketNum[r]++;
              break;
            }
          }
        }

        int base_n = features_.get_rows();
        while(p < (unsigned int)SP.search_init_num){
          unsigned int nn = rand() % base_n;
          if(tbflag.test(nn)) continue;
          tbflag.set(nn);
          pool[p++] = (nn);
        }

        std::vector<std::pair<float,size_t>> result;
        for(unsigned int i=0; i<pool.size();i++){
          result.push_back(std::make_pair(distance_->compare(query.get_row(cur), features_.get_row(pool[i]), features_.get_cols()),pool[i]));
        }
        if(bSorted){
          std::partial_sort(result.begin(), result.begin() + pool_size, result.end());
          result.resize(pool_size);
        }

        tbflag.reset();

        std::vector<Point> knn(K + SP.extend_to +1);
        std::vector<Point> results;

        for (unsigned iter = 0; iter < (unsigned)SP.search_epoches; iter++) {

          unsigned L = 0;
          for(unsigned j=0; j < (unsigned)SP.extend_to ; j++){
            if(!tbflag.test(result[iter*SP.extend_to+j].second)){
              tbflag.set(result[iter*SP.extend_to+j].second);
              knn[L].id = result[iter*SP.extend_to+j].second;
              knn[L].dist = result[iter*SP.extend_to+j].first;
              knn[L].flag = true;
              L++;
            }
          }
          if(~bSorted){
            std::sort(knn.begin(), knn.begin() + L);
          }

          unsigned int k =  0;
          while (k < L) {
            unsigned int nk = L;
            if (knn[k].flag) {
              knn[k].flag = false;
              unsigned n = knn[k].id;

              for(unsigned neighbor=0; neighbor < gs[n].size(); neighbor++){
                unsigned id = gs[n][neighbor];

                if(tbflag.test(id))continue;
                tbflag.set(id);

                float dist = distance_->compare(query.get_row(cur), features_.get_row(id), features_.get_cols());
                Point nn(id, dist);
                unsigned int r = InsertIntoKnn(&knn[0], L, nn);

                //if ( (r <= L) && (L + 1 < knn.size())) ++L;
                if ( L + 1 < knn.size()) ++L;
                if (r < nk) nk = r;
              }
            }
            if (nk <= k) k = nk;
            else ++k;
          }

          if (L > K) L = K;
          if (results.empty()) {
            results.reserve(K + 1);
            results.resize(L + 1);
            std::copy(knn.begin(), knn.begin() + L, results.begin());
          }
          else {
            for (unsigned int l = 0; l < L; ++l) {
              unsigned r = InsertIntoKnn(&results[0], results.size() - 1, knn[l]);
              if (r < results.size() /* inserted */ && results.size() < (K + 1)) {
                results.resize(results.size() + 1);
              }
            }
          }
        }

        std::vector<int> res;
        for(size_t i = 0; i < K && i < results.size();i++)
          res.push_back(results[i].id);
        nn_results.push_back(res);

      }
    }

    void outputVisitBucketNum(){
      unsigned i=0;
      std::cout<< "Radius " << i <<" bucket num: "<<HammingRadius[i]<< " points num: "<< VisitBucketNum[i]<<std::endl;

      for(i=1; i<HammingRadius.size();i++){
        std::cout<< "Radius " << i <<" bucket num: "<<HammingRadius[i] - HammingRadius[i-1]<< " points num: "<< VisitBucketNum[i]<<std::endl;
      }
      std::cout<< "Radius larger, points num: " << VisitBucketNum[i]<<std::endl;
    }

    void loadIndex(char* filename){}
    void saveIndex(char* filename){}
    void loadTrees(char* filename){}
    void saveTrees(char* filename){}
    void loadGraph(char* filename){
      std::ifstream in(filename,std::ios::binary);
      in.seekg(0,std::ios::end);
      std::ios::pos_type ss = in.tellg();
      size_t fsize = (size_t)ss;
      int dim;
      in.seekg(0,std::ios::beg);
      in.read((char*)&dim, sizeof(int));
      size_t num = fsize / (dim+1) / 4;
      std::cout<<"load g "<<num<<" "<<dim<< std::endl;
      in.seekg(0,std::ios::beg);
      gs.clear();
      for(size_t i = 0; i < num; i++){
        std::vector<unsigned> heap;
        in.read((char*)&dim, sizeof(int));
        for(int j =0; j < dim; j++){
          unsigned id;
          in.read((char*)&id, sizeof(int));
          heap.push_back(id);
        }
        gs.push_back(heap);
      }
      in.close();
    }
    void saveGraph(char* filename){}
    void initGraph(){}

  protected:
    int tablenum;
    int upbits;
    int codelength;
    int codelengthshift;
    int radius;
    USING_BASECLASS_SYMBOLS

    std::vector<HashTable> htb;
    std::vector<Codes> BaseCode;
    std::vector<Codes> QueryCode;
    std::vector<unsigned int> HammingBallMask;

    std::vector<HashTable64> htb64;
    std::vector<Codes64> BaseCode64;
    std::vector<Codes64> QueryCode64;
    std::vector<unsigned long> HammingBallMask64;

    std::vector<unsigned int> HammingRadius;

    // for statistic info
    std::vector<unsigned int> VisitBucketNum;

};
}
#endif


#pragma once
#include "../../utils/Units.hpp"
#include <Eigen/Core>
#include <string>
#include <variant>

/**
 * A namespace containing PODs of PDB records. For reference, see PDB format
 * report.
 */
namespace mdk::pdb::records {
    class Remark {
    public:
        int number = 6;
        std::string text = "";
    };

    class Atom {
    public:
        int serialNum = 0; // Zero-indexed here.
        std::string atomName;
        char altLocation = ' ';
        std::string residueName;
        char chainID = 'A';
        int residueSeqNum = 0; // Zero-indexed here.
        char insertionCode = ' ';
        Eigen::Vector3d pos;
        double occupancy = 1.0;
        double tempFactor = 0.0;
        std::string element;
        std::string charge;
    };

    class SSBond {
    public:
        int serialNum = 0;

        struct PerResidueData {
            char chainId = 'A';
            int residueSeqNum = 0; // Zero-indexed here.
            char insertionCode = ' ';
            std::string symmetryOp;
        };
        PerResidueData res[2];

        double dist0 = 0.0;
    };

    class Cryst1 {
    public:
        Eigen::Vector3d size;
        Eigen::Vector3d angles = { 90*degree, 90*degree, 90*degree };
        std::string spaceGroup;
        int z = 0;
    };

    class Ter {
    public:
        int serialNum = 0; // Zero-indexed here.
        std::string residueName;
        char chainId = 'A';
        int residueSeqNum = 0; // Zero-indexed here.
        char insertionCode = ' ';
    };

    class Link {
    public:
        struct PerResidueData {
            std::string atomName;
            char altLocation = ' ';
            std::string residueName;
            char chainId = 'A';
            int residueSeqNum;
            char insertionCode = ' ';
            std::string symmetryOp;
        };
        PerResidueData res[2];
        double linkLength;
    };

    class Model {
    public:
        int serialNum = 0; // Zero-indexed here.
    };

    class Endmdl {
    public:
    };

    class End {
    public:
    };

    /**
     * A variant type denoting either a record of some type, or no record
     * \p std::monostate in the case of a parsing failure.
     */
    using Record = std::variant<std::monostate,
        Remark, Atom, SSBond, Cryst1, End, Link,
        Model, Endmdl, Ter>;
}


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

#include "clotho/utility/random_generator.hpp"
#include <boost/random/poisson_distribution.hpp>
#include "clotho/mutation/mutation_rate_parameter.hpp"

namespace clotho {
namespace utility {

template < class URNG, class Sequence, class Generator >
class random_generator< URNG, sequence_mutator< Sequence, Generator > > {
public:
    typedef URNG                                        rng_type;
    typedef sequence_mutator< Sequence, Generator >     result_type;

    typedef typename result_type::generator_result_type           allele_type;
    typedef typename allele_type::real_type             real_type;

    typedef mutation_rate_parameter< real_type >        mutation_type;
    typedef Generator                                   mutation_generator_type;

    typedef boost::random::poisson_distribution< unsigned int, real_type > dist_type;

//    random_generator( rng_type & rng,  mutation_generator_type & mgen, double rate ) : m_rng(&rng), m_mgen( mgen ), m_dist(rate) {}
    random_generator( rng_type & rng, boost::property_tree::ptree & config ) :
        m_rng( &rng )
        , m_mut( config )
        , m_mgen( rng, config )
        , m_dist( mutation_type::DEFAULT_MUTATION_RATE ) {
        typename dist_type::param_type p( m_mut.m_mu );
        m_dist.param( p );
    }

    result_type operator()( unsigned int age = 0) {
        unsigned int nEvents = m_dist(*m_rng);
        return result_type( m_mgen, nEvents, age );
    }

protected:
    rng_type    * m_rng;
    mutation_type   m_mut;
    mutation_generator_type m_mgen;
    dist_type   m_dist;
};

}   // namespace utility {
}   // namespace clotho {

#endif  // SEQUENCE_MUTATOR_GENERATOR_HPP_