/* Copyright (c) 2016 - 2019, the adamantine authors.
 *
 * This file is subject to the Modified BSD License and may not be distributed
 * without copyright and license information. Please refer to the file LICENSE
 * for the text and further information on this license.
 */

#include <MaterialProperty.hh>
#include <instantiation.hh>

#include <deal.II/base/point.h>
#include <deal.II/base/quadrature_lib.h>
#include <deal.II/fe/fe_values.h>

#include <boost/optional.hpp>

namespace adamantine
{

template <int dim>
MaterialProperty<dim>::MaterialProperty(
    MPI_Comm const &communicator,
    dealii::parallel::distributed::Triangulation<dim> const &tria,
    boost::property_tree::ptree const &database)
    : _communicator(communicator), _fe(0), _mp_dof_handler(tria)
{
  // Because deal.II cannot easily attach data to a cell. We store the state
  // of the material in distributed::Vector. This allows to use deal.II to
  // compute the new state after refinement of the mesh. However, this
  // requires to use another DoFHandler.
  reinit_dofs();

  // Set the material state to the state defined in the geometry.
  set_state();

  // Fill the _properties map
  fill_properties(database);

  // Compute the alpha and beta constants
  compute_constants();
}

template <int dim>
template <typename NumberType>
double MaterialProperty<dim>::get(
    typename dealii::Triangulation<dim>::active_cell_iterator const &cell,
    Property prop, dealii::LA::distributed::Vector<NumberType> const &) const
{
  // TODO: For now, ignore field_state since we have a linear problem.
  double value = 0.;
  dealii::types::material_id material_id = cell->material_id();
  unsigned int property = static_cast<unsigned int>(prop);

  double const mp_dof_index = get_dof_index(cell);

  for (unsigned int i = 0; i < _n_material_states; ++i)
  {
    // We cannot use operator[] because the function is constant.
    auto const tmp = _properties.find(material_id);
    ASSERT(tmp != _properties.end(), "Material not found.");
    if ((tmp->second)[i][property] != nullptr)
      value += _state[i][mp_dof_index] *
               (tmp->second)[i][property]->value(dealii::Point<1>());
  }

  return value;
}

template <int dim>
template <typename NumberType>
dealii::LA::distributed::Vector<NumberType>
MaterialProperty<dim>::enthalpy_to_temperature(
    dealii::DoFHandler<dim> const &enthalpy_dof_handler,
    dealii::LA::distributed::Vector<NumberType> const &enthalpy)
{
  dealii::LA::distributed::Vector<NumberType> temperature(
      enthalpy.get_partitioner());
  dealii::LA::distributed::Vector<NumberType> dummy;

  update_state(enthalpy_dof_handler, enthalpy);

  unsigned int const dofs_per_cell =
      enthalpy_dof_handler.get_fe().dofs_per_cell;
  std::vector<dealii::types::global_dof_index> local_dof_indices(dofs_per_cell);
  for (auto cell :
       dealii::filter_iterators(enthalpy_dof_handler.active_cell_iterators(),
                                dealii::IteratorFilters::LocallyOwnedCell()))
  {
    cell->get_dof_indices(local_dof_indices);

    double const liquid_ratio = get_state_ratio(cell, MaterialState::liquid);
    double const density = get(cell, Property::density, dummy);
    double const specific_heat = get(cell, Property::specific_heat, dummy);
    double const liquidus = get(cell, Property::liquidus, dummy);
    double const solidus = get(cell, Property::solidus, dummy);
    double const solidus_enthalpy = solidus * density * specific_heat;
    double const latent_heat = get(cell, Property::latent_heat, dummy);
    double const liquidus_enthalpy = solidus_enthalpy + latent_heat;

    NumberType enth_to_temp = [liquid_ratio, liquidus, solidus,
                               liquidus_enthalpy, solidus_enthalpy, density,
                               specific_heat](double const enthalpy) {
      if (liquid_ratio > 0.)
      {
        if (liquid_ratio == 1.)
        {
          return liquidus +
                 (enthalpy - liquidus_enthalpy) / (density * specific_heat);
        }
        else
        {
          return solidus + (liquidus - solidus) *
                               (enthalpy - solidus_enthalpy) /
                               (liquidus_enthalpy - solidus_enthalpy);
        }
      }
      else
      {
        return enthalpy / (density * specific_heat);
      }
    };

    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      temperature[local_dof_indices[i]] =
          enth_to_temp(enthalpy[local_dof_indices[i]]);
  }

  return temperature;
}

template <int dim>
void MaterialProperty<dim>::reinit_dofs()
{
  _mp_dof_handler.distribute_dofs(_fe);
  // Initialize the state vectors
  for (auto &vec : _state)
    vec.reinit(_mp_dof_handler.locally_owned_dofs(), _communicator);
}

template <int dim>
template <typename NumberType>
void MaterialProperty<dim>::update_state(
    dealii::DoFHandler<dim> const &enthalpy_dof_handler,
    dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &enthalpy)
{
  auto enthalpy_average =
      compute_average_enthalpy(enthalpy_dof_handler, enthalpy);
  dealii::LA::distributed::Vector<NumberType> enthalpy_average_host(enthalpy_average.get_partitioner());
  enthalpy_average_host.import(enthalpy_average, dealii::VectorOperation::insert);

  std::vector<dealii::types::global_dof_index> mp_dof(1);
  for (auto cell :
       dealii::filter_iterators(_mp_dof_handler.active_cell_iterators(),
                                dealii::IteratorFilters::LocallyOwnedCell()))
  {
    dealii::types::material_id material_id = cell->material_id();
    auto const tmp = _properties.find(material_id);
    ASSERT(tmp != _properties.end(), "Material not found.");

    // TODO check that assumption enthalpy of solidus powder = enthalpy of
    // solidus solid
    unsigned int constexpr liquid =
        static_cast<unsigned int>(MaterialState::liquid);
    unsigned int constexpr powder =
        static_cast<unsigned int>(MaterialState::powder);
    unsigned int constexpr solid =
        static_cast<unsigned int>(MaterialState::solid);
    unsigned int constexpr prop_latent_heat =
        static_cast<unsigned int>(Property::latent_heat);
    unsigned int constexpr prop_solidus =
        static_cast<unsigned int>(Property::solidus);
    unsigned int constexpr prop_density =
        static_cast<unsigned int>(Property::density);
    unsigned int constexpr prop_specific_heat =
        static_cast<unsigned int>(Property::specific_heat);

    dealii::Point<1> const empty_pt;

    double const latent_heat =
        ((tmp->second)[solid][prop_latent_heat] == nullptr)
            ? std::numeric_limits<double>::max()
            : (tmp->second)[solid][prop_latent_heat]->value(empty_pt);
    double const solidus =
        ((tmp->second)[solid][prop_solidus] == nullptr)
            ? std::numeric_limits<double>::max()
            : (tmp->second)[solid][prop_solidus]->value(empty_pt);
    double const solidus_enthalpy =
        solidus * (tmp->second)[solid][prop_density]->value(empty_pt) *
        (tmp->second)[solid][prop_specific_heat]->value(empty_pt);
    double const liquidus_enthalpy = solidus_enthalpy + latent_heat;
    cell->get_dof_indices(mp_dof);
    unsigned int const dof = mp_dof[0];

    // First determine the ratio of liquid.
    double liquid_ratio = -1.;
    double powder_ratio = -1.;
    double solid_ratio = -1.;
    if (enthalpy_average_host[dof] < solidus_enthalpy)
      liquid_ratio = 0.;
    else if (enthalpy_average_host[dof] > liquidus_enthalpy)
      liquid_ratio = 1.;
    else
      liquid_ratio = (enthalpy_average_host[dof] - solidus_enthalpy) / latent_heat;
    // Because the powder can only become liquid, the solid can only become
    // liquid, and the liquid can only become solid, the ratio of powder can
    // only decrease.
    powder_ratio = std::min(1. - liquid_ratio, _state[powder][dof]);
    // Use max to make sure that we don't create matter because of round-off.
    solid_ratio = std::max(1 - liquid_ratio - powder_ratio, 0.);

    // Update the value
    _state[liquid][dof] = liquid_ratio;
    _state[powder][dof] = powder_ratio;
    _state[solid][dof] = solid_ratio;
  }
}

template <int dim>
void MaterialProperty<dim>::set_state()
{
  // Set the material state to the one defined by the user_index
  std::vector<dealii::types::global_dof_index> mp_dof(1);
  for (auto cell :
       dealii::filter_iterators(_mp_dof_handler.active_cell_iterators(),
                                dealii::IteratorFilters::LocallyOwnedCell()))
  {
    cell->get_dof_indices(mp_dof);
    _state[cell->user_index()][mp_dof[0]] = 1.;
  }
}

template <int dim>
void MaterialProperty<dim>::fill_properties(
    boost::property_tree::ptree const &database)
{
  std::array<std::string, _n_material_states> material_state = {
      {"powder", "solid", "liquid"}};
  std::array<std::string, _n_properties> properties = {
      {"density", "latent_heat", "liquidus", "solidus", "specific_heat",
       "thermal_conductivity"}};

  unsigned int const n_materials = database.get<unsigned int>("n_materials");
  dealii::types::material_id next_material_id = 0;
  for (unsigned int i = 0; i < n_materials; ++i)
  {
    // Try to find the material_id by checking every possible number.
    dealii::types::material_id material_id = next_material_id;
    while (material_id < dealii::numbers::invalid_material_id)
    {
      // If the child exists, exit the loop.
      if (database.count("material_" + std::to_string(material_id)) != 0)
        break;

      ++material_id;
    }
    ASSERT_THROW(material_id != dealii::numbers::invalid_material_id,
                 "Invalid material ID. Choose a smaller number");
    // Update the next possible material id
    ++next_material_id;

    // Get the material property tree.
    std::string variable = "T";
    boost::property_tree::ptree const &material_database =
        database.get_child("material_" + std::to_string(material_id));
    // For each material, loop over the possible states.
    bool valid_state = false;
    for (unsigned int state = 0; state < _n_material_states; ++state)
    {
      // The state may or may not exist for the material.
      boost::optional<boost::property_tree::ptree const &> state_database =
          material_database.get_child_optional(material_state[state]);
      if (state_database)
      {
        valid_state = true;
        // For each state, loop over the possible properties.
        for (unsigned int p = 0; p < _n_properties; ++p)
        {
          // The property may or may not exist for that state
          boost::optional<std::string> const property =
              state_database.get().get_optional<std::string>(properties[p]);
          // If the property exists, put it in the map. If the property does not
          // exist, we have a nullptr.
          if (property)
          {
            _properties[material_id][state][p] =
                std::make_unique<dealii::FunctionParser<1>>(1);
            _properties[material_id][state][p]->initialize(
                variable, property.get(), std::map<std::string, double>());
          }
        }
      }
    }
    // Check that there is at least one valid MaterialState
    ASSERT_THROW(
        valid_state == true,
        "Material without any valid state (solid, powder, or liquid).");

    // Check for the properties that are associated to a material but that
    // are independent of an individual state. These properties are duplicated
    // for every state.
    for (unsigned int p = 0; p < _n_properties; ++p)
    {
      // The property may or may not exist for that state
      boost::optional<std::string> const property =
          material_database.get_optional<std::string>(properties[p]);
      // If the property exists, put it in the map. If the property does not
      // exist, we have a nullptr.
      if (property)
      {
        for (unsigned int state = 0; state < _n_material_states; ++state)
        {
          _properties[material_id][state][p] =
              std::make_unique<dealii::FunctionParser<1>>(1);
          _properties[material_id][state][p]->initialize(
              variable, property.get(), std::map<std::string, double>());
        }
      }
    }
  }
}

template <int dim>
void MaterialProperty<dim>::compute_constants()
{
  unsigned int constexpr liquid =
      static_cast<unsigned int>(MaterialState::liquid);
  unsigned int constexpr solid =
      static_cast<unsigned int>(MaterialState::solid);
  unsigned int constexpr prop_latent_heat =
      static_cast<unsigned int>(Property::latent_heat);
  unsigned int constexpr solidus = static_cast<unsigned int>(Property::solidus);
  unsigned int constexpr liquidus =
      static_cast<unsigned int>(Property::liquidus);
  unsigned int constexpr density = static_cast<unsigned int>(Property::density);
  unsigned int constexpr specific_heat =
      static_cast<unsigned int>(Property::specific_heat);

  dealii::Point<1> const empty_pt;

  for (auto prop = _properties.begin(); prop != _properties.end(); ++prop)
  {
    dealii::types::material_id const material_id = prop->first;
    bool const liquidus_exist =
        (prop->second[solid][liquidus] == nullptr) ? false : true;
    bool const solidus_exist =
        (prop->second[solid][solidus] == nullptr) ? false : true;
    bool const latent_heat_exist =
        (prop->second[solid][prop_latent_heat] == nullptr) ? false : true;
    bool const density_exist =
        (prop->second[solid][density] == nullptr) ? false : true;
    bool const specific_heat_exist =
        (prop->second[solid][specific_heat] == nullptr) ? false : true;

    if (liquidus_exist && solidus_exist && latent_heat_exist)
      _mushy_alpha[material_id] =
          (prop->second[solid][liquidus]->value(empty_pt) -
           prop->second[solid][solidus]->value(empty_pt)) /
          prop->second[solid][prop_latent_heat]->value(empty_pt);

    if (liquidus_exist && solidus_exist && density_exist && specific_heat_exist)
    {
      double const solidus_enthalpy =
          prop->second[solid][solidus]->value(empty_pt) *
          prop->second[solid][density]->value(empty_pt) *
          prop->second[solid][specific_heat]->value(empty_pt);
      _mushy_beta[material_id] =
          -solidus_enthalpy /
              prop->second[solid][prop_latent_heat]->value(empty_pt) *
              (prop->second[solid][liquidus]->value(empty_pt) -
               prop->second[solid][solidus]->value(empty_pt)) +
          prop->second[solid][solidus]->value(empty_pt);
      // TODO this is true only if density and heat capacity are independent of
      // the temperature
      _liquid_beta[material_id] =
          -(solidus_enthalpy +
            prop->second[liquid][prop_latent_heat]->value(empty_pt)) /
              (prop->second[liquid][density]->value(empty_pt) *
               prop->second[liquid][specific_heat]->value(empty_pt)) +
          prop->second[liquid][liquidus]->value(empty_pt);
    }
  }
}

template <int dim>
template <typename NumberType>
dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA>
MaterialProperty<dim>::compute_average_enthalpy(
    dealii::DoFHandler<dim> const &enthalpy_dof_handler,
    dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> const &enthalpy) const
{
  // TODO: this should probably done in a matrix-free fashion.
  // The triangulation is the same for both DoFHandler
  dealii::LA::distributed::Vector<NumberType> enthalpy_average(
      _mp_dof_handler.locally_owned_dofs(), enthalpy.get_mpi_communicator());
  dealii::LA::distributed::Vector<NumberType> enthalpy_host(enthalpy.get_partitioner());
  enthalpy_host.import(enthalpy, dealii::VectorOperation::insert);
  enthalpy_average = 0.;
  auto mp_cell = _mp_dof_handler.begin_active();
  auto mp_end_cell = _mp_dof_handler.end();
  auto enth_cell = enthalpy_dof_handler.begin_active();
  dealii::FiniteElement<dim> const &fe = enthalpy_dof_handler.get_fe();
  // We can use a lower degree of quadrature since we are projecting on a
  // piecewise constant space
  dealii::QGauss<dim> quadrature(fe.degree);
  dealii::FEValues<dim> fe_values(
      fe, quadrature,
      dealii::UpdateFlags::update_values |
          dealii::UpdateFlags::update_quadrature_points |
          dealii::UpdateFlags::update_JxW_values);
  unsigned int const dofs_per_cell = fe.dofs_per_cell;
  std::vector<dealii::types::global_dof_index> mp_dof_indices(1);
  std::vector<dealii::types::global_dof_index> enth_dof_indices(dofs_per_cell);
  unsigned int const n_q_points = quadrature.size();
  for (; mp_cell != mp_end_cell; ++enth_cell, ++mp_cell)
    if (mp_cell->is_locally_owned())
    {
      fe_values.reinit(enth_cell);
      mp_cell->get_dof_indices(mp_dof_indices);
      dealii::types::global_dof_index const mp_dof_index = mp_dof_indices[0];
      enth_cell->get_dof_indices(enth_dof_indices);
      double area = 0.;
      for (unsigned int q = 0; q < n_q_points; ++q)
        for (unsigned int i = 0; i < dofs_per_cell; ++i)
        {
          Assert(enth_dof_indices[i] < enthalpy_dof_handler.n_dofs(), dealii::ExcInternalError());
          Assert(enth_dof_indices[i] < enthalpy_host.size(), dealii::ExcInternalError());
          area += fe_values.shape_value(i, q) * fe_values.JxW(q);
          enthalpy_average[mp_dof_index] += fe_values.shape_value(i, q) *
                                            enthalpy_host[enth_dof_indices[i]] *
                                            fe_values.JxW(q);
        }
      enthalpy_average[mp_dof_index] /= area;
    }

  dealii::LA::distributed::Vector<NumberType, dealii::MemorySpace::CUDA> enthalpy_average_device(
      _mp_dof_handler.locally_owned_dofs(), enthalpy.get_mpi_communicator());
  enthalpy_average_device.import(enthalpy_average, dealii::VectorOperation::insert);

  return enthalpy_average_device;
}
} // namespace adamantine

INSTANTIATE_DIM(MaterialProperty)

namespace adamantine
{
// Instantiate templated function: get
template double MaterialProperty<2>::get(
    dealii::Triangulation<2>::active_cell_iterator const &, Property prop,
    dealii::LA::distributed::Vector<float> const &) const;
template double MaterialProperty<2>::get(
    dealii::Triangulation<2>::active_cell_iterator const &, Property prop,
    dealii::LA::distributed::Vector<double> const &) const;
template double MaterialProperty<3>::get(
    dealii::Triangulation<3>::active_cell_iterator const &, Property prop,
    dealii::LA::distributed::Vector<float> const &) const;
template double MaterialProperty<3>::get(
    dealii::Triangulation<3>::active_cell_iterator const &, Property prop,
    dealii::LA::distributed::Vector<double> const &) const;

// Instantiate templated function: update_state
template void MaterialProperty<2>::update_state(
    dealii::DoFHandler<2> const &,
    dealii::LA::distributed::Vector<float, dealii::MemorySpace::CUDA> const &);
template void MaterialProperty<2>::update_state(
    dealii::DoFHandler<2> const &,
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::CUDA> const &);
template void MaterialProperty<3>::update_state(
    dealii::DoFHandler<3> const &,
    dealii::LA::distributed::Vector<float, dealii::MemorySpace::CUDA> const &);
template void MaterialProperty<3>::update_state(
    dealii::DoFHandler<3> const &,
    dealii::LA::distributed::Vector<double, dealii::MemorySpace::CUDA> const &);
} // namespace adamantine


// -------------------------------------------
//  @description: 产生骨架的三维点云
//  @author: hts
//  @data: 2020-04-11
//  @version: wpdwp
// -------------------------------------------
#include <stdlib.h>
#include <stdio.h>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <cmath>
#include <mutex>
#include <thread>
#include <chrono>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/cloud_viewer.h>

#include <opencv2/opencv.hpp>

#include <ros/ros.h>
#include <ros/spinner.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>

#include <cv_bridge/cv_bridge.h>

#include <image_transport/image_transport.h>
#include <image_transport/subscriber_filter.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/exact_time.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <kinect2_bridge/kinect2_definitions.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;

#define pi 3.14159265359

class Posepoint
{
private:
  std::mutex lock;
  cv::Mat color, depth;
  cv::Mat cameraMatrixColor, cameraMatrixDepth;
  cv::Mat lookupX, lookupY;
  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud;
  pcl::PCDWriter writer;
  bool visualize_stop;
  double pick_points[6];
  Vector3f Lshoulder, Rshoulder, Pelv;
public:
  Posepoint():visualize_stop(false)
  {
    cameraMatrixColor = cv::Mat::zeros(3, 3, CV_64F);
    static double my_points[6] = {255.67636108398438,292.16888427734375,320.4430236816406,266.26220703125,330.15802001953125,298.64556884765625};
    for(int i=0; i<6; i++)
    {
      pick_points[i] = my_points[i];
    }
  }
  ~Posepoint()
  {
  }
public:
    void start(std::string color_path, std::string depth_path)
  {
    color = cv::imread(color_path);
    depth = cv::imread(depth_path,2);
    cloud = pcl::PointCloud<pcl::PointXYZRGBA>::Ptr(new pcl::PointCloud<pcl::PointXYZRGBA>());
    cloud->height = color.rows;
    cloud->width = color.cols;
    cloud->is_dense = false;
    cloud->points.resize(cloud->height * cloud->width);
    readCameraInfo();
    createLookup(this->color.cols, this->color.rows); //像极坐标系中的值保存在lookupX　lookupY里面
    cloudViewer();
  }
  // 3x3内参矩阵
//   367.933   0         254.169
//     0     367.933     204.267
//     0       0            1
    void readCameraInfo()
  {
    double cameraInfoK[9]={367.933 , 0 , 254.169,
                           0 , 367.933 , 204.267,
                          0 , 0 , 1             };
    double *itC = cameraMatrixColor.ptr<double>(0, 0);
    for(size_t i = 0; i < 9; ++i, ++itC)
    {
      *itC = cameraInfoK[i];
    }
  }
    // 求在相机坐标系中的坐标
    void createLookup(size_t width, size_t height)
  {
    // width是列->x　height是行->y
    // 得到相机的内参数
    //  成像模型
// [u       [ fx　0 cx   [x
//  v  = 1/z  0  fy cy    y
//  1]        0   0  1]   z]
// 求逆矩阵　得到像极坐标系中的坐标
    const float fx = 1.0f / cameraMatrixColor.at<double>(0, 0);
    const float fy = 1.0f / cameraMatrixColor.at<double>(1, 1);
    const float cx = cameraMatrixColor.at<double>(0, 2);
    const float cy = cameraMatrixColor.at<double>(1, 2);
    float *it;
    // cout<<fx<<" "<<fy<<" "<<cx<<" "<<cy<<" "<<endl;
    lookupY = cv::Mat(1, height, CV_32F);
    it = lookupY.ptr<float>();
    for(size_t r = 0; r < height; ++r, ++it)
    {
      *it = (r - cy) * fy;
    }

    lookupX = cv::Mat(1, width, CV_32F);
    it = lookupX.ptr<float>();
    for(size_t c = 0; c < width; ++c, ++it)
    {
      *it = (c - cx) * fx;
    }
  }
    void cloudViewer()
  {
    cv::Mat color, depth;
    pcl::visualization::PCLVisualizer::Ptr visualizer(new pcl::visualization::PCLVisualizer("Cloud Viewer"));
    const std::string cloudName = "rendered";

    lock.lock();
    color = this->color;
    depth = this->depth;
    lock.unlock();

    createCloud(depth, color, cloud);
    // 将点云数据添加到视窗中，并为其定义一个唯一的字符串作为ID号，利用此ID号保证其他成员方法也能表示该点云。
    // 多次调用addPointCloud()可以实现多个点云的叠加，每调用一次就创建一个新的ID号。如果想要更新一个已经
    // 显示的点云，用户必须先调用removePointCloud()，并提供新的ID号。（在PCL1.1版本之后直接调用updatePointCloud()
    //  就可以了，不必手动调用removePointCloud()就可实现点云更新）
    visualizer->addPointCloud(cloud, cloudName);
    // 修改现实点云的尺寸。用户可通过该方法控制点云在视窗中的显示方式
    visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloudName);
    // 设置XYZ三个坐标轴的大小和长度，该值也可以缺省
    // 查看复杂的点云图像会让用户没有方向感，为了让用户保持正确的方向判断，需要显示坐标轴。三个坐标轴X（R，红色）
    // Y（G，绿色）Z（B，蓝色）分别用三种不同颜色的圆柱体代替
    // visualizer->addCoordinateSystem(1.0);
    // 通过设置相机参数是用户从默认的角度和方向观察点
    visualizer->initCameraParameters();
    // 设置窗口viewer的背景颜色
    visualizer->setBackgroundColor(0, 0, 0);
    visualizer->setPosition(0, 0);
    visualizer->setSize(color.cols, color.rows);
    visualizer->setShowFPS(true);
    visualizer->setCameraPosition(0, 0, 0, 0, -1, 0);
    visualizer->registerKeyboardCallback(&Posepoint::keyboardEvent, *this);
    saveCloud(cloud);
    // visualizer->spinOnce(300);
    while (!visualize_stop) {
        visualizer->spinOnce(100);
    }

    visualizer->close();
  }
    void createCloud(const cv::Mat &depth, const cv::Mat &color, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
  {
    int body_part;
    const float badPoint = std::numeric_limits<float>::quiet_NaN();
    // FILE *fp = NULL;
    // fp = fopen("/home/hts/catkin_ws/test.txt", "a");
    #pragma omp parallel for
    for(int r = 0; r < depth.rows; ++r)
    {
      // 创建点云row行，每一行有col列
      pcl::PointXYZRGBA *itP = &cloud->points[r * depth.cols];
      // ptr函数访问任意一行像素的首地址
      const uint16_t *itD = depth.ptr<uint16_t>(r);
      const cv::Vec3b *itC = color.ptr<cv::Vec3b>(r);
      const float y = lookupY.at<float>(0, r);
      const float *itX = lookupX.ptr<float>();

      for(size_t c = 0; c < (size_t)depth.cols; ++c, ++itP, ++itD, ++itC, ++itX)
      {
        bool picked = false;
          if(int(pick_points[0]) == r && size_t(pick_points[1]) == c)
        {
            // cout<<r<<"   "<<c<<"Pelv_succ"<<endl;
            // cout<<"Pelv"<<endl;
            picked = true;
            body_part = 0;
        }
          if(int(pick_points[2]) == r && size_t(pick_points[3]) == c)
        {
            // cout<<r<<"   "<<c<<"Rshoulder_succ"<<endl;
            // cout<<"Rshoulder"<<endl;
            picked = true;
            body_part = 1;
        }
          if(int(pick_points[4]) == r && size_t(pick_points[5]) == c)
        {
            // cout<<r<<"   "<<c<<"LShoulder_succ"<<endl;
            // cout<<"Lshoulder"<<endl;
            picked = true;
            body_part = 2;
        }
        if (picked)
        {
          // cout<<"picked"<<endl;
          // cout<<body_part<<endl;
          register const float depthValue = *itD / 1000.0f;
          // Check for invalid measurements
          if(*itD == 0)
          {
            // not valid
            itP->x = itP->y = itP->z = badPoint;
            itP->rgba = 0;
            continue;
          }
          itP->z = depthValue;
          itP->x = *itX * depthValue;
          itP->y = y * depthValue;
          itP->b = itC->val[0];
          itP->g = itC->val[1];
          itP->r = itC->val[2];
          itP->a = 255;
          // fprintf(fp, "%f,%f,%f,%d,%d,%d,%d\n", itP->z,itP->x,itP->y,itP->b,itP->g,itP->r,itP->a);
          switch(body_part)
          {
          case 0:;
                Lshoulder(0) = itP->x; Lshoulder(1) = itP->y; Lshoulder(2) = itP->z;
                // cout<<"Pelv"<<endl;
                // cout<<"Pelv"<<"  "<<Lshoulder(0)<<" "<<Lshoulder(1)<<" "<<Lshoulder(2)<<endl;
                break;
          case 1:
                Rshoulder(0) = itP->x; Rshoulder(1) = itP->y; Rshoulder(2) = itP->z;
                // cout<<"Rshoulder"<<endl;
                // cout<<"Rshoulder"<<"  "<<Rshoulder(0)<<" "<<Rshoulder(1)<<" "<<Rshoulder(2)<<endl;
                break;
          case 2:
                Pelv(0) = itP->x; Pelv(1) = itP->y; Pelv(2) = itP->z;
                // cout<<"Lshoulder"<<endl;
                // cout<<"Lshoulder"<<"  "<<Pelv(0)<<" "<<Pelv(1)<<" "<<Pelv(2)<<endl;
                break;
          }
        }
        else
        {
            itP->x = itP->y = itP->z = badPoint;
            itP->rgba = 0;
            continue;
        }

      }
    }
    // fclose(fp);
  }
    void saveCloud(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud)
  {
    std::ostringstream oss;
    oss.str("");
    oss << "/home/hts/kinect_pic/" << std::setfill('0') << std::setw(4);
    const std::string baseName = oss.str();
    const std::string cloudName = baseName + "_cloud.pcd";
    OUT_INFO("saving cloud: " << cloudName);
     // writer是该类的pcl::PCDWriter类型的成员变量
    writer.writeBinary(cloudName, *cloud);
  }
    void keyboardEvent(const pcl::visualization::KeyboardEvent &event, void *)
  {
    if(event.keyUp())
    {
      switch(event.getKeyCode())
      {
      case 27:
      case 'q':
        visualize_stop = true;
        break;
      }
    }
  }
  //叉积
  Vector3f ThreeCross()
  {
    Vector3f a, b,result;
    a=Lshoulder-Pelv;
    b=Rshoulder-Pelv;
    cout<<a<<"  "<<b<<endl;
    result = a.cross(b);
    cout<<result<<endl;
    // Eigen::Vector3d v3(0, 0, 0);
	  // v3.x() = 1;
	  // v3[2] = 1;
	  AngleAxisd angle_axis3(pi *25/ 18, Eigen::Vector3d(1, 0, 0));//1系绕x轴逆时针旋转250得到2系
    // angle_axis3.matrix().cast<float>()
	  Vector3f rotated_result = angle_axis3.matrix().cast<float>()*result;
	  cout << "绕x轴顺时针旋转250°(R12):" << endl << angle_axis3.matrix() << endl;
	  cout << "旋转后:" << endl << rotated_result.transpose() << endl;
    return result;
  }
};
int main(int argc, char**argv)
{
  std::string color_path = argv[1];
  std::string depth_path = argv[2];
  Posepoint posepoint;
  posepoint.start(color_path, depth_path);
  posepoint.ThreeCross();

  // Vector3f Lshoulder;
  // Lshoulder<<0,0,0;
  // Lshoulder(0)=1;
}


/*
Brian Staber (brian.staber@gmail.com)
*/

#include "Epetra_ConfigDefs.h"
#ifdef HAVE_MPI
#include "mpi.h"
#include "Epetra_MpiComm.h"
#else
#include "Epetra_SerialComm.h"
#endif

#include "Teuchos_CommandLineProcessor.hpp"
#include "Teuchos_StandardCatchMacros.hpp"
#include "Teuchos_ParameterList.hpp"
#include "Teuchos_XMLParameterListCoreHelpers.hpp"

#include "neumannInnerSurface_StochasticPolyconvexHGO.hpp"
#include <boost/math/special_functions/gamma.hpp>

#include "shinozukapp.hpp"

int main(int argc, char *argv[]){

    std::string    xmlInFileName = "";
    std::string    extraXmlFile = "";
    std::string    xmlOutFileName = "paramList.out";

    Teuchos::CommandLineProcessor  clp(false);
    clp.setOption("xml-in-file",&xmlInFileName,"The XML file to read into a parameter list");
    clp.setDocString("TO DO.");

    Teuchos::CommandLineProcessor::EParseCommandLineReturn
    parse_return = clp.parse(argc,argv);
    if( parse_return != Teuchos::CommandLineProcessor::PARSE_SUCCESSFUL ) {
        std::cout << "\nEnd Result: TEST FAILED" << std::endl;
        return parse_return;
    }

#ifdef HAVE_MPI
    MPI_Init(&argc, &argv);
    Epetra_MpiComm Comm(MPI_COMM_WORLD);
#else
    Epetra_SerialComm Comm;
#endif

    Teuchos::RCP<Teuchos::ParameterList> paramList = Teuchos::rcp(new Teuchos::ParameterList);
    if(xmlInFileName.length()) {
        Teuchos::updateParametersFromXmlFile(xmlInFileName, inoutArg(*paramList));
    }

    if (Comm.MyPID()==0){
        paramList->print(std::cout,2,true,true);
    }

    Teuchos::RCP<neumannInnerSurface_StochasticPolyconvexHGO> my_interface
    = Teuchos::rcp(new neumannInnerSurface_StochasticPolyconvexHGO(Comm,*paramList));

    std::ifstream parameters_file_1, parameters_file_2, parameters_file_3, parameters_file_4;

    std::string path1 = "/Users/Brian/Documents/Thesis/Trilinos_results/arteries/gmrf_neumann/a100_gamma3_delta010/";
    parameters_file_1.open(path1+"w1.txt");
    parameters_file_2.open(path1+"w2.txt");
    parameters_file_3.open(path1+"w3.txt");
    parameters_file_4.open(path1+"w4.txt");

    unsigned int n_cells_p1_med = 297828;
    unsigned int n_nodes_p1_med = 58464;

    Epetra_Map StandardMap(int(n_nodes_p1_med),0,Comm);

    Epetra_Vector c1(StandardMap);
    Epetra_Vector c2(StandardMap);
    Epetra_Vector u1(StandardMap);
    Epetra_Vector mu4(StandardMap);

    std::string path = "/Users/Brian/Documents/Thesis/Trilinos/arteries/mesh/connectivity_p1_media.txt";
    my_interface->get_media(n_cells_p1_med,n_nodes_p1_med,path);

    if (parameters_file_1.is_open() && parameters_file_2.is_open() && parameters_file_3.is_open() && parameters_file_4.is_open()){

        for (unsigned nmc=0; nmc<1; ++nmc){
            for (int i=0; i<n_nodes_p1_med; ++i){
                parameters_file_1 >> my_interface->w1_gmrf(i);
                parameters_file_2 >> my_interface->w2_gmrf(i);
                parameters_file_3 >> my_interface->w3_gmrf(i);
                parameters_file_4 >> my_interface->w4_gmrf(i);
            }
            for (int j=0; j<n_nodes_p1_med; ++j){
                if (StandardMap.MyGID(j)){
                    c1[StandardMap.LID(j)]  = my_interface->icdf_gamma(my_interface->w1_gmrf[j],my_interface->alpha1,my_interface->alpha2);
                    c2[StandardMap.LID(j)]  = my_interface->icdf_gamma(my_interface->w2_gmrf[j],my_interface->alpha3,my_interface->alpha4);
                    u1[StandardMap.LID(j)]  = my_interface->icdf_beta(my_interface->w3_gmrf[j],my_interface->tau1,my_interface->tau2);
                    mu4[StandardMap.LID(j)] = my_interface->icdf_gamma(my_interface->w4_gmrf[j],my_interface->alpha5,my_interface->alpha6);
                }
            }
        }
        Comm.Barrier();
        parameters_file_1.close();
        parameters_file_2.close();
        parameters_file_3.close();
        parameters_file_4.close();
    }
    else{
        std::cout << "Couldn't open one of the parameters_file.\n";
    }

    int error;
    int NumTargetElements = 0;
    if (Comm.MyPID()==0){
        NumTargetElements = n_nodes_p1_med;
    }
    Epetra_Map MapOnRoot(-1,NumTargetElements,0,Comm);
    Epetra_Export ExportOnRoot(StandardMap,MapOnRoot);
    Epetra_MultiVector lhs_root(MapOnRoot,true);
    lhs_root.Export(c1,ExportOnRoot,Insert);
    std::string filename = "/Users/Brian/Documents/Thesis/Trilinos_results/arteries/boost_gmrf/gmrf_nodes/c1_test.mtx";
    error = EpetraExt::MultiVectorToMatrixMarketFile(filename.c_str(),lhs_root,0,0,false);

    lhs_root.PutScalar(0.0);
    lhs_root.Export(c2,ExportOnRoot,Insert);
    filename = "/Users/Brian/Documents/Thesis/Trilinos_results/arteries/boost_gmrf/gmrf_nodes/c2_test.mtx";
    error = EpetraExt::MultiVectorToMatrixMarketFile(filename.c_str(),lhs_root,0,0,false);

    lhs_root.PutScalar(0.0);
    lhs_root.Export(u1,ExportOnRoot,Insert);
    filename = "/Users/Brian/Documents/Thesis/Trilinos_results/arteries/boost_gmrf/gmrf_nodes/u1_test.mtx";
    error = EpetraExt::MultiVectorToMatrixMarketFile(filename.c_str(),lhs_root,0,0,false);

    lhs_root.PutScalar(0.0);
    lhs_root.Export(mu4,ExportOnRoot,Insert);
    filename = "/Users/Brian/Documents/Thesis/Trilinos_results/arteries/boost_gmrf/gmrf_nodes/mu4_test.mtx";
    error = EpetraExt::MultiVectorToMatrixMarketFile(filename.c_str(),lhs_root,0,0,false);

#ifdef HAVE_MPI
    MPI_Finalize();
#endif
return 0;

}


#include "CollisionDetection.h"
#include "RigidBodyInstance.h"
#include "RigidBodyTemplate.h"
#include <Eigen/Core>
#include "VectorMath.h"
#include <Eigen/Geometry>
#include <iostream>

using namespace std;

struct BBox
{
    double mins[3];
    double maxs[3];
};

bool intersects(const BBox &b1, const BBox &b2)
{
    for(int i=0; i<3; i++)
    {
        if( (b1.maxs[i] < b2.mins[i]) || (b2.maxs[i] < b1.mins[i]))
            return false;
    }
    return true;
}

struct AABBNode
{
    AABBNode() : left(NULL), right(NULL), childtet(-1) {}
    ~AABBNode() {delete left; delete right;}

    AABBNode *left;
    AABBNode *right;
    BBox box;
    int childtet;
};

class NodeComparator
{
public:
    NodeComparator(int axis) : axis(axis) {}

    int axis;

    bool operator()(const AABBNode *left, const AABBNode *right) const
    {
        return left->box.mins[axis] < right->box.mins[axis];
    }
};

AABBNode *buildAABB(vector<AABBNode *> nodes)
{
    if(nodes.size() == 0)
        return NULL;
    else if(nodes.size() == 1)
        return nodes[0];

    double axismins[3];
    double axismaxs[3];
    for(int i=0; i<3; i++)
    {
        axismins[i] = numeric_limits<double>::infinity();
        axismaxs[i] = -numeric_limits<double>::infinity();
    }
    int nnodes = (int)nodes.size();
    for(int i=0; i<nnodes; i++)
    {
        for(int j=0; j<3; j++)
        {
            axismins[j] = min(axismins[j], nodes[i]->box.mins[j]);
            axismaxs[j] = max(axismaxs[j], nodes[i]->box.maxs[j]);
        }
    }
    double widths[3];
    for(int i=0; i<3; i++)
        widths[i] = axismaxs[i] - axismins[i];
    int splitaxis = -1;
    if(widths[0] >= widths[1] && widths[0] >= widths[2])
        splitaxis = 0;
    else if(widths[1] >= widths[0] && widths[1] >= widths[2])
        splitaxis = 1;
    else
        splitaxis = 2;
    std::sort(nodes.begin(), nodes.end(), NodeComparator(splitaxis));
    vector<AABBNode *> left(nnodes/2);
    vector<AABBNode *> right(nnodes - nnodes/2);
    for(int i=0; i<nnodes/2; i++)
    {
        left[i] = nodes[i];
    }
    for(int i=nnodes/2; i<nnodes; i++)
    {
        right[i-nnodes/2] = nodes[i];
    }
    AABBNode *node = new AABBNode;
    node->left = buildAABB(left);
    node->right = buildAABB(right);
    for(int i=0; i<3; i++)
    {
        node->box.mins[i] = min(node->left->box.mins[i], node->right->box.mins[i]);
        node->box.maxs[i] = max(node->left->box.maxs[i], node->right->box.maxs[i]);
    }
    return node;
}

void refitAABB(const RigidBodyInstance * instance, AABBNode *node)
{
    if(node->childtet != -1)
    {
        Eigen::Vector4i tet = instance->getTemplate().getTets().row(node->childtet);
        for(int k=0; k<3; k++)
        {
            node->box.mins[k] = numeric_limits<double>::infinity();
            node->box.maxs[k] = -numeric_limits<double>::infinity();
        }
        for(int k=0; k<4; k++)
        {
            Eigen::Vector3d point = instance->c + VectorMath::rotationMatrix(instance->theta)*instance->getTemplate().getVerts().row(tet[k]).transpose();
            for(int l=0; l<3; l++)
            {
                node->box.mins[l] = min(node->box.mins[l], point[l]);
                node->box.maxs[l] = max(node->box.maxs[l], point[l]);
            }
        }
    }
    else if(node->left && node->right)
    {
        refitAABB(instance, node->left);
        refitAABB(instance, node->right);
        for(int i=0; i<3; i++)
        {
            node->box.mins[i] = min(node->left->box.mins[i], node->right->box.mins[i]);
            node->box.maxs[i] = max(node->left->box.maxs[i], node->right->box.maxs[i]);
        }
    }
}

AABBNode *buildAABB(const RigidBodyInstance * instance)
{
    int ntets = (int)instance->getTemplate().getTets().rows();
    vector<AABBNode *> leaves(ntets);
    for(int j=0; j<ntets; j++)
    {
        AABBNode *leaf = new AABBNode;
        leaf->childtet = j;
        Eigen::Vector4i tet = instance->getTemplate().getTets().row(j);
        BBox box;
        for(int k=0; k<3; k++)
        {
            box.mins[k] = numeric_limits<double>::infinity();
            box.maxs[k] = -numeric_limits<double>::infinity();
        }
        for(int k=0; k<4; k++)
        {
            Eigen::Vector3d point = instance->c + VectorMath::rotationMatrix(instance->theta)*instance->getTemplate().getVerts().row(tet[k]).transpose();
            for(int l=0; l<3; l++)
            {
                box.mins[l] = min(box.mins[l], point[l]);
                box.maxs[l] = max(box.maxs[l], point[l]);
            }
        }
        leaf->box = box;
        leaves[j] = leaf;
    }
    return buildAABB(leaves);
}

bool vertInTet(const Eigen::Vector3d &p, const Eigen::Vector3d &q1, const Eigen::Vector3d &q2, const Eigen::Vector3d &q3, const Eigen::Vector3d &q4)
{
    if( (q2-p).cross(q3-p).dot(q4-p) < 0)
        return false;
    if( (p-q1).cross(q3-q1).dot(q4-q1) < 0)
        return false;
    if( (q2-q1).cross(p-q1).dot(q4-q1) < 0)
        return false;
    if( (q2-q1).cross(q3-q1).dot(p-q1) < 0)
        return false;
    return true;
}

void tetTetIntersect(const AABBNode *node1, const AABBNode *node2, int body1, int body2, const std::vector<RigidBodyInstance *> instances, std::set<Collision> &collisions)
{
    if(body1==body2)
        return;

    Eigen::Vector4i tet1 = instances[body1]->getTemplate().getTets().row(node1->childtet);
    Eigen::Vector4i tet2 = instances[body2]->getTemplate().getTets().row(node2->childtet);
    Eigen::Vector3d verts1[4];
    Eigen::Vector3d verts2[4];
    for(int i=0; i<4; i++)
    {
        verts1[i] = instances[body1]->c + VectorMath::rotationMatrix(instances[body1]->theta)*instances[body1]->getTemplate().getVerts().row(tet1[i]).transpose();
        verts2[i] = instances[body2]->c + VectorMath::rotationMatrix(instances[body2]->theta)*instances[body2]->getTemplate().getVerts().row(tet2[i]).transpose();
    }
    for(int i=0; i<4; i++)
    {
        if(vertInTet(verts1[i], verts2[0], verts2[1], verts2[2], verts2[3]))
        {
            Collision c;
            c.body1 = body1;
            c.body2 = body2;
            c.collidingVertex = tet1[i];
            c.collidingTet = node2->childtet;
            collisions.insert(c);
        }
        if(vertInTet(verts2[i], verts1[0], verts1[1], verts1[2], verts1[3]))
        {
            Collision c;
            c.body1 = body2;
            c.body2 = body1;
            c.collidingVertex = tet2[i];
            c.collidingTet = node1->childtet;
            collisions.insert(c);
        }
    }
}

void intersect(const AABBNode *node1, const AABBNode *node2, int body1, int body2, const std::vector<RigidBodyInstance *> instances, std::set<Collision> &collisions)
{
    if(!node1 || !node2)
        return;

    if(!intersects(node1->box, node2->box))
        return;

    if(node1->childtet != -1)
    {
        if(node2->childtet != -1)
        {
            tetTetIntersect(node1, node2, body1, body2, instances, collisions);
        }
        else
        {
            intersect(node1, node2->left, body1, body2, instances, collisions);
            intersect(node1, node2->right, body1, body2, instances, collisions);
        }
    }
    else
    {
        if(node2->childtet != -1)
        {
            intersect(node1->left, node2, body1, body2, instances, collisions);
            intersect(node1->right, node2, body1, body2, instances, collisions);
        }
        else
        {
            intersect(node1->left, node2->left, body1, body2, instances, collisions);
            intersect(node1->left, node2->right, body1, body2, instances, collisions);
            intersect(node1->right, node2->left, body1, body2, instances, collisions);
            intersect(node1->right, node2->right, body1, body2, instances, collisions);
        }
    }
}

void collisionDetection(const std::vector<RigidBodyInstance *> instances, std::set<Collision> &collisions)
{
    collisions.clear();
    int nbodies = (int)instances.size();

    for(int i=0; i<nbodies; i++)
    {
        refitAABB(instances[i], instances[i]->AABB);
    }

    for(int i=0; i<nbodies; i++)
    {
        for(int j=i+1; j<nbodies; j++)
        {
            intersect(instances[i]->AABB, instances[j]->AABB, i, j, instances, collisions);
        }
    }

    // floor

    for (int i=0; i<nbodies; i++)
    {
        int nverts = instances[i]->getTemplate().getVerts().rows();
        for (int j = 0; j < nverts; j++)
        {
            Eigen::Vector3d point = VectorMath::rotationMatrix(instances[i]->theta) * instances[i]->getTemplate().getVerts().row(j).transpose() + instances[i]->c;
            if (point[1] <= -1.0)
            {
                Collision c;
                c.body1 = i;
                c.body2 = -1;
                c.collidingTet = -1;
                c.collidingVertex = j;
                collisions.insert(c);
            }
        }
    }
}


#include "shortest_path.h"
#include <iostream>
#include <fstream>
#include <math.h>

#include <boost/algorithm/string.hpp>

namespace ShortestPath
{
    double eps = 1e-7;

    struct dwell
    {
        int xlabel;
        int ylabel;
        int xnode;
        int ynode;
        double overlap;
        double dist;

        dwell(int _xlabel, int _ylabel, int _xnode, int _ynode, double _overlap, double _dist)
        {
            xlabel = _xlabel;
            ylabel = _ylabel;
            xnode = _xnode;
            ynode = _ynode;
            overlap = _overlap;
            dist = _dist;
        }

        bool operator< (const dwell& that) const
        {
            return this->overlap > that.overlap || (fabs(this->overlap - that.overlap) < eps && this->dist < that.dist);
        }

        friend std::ostream& operator<< (std::ostream& out, const dwell& that)
        {
            out << "overlap " << that.xlabel << "[" << that.xnode << "] " << that.ylabel << "[" << that.ynode << "] = " << that.overlap;
            out << " distance = " << that.dist;
            return out;
        }
    };

    double euclidDistance(const Eigen::Vector3d& x, const Eigen::Vector3d& y)
    {
        Eigen::Vector3d z;
        z = x - y;
        return z.norm();
    }

    double arcDistance(const Eigen::Vector3d& x, const Eigen::Vector3d& y)
    {
        assert(fabs(x.norm() - 1.0) < 1e-7);
        assert(fabs(y.norm() - 1.0) < 1e-7);

        double costheta = x.dot(y);
        if (costheta < -1.0) costheta = -1.0;
        if (costheta > 1.0) costheta = 1.0;
        return acos(costheta);
    }

    std::vector<std::set<int>> init(const std::string& fileName)
    {
        std::ifstream poseKeypointResult("pose_keypoint_200_" + fileName + ".txt");
        std::vector<std::set<int>> visibleResult;
        std::string s;
        while (getline(poseKeypointResult, s))
        {
            std::set<int> set;
            getline(poseKeypointResult, s);
            std::vector<std::string> indices;
            boost::split(indices, s, boost::is_any_of(" "));
            for (const std::string& id: indices)
            {
                if (id.size() > 0)
                {
                    set.insert(std::stoi(id));
                }
            }
            visibleResult.push_back(set);
        }
        poseKeypointResult.close();
        return visibleResult;
    }

    double calcOverlap(const std::set<int>& A, const std::set<int>& B)
    {
        std::vector<int> i, u;
        std::set_intersection(A.begin(), A.end(), B.begin(), B.end(), std::back_inserter(i));
        std::set_union(A.begin(), A.end(), B.begin(), B.end(), std::back_inserter(u));
        return (double)i.size() / (double)u.size();
    }

    std::vector<Eigen::Vector3d> getView(const Eigen::Vector3d& camera_normal, int horizontal_count, int vertical_count)
    {
        std::vector<Eigen::Vector3d> views;

        double horizontal_angle = 2 * M_PI / horizontal_count;
        double vertical_angle = M_PI / (vertical_count - 1);

        for (int i = 0; i < vertical_count; ++i)
        {
            for (int j = 0; j < horizontal_count; ++j)
            {
                if ((i == 0 || i == vertical_count - 1) && j)
                {
                    continue;
                }

                double horizontal_spin = horizontal_angle * j;
                double vertical_spin = vertical_angle * i;

                Eigen::Matrix3d rotation, rotation_inv;
                rotation = Eigen::AngleAxisd(vertical_spin, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(horizontal_spin, camera_normal);
                rotation_inv = rotation.inverse();
                views.push_back(rotation_inv * camera_normal);
            }
        }
        return views;
    }

    void execute(const std::string& fileName)
    {
        std::cout << "name = " << fileName << std::endl;
        std::vector<std::set<int>> group = init(fileName);
        std::vector<Eigen::Vector3d> views = getView(Eigen::Vector3d(0, 0, -1), 18, 13);

        std::vector<int> selected_views = {31, 23, 84, 57, 94, 148, 179, 171, 50, 35, 26};

        int ecnt = 0;
        std::vector<dwell> dummy;

        int n = selected_views.size();
        double** g = new double*[n];
        for (int i = 0; i < n; ++i)
        {
            g[i] = new double[n];
        }
        double** f = new double*[1 << n];
        for (int i = 0; i < (1 << n); ++i)
        {
            f[i] = new double[n];
            for (int j = 0; j < n; ++j)
            {
                f[i][j] = 1e10;
            }
        }
        int** fa = new int*[1 << n];
        for (int i = 0; i < (1 << n); ++i)
        {
            fa[i] = new int[n];
            for (int j = 0; j < n; ++j)
            {
                fa[i][j] = -1;
            }
        }

        for (int i = 0; i < selected_views.size(); ++i)
        {
            g[i][i] = 0.0;
            for (int j = i + 1; j < selected_views.size(); ++j)
            {
                int x = selected_views[i];
                int y = selected_views[j];
                double overlap = calcOverlap(group[x], group[y]);
                double dist = arcDistance(views[x], views[y]);
                g[i][j] = g[j][i] = dist;
                /* if (overlap + eps >= 0.5 || dist <= 2.0 + eps)
                {
                    ++ecnt;
                    std::cout << "overlap " << i << "[" << x << "] " << j << "[" << y << "] = ";
                    std::cout << calcOverlap(group[x], group[y]) << "(" << group[x].size() << ", " << group[y].size() << ")";
                    std::cout << " distance = " << arcDistance(views[x], views[y]);
                    std::cout << std::endl;
                } */
                dummy.push_back(dwell(i, j, x, y, overlap, dist));
            }
        }
        std::sort(dummy.begin(), dummy.end());
        for (const dwell& dd: dummy)
        {
            std::cout << dd << std::endl;
        }

        // std::cout << "edge count = " << ecnt << std::endl;

        /*for (int selected_view: selected_views)
        {
            Eigen::Vector3d vv = views[selected_view];
            std::cout << "view " << selected_view << ": [" << vv[0] << ", " << vv[1] << ", " << vv[2] << "]" << std::endl;
        }*/

        // TSP

        for (int i = 0; i < n; ++i)
        {
            f[0][i] = 0.0;
        }
        for (int i = 1; i < (1 << n); ++i)
        {
            for (int v = 0; v < n; ++v)
            {
                if (i & (1 << v))
                {
                    // f[i][v] = ...
                    for (int u = 0; u < n; ++u)
                    {
                        double dist = f[i - (1 << v)][u] + g[u][v];
                        if (dist < f[i][v])
                        {
                            f[i][v] = dist;
                            fa[i][v] = u;
                        }
                    }
                }
            }
        }

        std::vector<int> answer;
        double mindist = 1e10;
        int where = -1;
        for (int i = 0; i < n; ++i)
        {
            if (f[(1 << n) - 1][i] < mindist)
            {
                mindist = f[(1 << n) - 1][i];
                where = i;
            }
        }

        int curbin = (1 << n) - 1;
        int where0 = where;
        while (curbin)
        {
            answer.push_back(where0);
            int nxt = fa[curbin][where0];
            curbin -= (1 << where0);
            where0 = nxt;
        }

        std::cout << "minimum distance = " << f[(1 << n) - 1][where] << std::endl;
        for (int i = 0; i < answer.size() - 1; ++i)
        {
            int x = selected_views[answer[i]];
            int y = selected_views[answer[i + 1]];
            std::cout << answer[i] << "(" << x << ") " << answer[i + 1] << "(" << y << ")";
            std::cout << " dis = " << g[answer[i]][answer[i + 1]] << " overlap = " << calcOverlap(group[x], group[y]);
            std::cout << std::endl;
        }

    }
}


//
// Copyright (c) 2015-2019 CNRS INRIA
// Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
//

#ifndef __pinocchio_joint_translation_hpp__
#define __pinocchio_joint_translation_hpp__

#include "pinocchio/macros.hpp"
#include "pinocchio/multibody/joint/joint-base.hpp"
#include "pinocchio/multibody/constraint.hpp"
#include "pinocchio/spatial/inertia.hpp"
#include "pinocchio/spatial/skew.hpp"

namespace pinocchio
{

  template<typename Scalar, int Options=0> struct MotionTranslationTpl;
  typedef MotionTranslationTpl<double> MotionTranslation;

  template<typename Scalar, int Options>
  struct SE3GroupAction< MotionTranslationTpl<Scalar,Options> >
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename Scalar, int Options, typename MotionDerived>
  struct MotionAlgebraAction< MotionTranslationTpl<Scalar,Options>, MotionDerived>
  {
    typedef MotionTpl<Scalar,Options> ReturnType;
  };

  template<typename _Scalar, int _Options>
  struct traits< MotionTranslationTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
    typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
    typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
    typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
    typedef Vector3 AngularType;
    typedef Vector3 LinearType;
    typedef const Vector3 ConstAngularType;
    typedef const Vector3 ConstLinearType;
    typedef Matrix6 ActionMatrixType;
    typedef MotionTpl<Scalar,Options> MotionPlain;
    typedef MotionPlain PlainReturnType;
    enum {
      LINEAR = 0,
      ANGULAR = 3
    };
  }; // traits MotionTranslationTpl

  template<typename _Scalar, int _Options>
  struct MotionTranslationTpl
  : MotionBase< MotionTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    MOTION_TYPEDEF_TPL(MotionTranslationTpl);

    MotionTranslationTpl() {}

    template<typename Vector3Like>
    MotionTranslationTpl(const Eigen::MatrixBase<Vector3Like> & v)
    : m_v(v)
    {}

    MotionTranslationTpl(const MotionTranslationTpl & other)
    : m_v(other.m_v)
    {}

    Vector3 & operator()() { return m_v; }
    const Vector3 & operator()() const { return m_v; }

    inline PlainReturnType plain() const
    {
      return PlainReturnType(m_v,PlainReturnType::Vector3::Zero());
    }

    bool isEqual_impl(const MotionTranslationTpl & other) const
    {
      return m_v == other.m_v;
    }

    MotionTranslationTpl & operator=(const MotionTranslationTpl & other)
    {
      m_v = other.m_v;
      return *this;
    }

    template<typename Derived>
    void addTo(MotionDense<Derived> & other) const
    {
      other.linear() += m_v;
    }

    template<typename Derived>
    void setTo(MotionDense<Derived> & other) const
    {
      other.linear() = m_v;
      other.angular().setZero();
    }

    template<typename S2, int O2, typename D2>
    void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      v.angular().setZero();
      v.linear().noalias() = m.rotation() * m_v; // TODO: check efficiency
    }

    template<typename S2, int O2>
    MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3Action_impl(m,res);
      return res;
    }

    template<typename S2, int O2, typename D2>
    void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
    {
      // Linear
      v.linear().noalias() = m.rotation().transpose() * m_v;

      // Angular
      v.angular().setZero();
    }

    template<typename S2, int O2>
    MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
    {
      MotionPlain res;
      se3ActionInverse_impl(m,res);
      return res;
    }

    template<typename M1, typename M2>
    void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
    {
      // Linear
      mout.linear().noalias() = v.angular().cross(m_v);

      // Angular
      mout.angular().setZero();
    }

    template<typename M1>
    MotionPlain motionAction(const MotionDense<M1> & v) const
    {
      MotionPlain res;
      motionAction(v,res);
      return res;
    }

    const Vector3 & linear() const { return m_v; }
    Vector3 & linear() { return m_v; }

  protected:

    Vector3 m_v;

  }; // struct MotionTranslationTpl

  template<typename S1, int O1, typename MotionDerived>
  inline typename MotionDerived::MotionPlain
  operator+(const MotionTranslationTpl<S1,O1> & m1,
            const MotionDense<MotionDerived> & m2)
  {
    return typename MotionDerived::MotionPlain(m2.linear() + m1.linear(), m2.angular());
  }

  template<typename Scalar, int Options> struct TransformTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< TransformTranslationTpl<_Scalar,_Options> >
  {
    enum {
      Options = _Options,
      LINEAR = 0,
      ANGULAR = 3
    };
    typedef _Scalar Scalar;
    typedef SE3Tpl<Scalar,Options> PlainType;
    typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
    typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
    typedef typename Matrix3::IdentityReturnType AngularType;
    typedef AngularType AngularRef;
    typedef AngularType ConstAngularRef;
    typedef Vector3 LinearType;
    typedef LinearType & LinearRef;
    typedef const LinearType & ConstLinearRef;
    typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
    typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
  }; // traits TransformTranslationTpl

  template<typename Scalar, int Options>
  struct SE3GroupAction< TransformTranslationTpl<Scalar,Options> >
  { typedef typename traits <TransformTranslationTpl<Scalar,Options> >::PlainType ReturnType; };

  template<typename _Scalar, int _Options>
  struct TransformTranslationTpl
  : SE3Base< TransformTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    PINOCCHIO_SE3_TYPEDEF_TPL(TransformTranslationTpl);
    typedef typename traits<TransformTranslationTpl>::Vector3 Vector3;

    TransformTranslationTpl() {}

    template<typename Vector3Like>
    TransformTranslationTpl(const Eigen::MatrixBase<Vector3Like> & translation)
    : m_translation(translation)
    {}

    PlainType plain() const
    {
      PlainType res(PlainType::Identity());
      res.rotation().setIdentity();
      res.translation() = translation();

      return res;
    }

    operator PlainType() const { return plain(); }

    template<typename S2, int O2>
    typename SE3GroupAction<TransformTranslationTpl>::ReturnType
    se3action(const SE3Tpl<S2,O2> & m) const
    {
      typedef typename SE3GroupAction<TransformTranslationTpl>::ReturnType ReturnType;
      ReturnType res(m);
      res.translation() += translation();

      return res;
    }

    ConstLinearRef translation() const { return m_translation; }
    LinearRef translation() { return m_translation; }

    AngularType rotation() const { return AngularType(3,3); }

    bool isEqual(const TransformTranslationTpl & other) const
    {
      return m_translation == other.m_translation;
    }

  protected:

    LinearType m_translation;
  };

  template<typename Scalar, int Options> struct ConstraintTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< ConstraintTranslationTpl<_Scalar,_Options> >
  {
    typedef _Scalar Scalar;

    enum { Options = _Options };
    enum { LINEAR = 0, ANGULAR = 3 };

    typedef MotionTranslationTpl<Scalar,Options> JointMotion;
    typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
    typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;

    typedef DenseBase MatrixReturnType;
    typedef const DenseBase ConstMatrixReturnType;
  }; // traits ConstraintTranslationTpl

  template<typename _Scalar, int _Options>
  struct ConstraintTranslationTpl
  : ConstraintBase< ConstraintTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintTranslationTpl)

    enum { NV = 3 };

    ConstraintTranslationTpl() {}

//    template<typename S1, int O1>
//    Motion operator*(const MotionTranslationTpl<S1,O1> & vj) const
//    { return Motion(vj(), Motion::Vector3::Zero()); }

    template<typename Vector3Like>
    JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
    {
      EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
      return JointMotion(v);
    }

    int nv_impl() const { return NV; }

    struct ConstraintTranspose
    {
      const ConstraintTranslationTpl & ref;
      ConstraintTranspose(const ConstraintTranslationTpl & ref) : ref(ref) {}

      template<typename Derived>
      typename ForceDense<Derived>::ConstLinearType
      operator* (const ForceDense<Derived> & phi)
      {
        return phi.linear();
      }

      /* [CRBA]  MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
      template<typename MatrixDerived>
      const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
      operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
      {
        assert(F.rows()==6);
        return F.derived().template middleRows<3>(LINEAR);
      }

    }; // struct ConstraintTranspose

    ConstraintTranspose transpose () const { return ConstraintTranspose(*this); }

    DenseBase matrix_impl() const
    {
      DenseBase S;
      S.template middleRows<3>(LINEAR).setIdentity();
      S.template middleRows<3>(ANGULAR).setZero();
      return S;
    }

    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> M;
      M.template middleRows<3>(LINEAR) = m.rotation();
      M.template middleRows<3>(ANGULAR).setZero();

      return M;
    }

    template<typename S1, int O1>
    Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
    {
      Eigen::Matrix<S1,6,3,O1> M;
      M.template middleRows<3>(LINEAR) = m.rotation().transpose();
      M.template middleRows<3>(ANGULAR).setZero();

      return M;
    }

    template<typename MotionDerived>
    DenseBase motionAction(const MotionDense<MotionDerived> & m) const
    {
      const typename MotionDerived::ConstAngularType w = m.angular();

      DenseBase res;
      skew(w,res.template middleRows<3>(LINEAR));
      res.template middleRows<3>(ANGULAR).setZero();

      return res;
    }

    bool isEqual(const ConstraintTranslationTpl &) const { return true; }

  }; // struct ConstraintTranslationTpl

  template<typename MotionDerived, typename S2, int O2>
  inline typename MotionDerived::MotionPlain
  operator^(const MotionDense<MotionDerived> & m1,
            const MotionTranslationTpl<S2,O2> & m2)
  {
    return m2.motionAction(m1);
  }

  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
  template<typename S1, int O1, typename S2, int O2>
  inline Eigen::Matrix<S2,6,3,O2>
  operator*(const InertiaTpl<S1,O1> & Y,
            const ConstraintTranslationTpl<S2,O2> &)
  {
    typedef ConstraintTranslationTpl<S2,O2> Constraint;
    Eigen::Matrix<S2,6,3,O2> M;
    alphaSkew(Y.mass(),Y.lever(),M.template middleRows<3>(Constraint::ANGULAR));
    M.template middleRows<3>(Constraint::LINEAR).setZero();
    M.template middleRows<3>(Constraint::LINEAR).diagonal().fill(Y.mass ());

    return M;
  }

  /* [ABA] Y*S operator*/
  template<typename M6Like, typename S2, int O2>
  inline const typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
  operator*(const Eigen::MatrixBase<M6Like> & Y,
            const ConstraintTranslationTpl<S2,O2> &)
  {
    typedef ConstraintTranslationTpl<S2,O2> Constraint;
    return Y.derived().template middleCols<3>(Constraint::LINEAR);
  }

  template<typename S1, int O1>
  struct SE3GroupAction< ConstraintTranslationTpl<S1,O1> >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };

  template<typename S1, int O1, typename MotionDerived>
  struct MotionAlgebraAction< ConstraintTranslationTpl<S1,O1>,MotionDerived >
  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };

  template<typename Scalar, int Options> struct JointTranslationTpl;

  template<typename _Scalar, int _Options>
  struct traits< JointTranslationTpl<_Scalar,_Options> >
  {
    enum {
      NQ = 3,
      NV = 3
    };
    typedef _Scalar Scalar;
    enum { Options = _Options };
    typedef JointDataTranslationTpl<Scalar,Options> JointDataDerived;
    typedef JointModelTranslationTpl<Scalar,Options> JointModelDerived;
    typedef ConstraintTranslationTpl<Scalar,Options> Constraint_t;
    typedef TransformTranslationTpl<Scalar,Options> Transformation_t;
    typedef MotionTranslationTpl<Scalar,Options> Motion_t;
    typedef MotionZeroTpl<Scalar,Options> Bias_t;

    // [ABA]
    typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
    typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
    typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;

    PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE

    typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
    typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
  }; // traits JointTranslationTpl

  template<typename Scalar, int Options>
  struct traits< JointDataTranslationTpl<Scalar,Options> >
  { typedef JointTranslationTpl<Scalar,Options> JointDerived; };

  template<typename Scalar, int Options>
  struct traits< JointModelTranslationTpl<Scalar,Options> >
  { typedef JointTranslationTpl<Scalar,Options> JointDerived; };

  template<typename _Scalar, int _Options>
  struct JointDataTranslationTpl
  : public JointDataBase< JointDataTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointTranslationTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
    PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR

    Constraint_t S;
    Transformation_t M;
    Motion_t v;
    Bias_t c;

    // [ABA] specific data
    U_t U;
    D_t Dinv;
    UD_t UDinv;

    JointDataTranslationTpl()
    : M(Transformation_t::Vector3::Zero())
    , v(Motion_t::Vector3::Zero())
    , U(U_t::Zero())
    , Dinv(D_t::Zero())
    , UDinv(UD_t::Zero())
    {}

    static std::string classname() { return std::string("JointDataTranslation"); }
    std::string shortname() const { return classname(); }
  }; // struct JointDataTranslationTpl

  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTranslationTpl);
  template<typename _Scalar, int _Options>
  struct JointModelTranslationTpl
  : public JointModelBase< JointModelTranslationTpl<_Scalar,_Options> >
  {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef JointTranslationTpl<_Scalar,_Options> JointDerived;
    PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);

    typedef JointModelBase<JointModelTranslationTpl> Base;
    using Base::id;
    using Base::idx_q;
    using Base::idx_v;
    using Base::setIndexes;

    JointDataDerived createData() const { return JointDataDerived(); }

    template<typename ConfigVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs) const
    {
      data.M.translation() = this->jointConfigSelector(qs);
    }

    template<typename ConfigVector, typename TangentVector>
    void calc(JointDataDerived & data,
              const typename Eigen::MatrixBase<ConfigVector> & qs,
              const typename Eigen::MatrixBase<TangentVector> & vs) const
    {
      calc(data,qs.derived());

      data.v.linear() = this->jointVelocitySelector(vs);
    }

    template<typename Matrix6Like>
    void calc_aba(JointDataDerived & data,
                  const Eigen::MatrixBase<Matrix6Like> & I,
                  const bool update_I) const
    {
      data.U = I.template middleCols<3>(Inertia::LINEAR);

      // compute inverse
//      data.Dinv.setIdentity();
//      data.U.template middleRows<3>(Inertia::LINEAR).llt().solveInPlace(data.Dinv);
      internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::LINEAR),data.Dinv);

      data.UDinv.template middleRows<3>(Inertia::LINEAR).setIdentity(); // can be put in data constructor
      data.UDinv.template middleRows<3>(Inertia::ANGULAR).noalias() = data.U.template middleRows<3>(Inertia::ANGULAR) * data.Dinv;

      if (update_I)
      {
        Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
        I_.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR)
        -= data.UDinv.template middleRows<3>(Inertia::ANGULAR) * I_.template block<3,3>(Inertia::LINEAR, Inertia::ANGULAR);
        I_.template middleCols<3>(Inertia::LINEAR).setZero();
        I_.template block<3,3>(Inertia::LINEAR,Inertia::ANGULAR).setZero();
      }
    }

    static std::string classname() { return std::string("JointModelTranslation"); }
    std::string shortname() const { return classname(); }

    /// \returns An expression of *this with the Scalar type casted to NewScalar.
    template<typename NewScalar>
    JointModelTranslationTpl<NewScalar,Options> cast() const
    {
      typedef JointModelTranslationTpl<NewScalar,Options> ReturnType;
      ReturnType res;
      res.setIndexes(id(),idx_q(),idx_v());
      return res;
    }

  }; // struct JointModelTranslationTpl

} // namespace pinocchio

#include <boost/type_traits.hpp>

namespace boost
{
  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointModelTranslationTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointModelTranslationTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_constructor< ::pinocchio::JointDataTranslationTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};

  template<typename Scalar, int Options>
  struct has_nothrow_copy< ::pinocchio::JointDataTranslationTpl<Scalar,Options> >
  : public integral_constant<bool,true> {};
}

#endif // ifndef __pinocchio_joint_translation_hpp__


/*
	Copyright (C) 2003-2013 by David White <davewx7@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 2 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef DECIMAL_HPP_INCLUDED
#define DECIMAL_HPP_INCLUDED

#include <string>

#include <iosfwd>
#include <boost/cstdint.hpp>
#if defined(TARGET_BLACKBERRY)
#include <math.h>
#endif

static const int64_t DECIMAL_PRECISION = 1000000;
static const int64_t DECIMAL_PLACES = 6;

class decimal
{
public:
	static decimal from_string(const std::string& s);
	static decimal from_int(int v) { return decimal(v); }
	static decimal from_raw_value(int64_t v) { decimal d; d.value_ = v; return d; }
	static decimal epsilon() { return decimal::from_raw_value(static_cast<int64_t>(1)); }
	decimal() : value_(0) {}
	explicit decimal(int value) : value_(int64_t(value)*DECIMAL_PRECISION) {}
#if defined(TARGET_BLACKBERRY)
	explicit decimal(double value) : value_(llround(value*DECIMAL_PRECISION)) {}
#else
	explicit decimal(double value) : value_(int64_t(value*DECIMAL_PRECISION)) {}
#endif

	int64_t value() const { return value_; }
	int as_int() const { return int( value_/DECIMAL_PRECISION ); }
	double as_float() const { return value_/double(DECIMAL_PRECISION); }
	int64_t fractional() const { return value_%DECIMAL_PRECISION; }

	decimal operator-() const {
		return decimal(from_raw_value(-value_));
	}

	friend decimal operator+(const decimal& a, const decimal& b);
	friend decimal operator-(const decimal& a, const decimal& b);
	friend decimal operator*(const decimal& a, const decimal& b);
	friend decimal operator/(const decimal& a, const decimal& b);

	void operator+=(decimal a) { *this = *this + a; }
	void operator-=(decimal a) { *this = *this - a; }
	void operator*=(decimal a) { *this = *this * a; }
	void operator/=(decimal a) { *this = *this / a; }

	void operator+=(int a) { operator+=(decimal::from_int(a)); }
	void operator-=(int a) { operator-=(decimal::from_int(a)); }
	void operator*=(int a) { operator*=(decimal::from_int(a)); }
	void operator/=(int a) { operator/=(decimal::from_int(a)); }

private:
	int64_t value_;
};

inline decimal operator+(const decimal& a, const decimal& b) {
	return decimal::from_raw_value(a.value() + b.value());
}

inline decimal operator-(const decimal& a, const decimal& b) {
	return decimal::from_raw_value(a.value() - b.value());
}

decimal operator*(const decimal& a, const decimal& b);
decimal operator/(const decimal& a, const decimal& b);

inline bool operator==(const decimal& a, const decimal& b) {
	return a.value() == b.value();
}

inline bool operator!=(const decimal& a, const decimal& b) {
	return !operator==(a, b);
}

inline bool operator<=(const decimal& a, const decimal& b) {
	return a.value() <= b.value();
}

inline bool operator>=(const decimal& a, const decimal& b) {
	return b <= a;
}

inline bool operator<(const decimal& a, const decimal& b) {
	return !(b <= a);
}

inline bool operator>(const decimal& a, const decimal& b) {
	return !(a <= b);
}

inline decimal operator+(decimal a, int b) { return operator+(a, decimal::from_int(b)); }
inline decimal operator-(decimal a, int b) { return operator-(a, decimal::from_int(b)); }
inline decimal operator*(decimal a, int b) { return operator*(a, decimal::from_int(b)); }
inline decimal operator/(decimal a, int b) { return operator/(a, decimal::from_int(b)); }
inline bool operator<(decimal a, int b) { return operator<(a, decimal::from_int(b)); }
inline bool operator>(decimal a, int b) { return operator>(a, decimal::from_int(b)); }
inline bool operator<=(decimal a, int b) { return operator<=(a, decimal::from_int(b)); }
inline bool operator>=(decimal a, int b) { return operator>=(a, decimal::from_int(b)); }

inline decimal operator+(int a, decimal b) { return operator+(decimal::from_int(a), b); }
inline decimal operator-(int a, decimal b) { return operator-(decimal::from_int(a), b); }
inline decimal operator*(int a, decimal b) { return operator*(decimal::from_int(a), b); }
inline decimal operator/(int a, decimal b) { return operator/(decimal::from_int(a), b); }
inline bool operator<(int a, decimal b) { return operator<(decimal::from_int(a), b); }
inline bool operator>(int a, decimal b) { return operator>(decimal::from_int(a), b); }
inline bool operator<=(int a, decimal b) { return operator<=(decimal::from_int(a), b); }
inline bool operator>=(int a, decimal b) { return operator>=(decimal::from_int(a), b); }

std::ostream& operator<<(std::ostream& s, decimal d);

#endif


#include <iostream>
#include <iterator>
#include <iomanip>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <cmath>
#ifdef _OPENMP
#include <omp.h>
#endif

#include <boost/scope_exit.hpp>

#include <boost/program_options.hpp>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

#include <amgcl/amg.hpp>
#include <amgcl/coarsening/runtime.hpp>
#include <amgcl/relaxation/runtime.hpp>
#include <amgcl/mpi/subdomain_deflation.hpp>
#include <amgcl/mpi/solver/runtime.hpp>
#include <amgcl/mpi/direct_solver/runtime.hpp>
#include <amgcl/profiler.hpp>

namespace amgcl {
    profiler<> prof;
}

using amgcl::precondition;

//---------------------------------------------------------------------------
std::vector<ptrdiff_t> read_problem(
        const amgcl::mpi::communicator &world,
        const std::string &A_file,
        const std::string &rhs_file,
        const std::string &part_file,
        int block_size,
        std::vector<ptrdiff_t> &ptr,
        std::vector<ptrdiff_t> &col,
        std::vector<double>    &val,
        std::vector<double>    &rhs
        )
{
    // Read partition
    std::vector<int> part;
    std::vector<ptrdiff_t> domain(world.size + 1, 0);
    ptrdiff_t n = 0;
    std::string line;

    {
        std::ifstream f(part_file.c_str());
        precondition(f, "Failed to open part file (" + part_file + ")");

        while (std::getline(f, line)) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            ptrdiff_t cols;
            precondition(is >> n >> cols, "Unsupported format in matrix file");
            break;
        }

        precondition(n, "Zero sized partition vector");
        precondition(n % block_size == 0, "Matrix size is not divisible by block_size");

        part.reserve(n);

        while (std::getline(f, line)) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            int p;
            precondition(is >> p, "Unsupported format in part file");
            precondition(p < world.size, "MPI world does not correspond to partition");

            part.push_back(p);
            ++domain[p+1];
        }

        std::partial_sum(domain.begin(), domain.end(), domain.begin());
    }

    ptrdiff_t chunk_beg = domain[world.rank];
    ptrdiff_t chunk_end = domain[world.rank + 1];
    ptrdiff_t chunk     = chunk_end - chunk_beg;

    // Reorder unknowns
    std::vector<ptrdiff_t> order(n);
    {
        for(ptrdiff_t i = 0; i < n; ++i) {
            int p = part[i];
            int j = domain[p]++;

            order[i] = j;
        }

        std::rotate(domain.begin(), domain.end()-1, domain.end());
        domain[0] = 0;
    }

    // Read matrix chunk
    {
        std::ifstream A(A_file.c_str());
        precondition(A, "Failed to open matrix file (" + A_file + ")");

        ptrdiff_t nnz = 0;
        while ( std::getline(A, line) ) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            ptrdiff_t rows, m;
            precondition(is >> rows >> m >> nnz, "Unsupported format in matrix file");
            precondition(rows == n, "Matrix and partition have incompatible sizes");
            precondition(n == m, "Non-square matrix in matrix file");
            break;
        }

        {
            std::vector<ptrdiff_t> I, J;
            std::vector<double>    V;

            ptr.clear();
            ptr.resize(chunk + 1, 0);

            while (std::getline(A, line)) {
                if (line[0] == '%') continue;
                std::istringstream is(line);
                ptrdiff_t i, j;
                double v;
                precondition(is >> i >> j >> v, "Unsupported format in matrix file");
                --i;
                --j;

                if (part[i] != world.rank) continue;

                ++ptr[order[i] + 1 - chunk_beg];

                I.push_back(order[i] - chunk_beg);
                J.push_back(order[j]);
                V.push_back(v);
            }

            std::partial_sum(ptr.begin(), ptr.end(), ptr.begin());

            ptrdiff_t loc_nnz = ptr.back();

            col.clear(); col.resize(loc_nnz);
            val.clear(); val.resize(loc_nnz);

            for(ptrdiff_t i = 0; i < loc_nnz; ++i) {
                ptrdiff_t row = I[i];
                col[ptr[row]] = J[i];
                val[ptr[row]] = V[i];
                ++ptr[row];
            }
            std::rotate(ptr.begin(), ptr.end()-1, ptr.end());
            ptr[0] = 0;
        }
    }

    // Read RHS chunk.
    {
        std::ifstream f(rhs_file.c_str());
        precondition(f, "Failed to open rhs file (" + rhs_file + ")");

        while (std::getline(f, line)) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            ptrdiff_t rows, cols;
            precondition(is >> rows >> cols, "Unsupported format in matrix file");
            precondition(rows == n, "RHS size should coincide with matrix size");
            break;
        }

        rhs.clear();
        rhs.reserve(chunk);

        ptrdiff_t pos = 0;
        while (std::getline(f, line)) {
            if (line[0] == '%') continue;
            std::istringstream is(line);
            double v;
            precondition(is >> v, "Unsupported format in RHS file");

            if (part[pos++] != world.rank) continue;

            rhs.push_back(v);
        }

        assert(rhs.size() + 1 == ptr.size());
    }

    return domain;
}

//---------------------------------------------------------------------------
int main(int argc, char *argv[]) {
    int provided;
    MPI_Init_thread(&argc, &argv, MPI_THREAD_MULTIPLE, &provided);
    BOOST_SCOPE_EXIT(void) {
        MPI_Finalize();
    } BOOST_SCOPE_EXIT_END

    amgcl::mpi::communicator world(MPI_COMM_WORLD);

    if (world.rank == 0)
        std::cout << "World size: " << world.size << std::endl;

    // Read configuration from command line
    amgcl::runtime::coarsening::type    coarsening       = amgcl::runtime::coarsening::smoothed_aggregation;
    amgcl::runtime::relaxation::type    relaxation       = amgcl::runtime::relaxation::spai0;
    amgcl::runtime::solver::type        iterative_solver = amgcl::runtime::solver::bicgstabl;
    amgcl::runtime::mpi::direct::type   direct_solver    = amgcl::runtime::mpi::direct::skyline_lu;
    std::string parameter_file;
    std::string A_file    = "A.mtx";
    std::string rhs_file  = "b.mtx";
    std::string part_file = "partition.mtx";
    std::string out_file;

    namespace po = boost::program_options;
    po::options_description desc("Options");

    desc.add_options()
        ("help,h", "show help")
        (
         "coarsening,c",
         po::value<amgcl::runtime::coarsening::type>(&coarsening)->default_value(coarsening),
         "ruge_stuben, aggregation, smoothed_aggregation, smoothed_aggr_emin"
        )
        (
         "relaxation,r",
         po::value<amgcl::runtime::relaxation::type>(&relaxation)->default_value(relaxation),
         "gauss_seidel, ilu0, damped_jacobi, spai0, chebyshev"
        )
        (
         "iter_solver,i",
         po::value<amgcl::runtime::solver::type>(&iterative_solver)->default_value(iterative_solver),
         "cg, bicgstab, bicgstabl, gmres"
        )
        (
         "dir_solver,d",
         po::value<amgcl::runtime::mpi::direct::type>(&direct_solver)->default_value(direct_solver),
         "skyline_lu"
#ifdef AMGCL_HAVE_PASTIX
         ", pastix"
#endif
        )
        (
         "params,p",
         po::value<std::string>(&parameter_file),
         "parameter file in json format"
        )
        (
         "matrix,A",
         po::value<std::string>(&A_file)->default_value(A_file),
         "The system matrix in MatrixMarket format"
        )
        (
         "rhs,b",
         po::value<std::string>(&rhs_file)->default_value(rhs_file),
         "The right-hand side in MatrixMarket format"
        )
        (
         "part,s",
         po::value<std::string>(&part_file)->default_value(part_file),
         "Partitioning of the problem in MatrixMarket format"
        )
        (
         "output,o",
         po::value<std::string>(&out_file),
         "The output file (saved in MatrixMarket format)"
        )
        ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help")) {
        if (world.rank == 0)
            std::cout << desc << std::endl;
        return 0;
    }

    boost::property_tree::ptree prm;
    if (vm.count("params")) read_json(parameter_file, prm);

    prm.put("local.coarsening.type", coarsening);
    prm.put("local.relax.type",      relaxation);
    prm.put("isolver.type",          iterative_solver);
    prm.put("dsolver.type",          direct_solver);

    using amgcl::prof;

    int block_size = prm.get("precond.coarsening.aggr.block_size", 1);

    prof.tic("read problem");
    std::vector<ptrdiff_t> ptr;
    std::vector<ptrdiff_t> col;
    std::vector<double>    val;
    std::vector<double>    rhs;

    std::vector<ptrdiff_t> domain = read_problem(
            world, A_file, rhs_file, part_file, block_size, ptr, col, val, rhs
            );

    ptrdiff_t chunk = domain[world.rank + 1] - domain[world.rank];
    prof.toc("read problem");

    prof.tic("setup");
    typedef
        amgcl::mpi::subdomain_deflation<
            amgcl::amg<
                amgcl::backend::builtin<double>,
                amgcl::runtime::coarsening::wrapper,
                amgcl::runtime::relaxation::wrapper
                >,
            amgcl::runtime::mpi::solver::wrapper<amgcl::backend::builtin<double>>,
            amgcl::runtime::mpi::direct::solver<double>
        > SDD;

    std::function<double(ptrdiff_t,unsigned)> dv = amgcl::mpi::constant_deflation(block_size);
    prm.put("num_def_vec", block_size);
    prm.put("def_vec", &dv);

    SDD solve(world, std::tie(chunk, ptr, col, val), prm);
    double tm_setup = prof.toc("setup");

    std::vector<double> x(chunk, 0);

    prof.tic("solve");
    size_t iters;
    double resid;
    std::tie(iters, resid) = solve(rhs, x);
    double tm_solve = prof.toc("solve");

    if (vm.count("output")) {
        prof.tic("save");
        for(int r = 0; r < world.size; ++r) {
            if (r == world.rank) {
                std::ofstream f(out_file.c_str(), r == 0 ? std::ios::trunc : std::ios::app);

                if (r == 0) {
                    f << "%%MatrixMarket matrix array real general\n"
                      << domain.back() << " 1\n";
                }

                std::ostream_iterator<double> oi(f, "\n");
                std::copy(x.begin(), x.end(), oi);
            }
            MPI_Barrier(world);
        }
        prof.toc("save");
    }

    if (world.rank == 0) {
        std::cout
            << "Iterations: " << iters << std::endl
            << "Error:      " << resid << std::endl
            << std::endl
            << prof << std::endl;

#ifdef _OPENMP
        int nt = omp_get_max_threads();
#else
        int nt = 1;
#endif
        std::ostringstream log_name;
        log_name << "log_" << domain.back() << "_" << nt << "_" << world.size << ".txt";
        std::ofstream log(log_name.str().c_str(), std::ios::app);
        log << domain.back() << "\t" << nt << "\t" << world.size
            << "\t" << tm_setup << "\t" << tm_solve
            << "\t" << iters << "\t" << std::endl;
    }

}


/*
 *            Copyright 2009-2018 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      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 "A_ol I_ol" BA_olI_ol,
 * WITHOUT WARRANTIE_ol OR CONDITION_ol OF ANY KIND, either express or implied.
 * _olee the License for the specific language governing permissions and
 * limitations under the License.
 *
 */

#include <boost/math/constants/constants.hpp>
#include "votca/xtp/radial_euler_maclaurin_rule.h"
#include "votca/xtp/aobasis.h"
#include "votca/xtp/qmatom.h"
#include <votca/xtp/aomatrix.h>

namespace votca { namespace xtp {

    std::vector<double> EulerMaclaurinGrid::CalculatePruningIntervals(const std::string & element){
        std::vector<double> r;
        // get Bragg-Slater Radius for this element
        double BSradius = _BraggSlaterRadii.at(element);
        // row type of element
        int RowType = _pruning_set.at(element);

        if ( RowType == 1 ){
            r.push_back( 0.25 * BSradius );
            r.push_back( 0.5 * BSradius );
            r.push_back( 1.0 * BSradius );
            r.push_back( 4.5 * BSradius );
        } else if ( RowType == 2 ){
            r.push_back( 0.1667 * BSradius );
            r.push_back( 0.5 * BSradius );
            r.push_back( 0.9 * BSradius );
            r.push_back( 3.5 * BSradius );
        } else if ( RowType == 3 ) {
            r.push_back( 0.1 * BSradius );
            r.push_back( 0.4 * BSradius );
            r.push_back( 0.8 * BSradius );
            r.push_back( 2.5 * BSradius );
        } else {
          throw std::runtime_error("EulerMaclaurinGrid::CalculatePruningIntervals:Pruning unsupported for RowType");
        }
        return r;
     }

    void EulerMaclaurinGrid::FillElementRangeMap(const AOBasis& aobasis, std::vector<QMAtom*>& atoms, double eps){
      std::map<std::string, min_exp>::iterator it;
      for (QMAtom* atom:atoms) {
        std::string name = atom->getType();
        // is this element already in map?
        it = _element_ranges.find(name);
        // only proceed, if element data does not exist yet
        if (it == _element_ranges.end()) {
          min_exp this_atom;
          double range_max = std::numeric_limits<double>::min();
          double decaymin = std::numeric_limits<double>::max();
          int    lvalue = std::numeric_limits<int>::min();
          const std::vector<const AOShell*>  shells=aobasis.getShellsofAtom(atom->getAtomID());
          // and loop over all shells to figure out minimum decay constant and angular momentum of this function
          for (const AOShell* shell:shells) {
            int lmax = shell->getLmax();
            if (shell->getMinDecay() < decaymin) {
              decaymin =shell->getMinDecay();
              lvalue = lmax;
            }
            double range = DetermineCutoff(2 * decaymin, 2 * lvalue + 2, eps);
            if (range > range_max) {
              this_atom.alpha = decaymin;
              this_atom.l = lvalue;
              this_atom.range = range;
              range_max = range;
            }
          } // shells
          _element_ranges[name] = this_atom;
        } // new element
      } // atoms
    }

    void EulerMaclaurinGrid::RefineElementRangeMap(const AOBasis& aobasis, std::vector<QMAtom*>& atoms, double eps){
      AOOverlap overlap;
      overlap.Fill(aobasis);

      // get collapsed index list
      std::vector<int> idxstart;
      const std::vector<int>& idxsize = aobasis.getFuncPerAtom();
      int start = 0;
      for (int size : idxsize) {
        idxstart.push_back(start);
        start += size;
      }
      // refining by going through all atom combinations
      for (unsigned i = 0; i < atoms.size(); ++i) {
        QMAtom* atom_a = atoms[i];
        int a_start = idxstart[i];
        int a_size = idxsize[i];
        double range_max = std::numeric_limits<double>::min();
        // get preset values for this atom type
        double alpha_a = _element_ranges.at(atom_a->getType()).alpha;
        int l_a = _element_ranges.at(atom_a->getType()).l;
        const tools::vec& pos_a = atom_a->getPos();
        // Cannot iterate only over j<i because it is not symmetric due to shift_2g
        for (unsigned j = 0; j < atoms.size(); ++j) {
          if (i == j) {
            continue;
          }
          QMAtom* atom_b = atoms[j];
          int b_start = idxstart[j];
          int b_size = idxsize[j];
          const tools::vec& pos_b = atom_b->getPos();
          // find overlap block of these two atoms
          Eigen::MatrixXd overlapblock = overlap.Matrix().block(a_start, b_start, a_size, b_size);
          // determine abs max of this block
          double s_max = overlapblock.cwiseAbs().maxCoeff();

          if (s_max > 1e-5) {
            double range = DetermineCutoff(alpha_a + _element_ranges.at(atom_b->getType()).alpha, l_a + _element_ranges.at(atom_b->getType()).l + 2, eps);
            // now do some update trickery from Gaussian product formula
            double dist = tools::abs(pos_b - pos_a);
            double shift_2g = dist * alpha_a / (alpha_a + _element_ranges.at(atom_b->getType()).alpha);
            range += (shift_2g + dist);
            if (range > range_max) {
              range_max = range;
            }
          }
        }
        if (std::round(range_max) > _element_ranges.at(atom_a->getType()).range) {
          _element_ranges.at(atom_a->getType()).range = std::round(range_max);
        }
      }
    }

    void EulerMaclaurinGrid::CalculateRadialCutoffs(const AOBasis& aobasis, std::vector<QMAtom* > atoms, const std::string& gridtype) {

      double eps = Accuracy[gridtype];
      FillElementRangeMap(aobasis, atoms, eps);
      RefineElementRangeMap(aobasis, atoms, eps);
      return;
    }

    std::map<std::string, GridContainers::radial_grid> EulerMaclaurinGrid::CalculateAtomicRadialGrids(const AOBasis& aobasis , std::vector<QMAtom* > atoms,const std::string& type) {

    CalculateRadialCutoffs(aobasis,atoms,type);
    std::map<std::string,GridContainers::radial_grid>result;
    for (const auto& element:_element_ranges){
      result[element.first]=CalculateRadialGridforAtom( type,element );
    }
     return result;
    }

    GridContainers::radial_grid EulerMaclaurinGrid::CalculateRadialGridforAtom(const std::string& type, const std::pair<std::string, min_exp>& element) {
      GridContainers::radial_grid result;
      int np = getGridParameters(element.first, type);
      double cutoff = element.second.range;
      result.radius=Eigen::VectorXd::Zero(np);
      result.weight=Eigen::VectorXd::Zero(np);
      double alpha = -cutoff / (log(1.0 - std::pow((1.0 + double(np)) / (2.0 + double(np)), 3)));
      double factor = 3.0 / (1.0 + double(np));

      for (int i = 0; i < np; i++) {
        double q = double(i + 1) / (double(np) + 1.0);
        double r = -alpha * std::log(1.0 - std::pow(q, 3));
        double w = factor * alpha * r * r / (1.0 - std::pow(q, 3)) * std::pow(q, 2);
        result.radius[i]=r;
        result.weight[i]=w;
      }
      return result;
    }

    double EulerMaclaurinGrid::DetermineCutoff(double alpha, int l, double eps){
      // determine norm of function
     /* For a function f(r) = r^k*exp(-alpha*r^2) determine
        the radial distance r such that the fraction of the
        function norm that is neglected if the 3D volume
        integration is terminated at a distance r is less
        than or equal to eps. */

        double cutoff    = 1.0; // initial value
        double increment = 0.5; // increment

            while (increment > 0.01) {
                double residual = CalcResidual(alpha,l ,  cutoff);
                if (residual > eps) {
                    cutoff += increment;
                } else {
                    cutoff -= increment;
                    if (cutoff < 0.0) cutoff = 0.0;
                    increment = 0.5 * increment;
                    cutoff += increment;
                }
            }
        return cutoff;
    }

    double EulerMaclaurinGrid::CalcResidual(double alpha, int l, double cutoff){
        return RadialIntegral(alpha,l+2,cutoff) / RadialIntegral(alpha,l+2, 0.0);
    }

    double EulerMaclaurinGrid::RadialIntegral(double alpha, int l, double cutoff){
        const double pi = boost::math::constants::pi<double>();
        int ilo = l % 2;
        double value = 0.0;
        double valexp;
        if ( ilo == 0 ){
            double expo = sqrt(alpha)*cutoff;
            if ( expo > 40.0 ) {
                value = 0.0;
            } else {
                value = 0.5 * sqrt(  pi /alpha  ) * std::erfc(expo);
            }
        }
        double exponent = alpha*cutoff*cutoff;
        if ( exponent > 500.0 ) {
            valexp = 0.0;
            value = 0.0;
        } else {
            valexp = exp(-exponent);
            value = valexp/2.0/alpha;
        }
        for (int i = ilo+2; i <= l; i+=2){
            value = ((i-1)*value + std::pow(cutoff,i-1)*valexp)/2.0/alpha;
        }
        return value;
    }

    int EulerMaclaurinGrid::getGridParameters(const std::string& element, const std::string& type){
        if ( type == "medium"){
            return MediumGrid.at(element);
        }
        else if ( type == "coarse"){
            return CoarseGrid.at(element);
        }
        else if ( type == "xcoarse"){
            return XcoarseGrid.at(element);
        }
        else if ( type == "fine"){
            return FineGrid.at(element);
        }
        else if ( type == "xfine"){
            return XfineGrid.at(element);
        }
        throw std::runtime_error("Grid type "+type+" is not implemented");
        return -1;
    }

}
}