/* 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


#include <ndt_registration/ndt_matcher_p2d.h>
#include <ndt_registration/ndt_matcher_d2d_2d.h>
#include <ndt_registration/ndt_matcher_d2d.h>
#include <ndt_map/ndt_map.h>
#include <pointcloud_vrml/pointcloud_utils.h>

#include "pcl/point_cloud.h"
#include <cstdio>
#include <Eigen/Eigen>
#include <Eigen/Geometry>

#include <iostream>
#include <sstream>

using namespace std;

int
main (int argc, char** argv)
{
    if(argc!=9) {
	std::cout<<"usage "<<argv[0]<<" x y z r p t cloud1.wrl cloud2.wrl\n";
	return -1;
    }

    istringstream roll_c(argv[4]),pitch_c(argv[5]),yaw_c(argv[6]),xoffset_c(argv[1]),yoffset_c(argv[2]),zoffset_c(argv[3]);
    double roll,pitch,yaw,xoffset,yoffset,zoffset;
    roll_c >> roll;
    pitch_c >> pitch;
    yaw_c >> yaw;
    xoffset_c >> xoffset;
    yoffset_c >> yoffset;
    zoffset_c >> zoffset;

    printf("X %f Y %f Z %f Roll %f Pitch %f Yaw %f \n",xoffset,yoffset,zoffset,roll,pitch,yaw);

    char fname[50];
    FILE *fout;
    double __res[] = {0.5, 1, 2, 4};
    std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));

    struct timeval tv_start,tv_end,tv_reg_start,tv_reg_end;

    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tout;
    //Tout.setIdentity();
    Tout =  Eigen::Translation<double,3>(xoffset,yoffset,zoffset)*
	Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX()) *
	Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
	Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) ;

    gettimeofday(&tv_start,NULL);
    //we do a single scan to scan registration
    perception_oru::NDTMap<pcl::PointXYZI> ndmap(new perception_oru::LazyGrid<pcl::PointXYZI>(0.4)), local_map(new perception_oru::LazyGrid<pcl::PointXYZI>(0.4));
    ndmap.loadFromJFF(argv[7]);
    local_map.loadFromJFF(argv[8]);

    //lslgeneric::NDTMatcherD2D_2D<pcl::PointXYZI,pcl::PointXYZI> matcherD2D(false, false, resolutions);
    perception_oru::NDTMatcherD2D<pcl::PointXYZI,pcl::PointXYZI> matcherD2D(false, false, resolutions);
    bool ret = matcherD2D.match(ndmap,local_map,Tout,true);

    std::cout<<"Transform: \n"<<Tout.matrix()<<std::endl;

}


#include <boost/multiprecision/float128.hpp>


// Copyright (C) 2019 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
#include <boost/stl_interfaces/iterator_interface.hpp>

#include <string>

#include <cassert>

//[ repeated_chars_iterator
struct repeated_chars_iterator : boost::stl_interfaces::iterator_interface<
                                     repeated_chars_iterator,
                                     std::random_access_iterator_tag,
                                     char,
                                     char>
{
    constexpr repeated_chars_iterator() noexcept :
        first_(nullptr),
        size_(0),
        n_(0)
    {}
    constexpr repeated_chars_iterator(
        char const * first, difference_type size, difference_type n) noexcept :
        first_(first),
        size_(size),
        n_(n)
    {}

    constexpr char operator*() const noexcept { return first_[n_ % size_]; }
    constexpr repeated_chars_iterator & operator+=(std::ptrdiff_t i) noexcept
    {
        n_ += i;
        return *this;
    }
    constexpr auto operator-(repeated_chars_iterator other) const noexcept
    {
        return n_ - other.n_;
    }

private:
    char const * first_;
    difference_type size_;
    difference_type n_;
};
//]

int main()
{
    //[ repeated_chars_iterator_usage
    repeated_chars_iterator first("foo", 3, 0); // 3 is the length of "foo", 0 is this iterator's position.
    repeated_chars_iterator last("foo", 3, 7);  // Same as above, but now the iterator's position is 7.
    std::string result;
    std::copy(first, last, std::back_inserter(result));
    assert(result == "foofoof");
    //]
}


/*******************************************************************************
 * Copyright 2013-2014 Sebastian Niemann <niemann@sra.uni-hannover.de>.
 *
 * Licensed under the MIT License (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://opensource.org/licenses/MIT
 *
 * Developers:
 *   Sebastian Niemann - Lead developer
 *   Daniel Kiechle - Unit testing
 ******************************************************************************/
#include <Expected.hpp>
using armadilloJava::Expected;

#include <iostream>
using std::cout;
using std::endl;

#include <algorithm>
using std::min;

#include <utility>
using std::pair;

#include <armadillo>
using arma::Col;
using arma::Row;
using arma::rowvec;

#include <InputClass.hpp>
using armadilloJava::InputClass;

#include <Input.hpp>
using armadilloJava::Input;

namespace armadilloJava {
  class ExpectedInPlaceGenRowVecGenColVec : public Expected {
    public:
      ExpectedInPlaceGenRowVecGenColVec() {
        cout << "Compute ExpectedInPlaceGenRowVecGenColVec(): " << endl;

          vector<vector<pair<string, void*>>> inputs = Input::getTestParameters({
            InputClass::GenRowVec,
            InputClass::GenColVec
          });

          for (vector<pair<string, void*>> input : inputs) {
            _fileSuffix = "";

            int n = 0;
            for (pair<string, void*> value : input) {
              switch (n) {
                case 0:
                  _fileSuffix += value.first;
                  _genRowVec = *static_cast<Row<double>*>(value.second);
                  break;
                case 1:
                  _fileSuffix += "," + value.first;
                  _genColVec = *static_cast<Col<double>*>(value.second);
                  break;
              }
              ++n;
            }

            cout << "Using input: " << _fileSuffix << endl;

            _copyOfGenRowVec = _genRowVec;
            _copyOfGenColVec = _genColVec;

            expectedRowInPlaceEqual();

            _genRowVec = _copyOfGenRowVec;
            _genColVec = _copyOfGenColVec;
            expectedRowInPlacePlus();

            _genRowVec = _copyOfGenRowVec;
            _genColVec = _copyOfGenColVec;
            expectedRowInPlaceMinus();

            _genRowVec = _copyOfGenRowVec;
            _genColVec = _copyOfGenColVec;
            expectedRowInPlaceTimes();

            _genRowVec = _copyOfGenRowVec;
            _genColVec = _copyOfGenColVec;
            expectedRowInPlaceElemTimes();

            _genRowVec = _copyOfGenRowVec;
            _genColVec = _copyOfGenColVec;
            expectedRowInPlaceElemDivide();

            _genRowVec = _copyOfGenRowVec;
            _genColVec = _copyOfGenColVec;
            expectedRowSwap();

            _genRowVec = _copyOfGenRowVec;
            _genColVec = _copyOfGenColVec;
            expectedRowCopy_size();

            _genRowVec = _copyOfGenRowVec;
            _genColVec = _copyOfGenColVec;
            expectedRowRow();
          }

          cout << "done." << endl;
        }

    protected:
      Row<double> _genRowVec;
      Row<double> _copyOfGenRowVec;

      Col<double> _genColVec;
      Col<double> _copyOfGenColVec;

      void expectedRowInPlaceEqual() {
    	if(!_genColVec.is_rowvec() || _genRowVec.n_elem != _genColVec.n_elem) {
          return;
        }
        cout << "- Compute expectedRowInPlaceEqual() ... ";

        _genRowVec = _genColVec;
        save<double>("Row.inPlaceEqual", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlacePlus() {
        if(_genRowVec.n_cols != _genColVec.n_cols || _genRowVec.n_rows != _genColVec.n_rows) {
          return;
        }

        cout << "- Compute expectedRowInPlace() ... ";

        _genRowVec += _genColVec;
        save<double>("Row.inPlacePlus", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlaceMinus() {
        if(_genRowVec.n_cols != _genColVec.n_cols || _genRowVec.n_rows != _genColVec.n_rows) {
          return;
        }

        cout << "- Compute expectedRowInPlaceMinus() ... ";

        _genRowVec -= _genColVec;
        save<double>("Row.inPlaceMinus", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlaceTimes() {
        if(_genRowVec.n_cols != _genColVec.n_rows) {
          return;
        }
        if(_genRowVec.n_cols != _genColVec.n_cols) {
          return;
        }

        cout << "- Compute expectedRowInPlaceTimes() ... ";

        _genRowVec *= _genColVec;
        save<double>("Row.inPlaceTimes", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlaceElemTimes() {
        if(_genRowVec.n_cols != _genColVec.n_cols || _genRowVec.n_rows != _genColVec.n_rows) {
          return;
        }

        cout << "- Compute expectedRowInPlaceElemTimes() ... ";

        _genRowVec %= _genColVec;
        save<double>("Row.inPlaceElemTimes", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowInPlaceElemDivide() {
        if(_genRowVec.n_cols != _genColVec.n_cols || _genRowVec.n_rows != _genColVec.n_rows) {
          return;
        }

        cout << "- Compute expectedRowInPlaceElemDivide() ... ";

        _genRowVec /= _genColVec;
        save<double>("Row.inPlaceElemDivide", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowSwap() {
        if(!_genColVec.is_rowvec()) {
          return;
        }
        if(!_genRowVec.is_colvec()) {
          return;
        }

        cout << "- Compute expectedRowSwap() ... ";

        _genRowVec.swap(_genColVec);
        save<double>("Row.swap", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowCopy_size() {
        if(!_genColVec.is_rowvec()) {
          return;
        }
        cout << "- Compute expectedRowCopy_size() ... ";

        _genRowVec.copy_size(_genColVec);
        save<double>("Row.copy_size", _genRowVec);

        cout << "done." << endl;
      }

      void expectedRowRow() {
        if(!_genColVec.is_rowvec()) {
          return;
        }

        cout << "- Compute expectedRowRow() ... ";

        _genRowVec = rowvec(_genColVec);
        save<double>("Row.row", _genRowVec);

        cout << "done." << endl;
      }
  };
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana.hpp>
#include <boost/hana/ext/boost/mpl.hpp>
#include <boost/hana/ext/std.hpp>

#include <boost/mpl/lambda.hpp>
#include <boost/mpl/placeholders.hpp>
#include <boost/mpl/quote.hpp>

#include <iostream>
#include <type_traits>
namespace hana = boost::hana;
namespace mpl = boost::mpl;

namespace hpl {
//////////////////////////////////////////////////////////////////////////////
// Utilities
//////////////////////////////////////////////////////////////////////////////
namespace detail {
    template <typename Pred>
    constexpr auto mpl_predicate = hana::integral(hana::metafunction_class<
        typename mpl::lambda<Pred>::type
    >);

    template <typename F>
    constexpr auto mpl_metafunction = hana::metafunction_class<
        typename mpl::lambda<F>::type
    >;
}

//////////////////////////////////////////////////////////////////////////////
// integral_c
//////////////////////////////////////////////////////////////////////////////
template <typename T, T v>
using integral_c = std::integral_constant<T, v>;

template <int i>
using int_ = integral_c<int, i>;

template <long i>
using long_ = integral_c<long, i>;

template <bool b>
using bool_ = integral_c<bool, b>;

using true_ = bool_<true>;
using false_ = bool_<false>;

//////////////////////////////////////////////////////////////////////////////
// Sequences, compile-time integers & al
//
// Differences with the MPL:
// 1. `pair<...>::first` and `pair<...>::second` won't work;
//    use `first<pair<...>>` instead
//////////////////////////////////////////////////////////////////////////////
template <typename ...T>
using vector = hana::tuple<hana::type<T>...>;

template <typename T, T ...v>
using vector_c = hana::tuple<hana::integral_constant<T, v>...>;

template <typename T, T from, T to>
using range_c = decltype(hana::range_c<T, from, to>);

template <typename T, typename U>
using pair = hana::pair<hana::type<T>, hana::type<U>>;

template <typename P>
struct first : decltype(+hana::first(P{})) { };

template <typename P>
struct second : decltype(+hana::second(P{})) { };

//////////////////////////////////////////////////////////////////////////////
// Miscellaneous metafunctions
//////////////////////////////////////////////////////////////////////////////
template <typename C1, typename C2>
struct equal_to
    : bool_<C1::value == C2::value>
{ };

template <typename C1, typename C2>
struct less
    : bool_<(C1::value < C2::value)>
{ };

template <typename C1, typename C2>
struct greater
    : bool_<(C1::value > C2::value)>
{ };

template <typename N>
struct next
    : integral_c<typename N::value_type, N::value + 1>
{ };

//////////////////////////////////////////////////////////////////////////////
// Intrinsics
//
// Differences with the MPL:
// 1. `at` does not work for associative sequences; use `find` instead.
// 2. `begin`, `end`, `clear`, `erase`, `erase_key`, `insert`, `insert_range`,
//    `is_sequence`, `key_type`, `order`, `sequence_tag`, `value_type`: not implemented
//////////////////////////////////////////////////////////////////////////////
template <typename Sequence, typename N>
struct at
    : decltype(hana::at(Sequence{}, N{}))
{ };

template <typename Sequence, long n>
using at_c = at<Sequence, long_<n>>;

template <typename Sequence>
struct back
    : decltype(+hana::back(Sequence{}))
{ };

template <typename Sequence>
struct empty
    : decltype(hana::is_empty(Sequence{}))
{ };

template <typename Sequence>
struct front
    : decltype(+hana::front(Sequence{}))
{ };

template <typename Sequence>
struct pop_back {
    using type = decltype(hana::drop_back(
        hana::to_tuple(Sequence{}), hana::size_c<1>
    ));
};

template <typename Sequence>
struct pop_front {
    using type = decltype(hana::drop_front(Sequence{}));
};

template <typename Sequence, typename T>
struct push_back {
    using type = decltype(hana::append(Sequence{}, hana::type_c<T>));
};

template <typename Sequence, typename T>
struct push_front {
    using type = decltype(hana::prepend(Sequence{}, hana::type_c<T>));
};

template <typename Sequence>
struct size
    : decltype(hana::length(Sequence{}))
{ };

//////////////////////////////////////////////////////////////////////////////
// Iteration algorithms
//
// Differences with the MPL:
// 1. reverse_fold:
//    Does not take an optional additional ForwardOp argument.
//
// 2. iter_fold, reverse_iter_fold:
//    Not implemented because we don't use iterators
//////////////////////////////////////////////////////////////////////////////
template <typename Sequence, typename State, typename F>
struct fold
    : decltype(hana::fold(
        Sequence{}, hana::type_c<State>, detail::mpl_metafunction<F>
    ))
{ };

template <typename Sequence, typename State, typename F>
struct reverse_fold
    : decltype(hana::reverse_fold(
        Sequence{}, hana::type_c<State>, detail::mpl_metafunction<F>
    ))
{ };

template <typename Sequence, typename State, typename F>
using accumulate = fold<Sequence, State, F>;

//////////////////////////////////////////////////////////////////////////////
// Query algorithms
//
// Differences with the MPL:
// 1. find_if and find:
//    Instead of returning an iterator, they either have a nested `::type`
//    alias to the answer, or they have no nested `::type` at all, which
//    makes them SFINAE-friendly.
//
// 2. lower_bound, upper_bound:
//    Not implemented.
//
// 3. {min,max}_element:
//    Not returning an iterator, and also won't work on empty sequences.
//////////////////////////////////////////////////////////////////////////////
template <typename Sequence, typename Pred>
struct find_if
    : decltype(hana::find_if(Sequence{}, detail::mpl_predicate<Pred>))
{ };

template <typename Sequence, typename T>
struct find
    : decltype(hana::find(Sequence{}, hana::type_c<T>))
{ };

template <typename Sequence, typename T>
struct contains
    : decltype(hana::contains(Sequence{}, hana::type_c<T>))
{ };

template <typename Sequence, typename T>
struct count
    : decltype(hana::count(Sequence{}, hana::type_c<T>))
{ };

template <typename Sequence, typename Pred>
struct count_if
    : decltype(hana::count_if(Sequence{}, detail::mpl_predicate<Pred>))
{ };

template <typename Sequence, typename Pred = mpl::quote2<less>>
struct min_element
    : decltype(hana::minimum(Sequence{}, detail::mpl_predicate<Pred>))
{ };

template <typename Sequence, typename Pred = mpl::quote2<less>>
struct max_element
    : decltype(hana::maximum(Sequence{}, detail::mpl_predicate<Pred>))
{ };

template <typename S1, typename S2, typename Pred = mpl::quote2<std::is_same>>
struct equal
    : decltype( // inefficient but whatever
        hana::length(S1{}) == hana::length(S2{}) &&
        hana::all(hana::zip_shortest_with(detail::mpl_predicate<Pred>,
                hana::to_tuple(S1{}),
                hana::to_tuple(S2{})))
    )
{ };

//////////////////////////////////////////////////////////////////////////////
// Transformation algorithms
//
// Differences from the MPL:
// 1. The algorithms do not accept an optional inserter, and they always
//    return a `vector`.
// 2. stable_partition: not implemented
// 3. All the reverse_* algorithms are not implemented.
//////////////////////////////////////////////////////////////////////////////
template <typename Sequence>
struct copy {
    using type = decltype(hana::to_tuple(Sequence{}));
};

template <typename Sequence, typename Pred>
struct copy_if {
    using type = decltype(hana::filter(
        hana::to_tuple(Sequence{}),
        detail::mpl_predicate<Pred>
    ));
};

template <typename Sequence, typename Sequence_or_Op, typename = void>
struct transform;

template <typename Sequence, typename Op>
struct transform<Sequence, Op> {
    using type = decltype(hana::transform(
        hana::to_tuple(Sequence{}), detail::mpl_metafunction<Op>
    ));
};

template <typename S1, typename S2, typename Op>
struct transform {
    using type = decltype(hana::zip_with(
        detail::mpl_metafunction<Op>,
        hana::to_tuple(S1{}),
        hana::to_tuple(S2{})
    ));
};

template <typename Sequence, typename OldType, typename NewType>
struct replace {
    using type = decltype(hana::replace(
        hana::to_tuple(Sequence{}),
        hana::type_c<OldType>,
        hana::type_c<NewType>
    ));
};

template <typename Sequence, typename Pred, typename NewType>
struct replace_if {
    using type = decltype(hana::replace_if(
        hana::to_tuple(Sequence{}),
        detail::mpl_predicate<Pred>,
        hana::type_c<NewType>
    ));
};

template <typename Sequence, typename T>
struct remove {
    using type = decltype(hana::filter(
        hana::to_tuple(Sequence{}),
        hana::not_equal.to(hana::type_c<T>)
    ));
};

template <typename Sequence, typename Pred>
struct remove_if {
    using type = decltype(hana::filter(
        hana::to_tuple(Sequence{}),
        hana::compose(hana::not_, detail::mpl_predicate<Pred>)
    ));
};

template <typename Sequence, typename Pred>
struct unique {
    using type = decltype(hana::unique(
        hana::to_tuple(Sequence{}),
        detail::mpl_predicate<Pred>
    ));
};

template <typename Sequence, typename Pred>
struct partition {
    using hana_pair = decltype(hana::partition(
        hana::to_tuple(Sequence{}),
        detail::mpl_predicate<Pred>
    ));
    using type = pair<
        decltype(hana::first(hana_pair{})),
        decltype(hana::second(hana_pair{}))
    >;
};

template <typename Sequence, typename Pred = mpl::quote2<less>>
struct sort {
    using type = decltype(hana::sort(
        hana::to_tuple(Sequence{}), detail::mpl_predicate<Pred>
    ));
};

template <typename Sequence>
struct reverse {
    using type = decltype(hana::reverse(hana::to_tuple(Sequence{})));
};

//////////////////////////////////////////////////////////////////////////////
// Runtime algorithms
//////////////////////////////////////////////////////////////////////////////
template <typename Sequence, typename F>
void for_each(F f) {
    hana::for_each(Sequence{}, [&f](auto t) {
        f(typename decltype(t)::type{});
    });
}

template <typename Sequence, typename TransformOp, typename F>
void for_each(F f) {
    for_each<typename transform<Sequence, TransformOp>::type>(f);
}

} // end namespace hpl

template <typename N>
struct is_odd
    : hpl::bool_<(N::value % 2)>
{ };

int main() {
using namespace hpl;

//////////////////////////////////////////////////////////////////////////////
// Misc
//////////////////////////////////////////////////////////////////////////////

// pair
{
    static_assert(std::is_same<first<pair<int, float>>::type, int>{}, "");
    static_assert(std::is_same<second<pair<int, float>>::type, float>{}, "");
}

//////////////////////////////////////////////////////////////////////////////
// Intrinsics
//////////////////////////////////////////////////////////////////////////////

// at
{
    using range = range_c<long,10,50>;
    static_assert(at<range, int_<0>>::value == 10, "");
    static_assert(at<range, int_<10>>::value == 20, "");
    static_assert(at<range, int_<40>>::value == 50, "");
}

// at_c
{
    using range = range_c<long, 10, 50>;
    static_assert(at_c<range, 0>::value == 10, "");
    static_assert(at_c<range, 10>::value == 20, "");
    static_assert(at_c<range, 40>::value == 50, "");
}

// back
{
    using range1 = range_c<int,0,1>;
    using range2 = range_c<int,0,10>;
    using range3 = range_c<int,-10,0>;
    using types = vector<int, char, float>;
    static_assert(back<range1>::value == 0, "");
    static_assert(back<range2>::value == 9, "");
    static_assert(back<range3>::value == -1, "");
    static_assert(std::is_same<back<types>::type, float>{}, "");
}

// empty
{
    using empty_range = range_c<int,0,0>;
    using types = vector<long,float,double>;
    static_assert(empty<empty_range>{}, "");
    static_assert(!empty<types>{}, "");
}

// front
{
    using types1 = vector<long>;
    using types2 = vector<int,long>;
    using types3 = vector<char,int,long>;
    static_assert(std::is_same<front<types1>::type, long>{}, "");
    static_assert(std::is_same<front<types2>::type, int>{}, "");
    static_assert(std::is_same<front<types3>::type, char>{}, "");
}

// pop_back
{
    using types1 = vector<long>;
    using types2 = vector<long,int>;
    using types3 = vector<long,int,char>;

    using result1 = pop_back<types1>::type;
    using result2 = pop_back<types2>::type;
    using result3 = pop_back<types3>::type;

    static_assert(size<result1>::value == 0, "");
    static_assert(size<result2>::value == 1, "");
    static_assert(size<result3>::value == 2, "");

    static_assert(std::is_same< back<result2>::type, long>{}, "");
    static_assert(std::is_same< back<result3>::type, int>{}, "");
}

// pop_front
{
    using types1 = vector<long>;
    using types2 = vector<int,long>;
    using types3 = vector<char,int,long>;

    using result1 = pop_front<types1>::type;
    using result2 = pop_front<types2>::type;
    using result3 = pop_front<types3>::type;

    static_assert(size<result1>::value == 0, "");
    static_assert(size<result2>::value == 1, "");
    static_assert(size<result3>::value == 2, "");

    static_assert(std::is_same<front<result2>::type, long>{}, "");
    static_assert(std::is_same<front<result3>::type, int>{}, "");
}

// push_back
{
    using bools = vector_c<bool,false,false,false,true,true,true,false,false>;
    using message = push_back<bools, false_>::type;
    static_assert(back<message>::type::value == false, "");
    static_assert(count_if<message, equal_to<mpl::_1, false_>>{} == 6u, "");
}

// push_front
{
    using v = vector_c<int,1,2,3,5,8,13,21>;
    static_assert(size<v>{} == 7u, "");

    using fibonacci = push_front<v, int_<1>>::type;
    static_assert(size<fibonacci>{} == 8u, "");

    static_assert(equal<
        fibonacci,
        vector_c<int,1,1,2,3,5,8,13,21>,
        equal_to<mpl::_, mpl::_>
    >{}, "");
}

// size
{
    using empty_list = vector<>;
    using numbers = vector_c<int,0,1,2,3,4,5>;
    using more_numbers = range_c<int,0,100>;

    static_assert(size<empty_list>{} == 0u, "");
    static_assert(size<numbers>{} == 6u, "");
    static_assert(size<more_numbers>{} == 100u, "");
}

//////////////////////////////////////////////////////////////////////////////
// Iteration algorithms
//////////////////////////////////////////////////////////////////////////////

// fold
{
    using types = vector<long,float,short,double,float,long,long double>;
    using number_of_floats = fold<types, int_<0>,
        mpl::if_<std::is_floating_point<mpl::_2>,
            next<mpl::_1>,
            mpl::_1
        >
    >::type;
    static_assert(number_of_floats{} == 4, "");
}

// reverse_fold
{
    using numbers = vector_c<int,5,-1,0,-7,-2,0,-5,4>;
    using negatives = vector_c<int,-1,-7,-2,-5>;
    using result = reverse_fold<numbers, vector_c<int>,
        mpl::if_<less<mpl::_2, int_<0>>,
            push_front<mpl::_1, mpl::_2>,
            mpl::_1
        >
    >::type;
    static_assert(equal<negatives, result>{}, "");
}

//////////////////////////////////////////////////////////////////////////////
// Query algorithms
//////////////////////////////////////////////////////////////////////////////

// find_if
{
    using types = vector<char,int,unsigned,long,unsigned long>;
    using found = find_if<types, std::is_same<mpl::_1, unsigned>>::type;
    static_assert(std::is_same<found, unsigned>{}, "");
}

// find
{
    using types = vector<char,int,unsigned,long,unsigned long>;
    static_assert(std::is_same<find<types, unsigned>::type, unsigned>{}, "");
}

// contains
{
    using types = vector<char,int,unsigned,long,unsigned long>;
    static_assert(!contains<types, bool>{}, "");
}

// count
{
    using types = vector<int,char,long,short,char,short,double,long>;
    static_assert(count<types, short>{} == 2u, "");
}

// count_if
{
    using types = vector<int,char,long,short,char,long,double,long>;
    static_assert(count_if<types, std::is_floating_point<mpl::_>>{} == 1u, "");
    static_assert(count_if<types, std::is_same<mpl::_, char>>{} == 2u, "");
    static_assert(count_if<types, std::is_same<mpl::_, void>>{} == 0u, "");
}

// min_element (MPL's example is completely broken)
{
}

// max_element (MPL's example is completely broken)
{
}

// equal
{
    using s1 = vector<char,int,unsigned,long,unsigned long>;
    using s2 = vector<char,int,unsigned,long>;
    static_assert(!equal<s1,s2>{}, "");
}

//////////////////////////////////////////////////////////////////////////////
// Transformaton algorithms
//////////////////////////////////////////////////////////////////////////////
// copy
{
    using numbers = vector_c<int,10, 11, 12, 13, 14, 15, 16, 17, 18, 19>;
    using result = copy<range_c<int, 10, 20>>::type;
    static_assert(size<result>{} == 10u, "");
    static_assert(equal<result, numbers, mpl::quote2<equal_to>>{}, "");
}

// copy_if
{
    using result = copy_if<range_c<int, 0, 10>, less<mpl::_1, int_<5>>>::type;
    static_assert(size<result>{} == 5u, "");
    static_assert(equal<result, range_c<int, 0, 5>>{}, "");
}

// transform
{
    using types = vector<char,short,int,long,float,double>;
    using pointers = vector<char*,short*,int*,long*,float*,double*>;
    using result = transform<types,std::add_pointer<mpl::_1>>::type;
    static_assert(equal<result, pointers>{}, "");
}

// replace
{
    using types = vector<int,float,char,float,float,double>;
    using expected = vector<int,double,char,double,double,double>;
    using result = replace< types,float,double >::type;
    static_assert(equal<result, expected>{}, "");
}

// replace_if
{
    using numbers = vector_c<int,1,4,5,2,7,5,3,5>;
    using expected = vector_c<int,1,4,0,2,0,0,3,0>;
    using result = replace_if<numbers, greater<mpl::_, int_<4>>, int_<0>>::type;
    static_assert(equal<result, expected, mpl::quote2<equal_to>>{}, "");
}

// remove
{
    using types = vector<int,float,char,float,float,double>;
    using result = hpl::remove<types, float>::type;
    static_assert(equal<result, vector<int, char, double>>{}, "");
}

// remove_if
{
    using numbers = vector_c<int,1,4,5,2,7,5,3,5>;
    using result = remove_if<numbers, greater<mpl::_, int_<4> > >::type;
    static_assert(equal<result, vector_c<int,1,4,2,3>, mpl::quote2<equal_to>>{}, "");
}

// unique
{
    using types = vector<int,float,float,char,int,int,int,double>;
    using expected = vector<int,float,char,int,double>;
    using result = unique<types, std::is_same<mpl::_1, mpl::_2>>::type;
    static_assert(equal<result, expected>{}, "");
}

// partition
{
    using r = partition<range_c<int,0,10>, is_odd<mpl::_1>>::type;
    static_assert(equal<first<r>::type, vector_c<int,1,3,5,7,9>>{}, "");
    static_assert(equal<second<r>::type, vector_c<int,0,2,4,6,8>>{}, "");
}

// sort
{
    using numbers = vector_c<int,3,4,0,-5,8,-1,7>;
    using expected = vector_c<int,-5,-1,0,3,4,7,8>;
    using result = sort<numbers>::type;
    static_assert(equal<result, expected, equal_to<mpl::_, mpl::_>>{}, "");
}

// reverse
{
    using numbers = vector_c<int,9,8,7,6,5,4,3,2,1,0>;
    using result = reverse<numbers>::type;
    static_assert(equal<result, range_c<int,0,10>>{}, "");
}

//////////////////////////////////////////////////////////////////////////////
// Runtime algorithms
//////////////////////////////////////////////////////////////////////////////

// for_each
{
    auto value_printer = [](auto x) {
        std::cout << x << '\n';
    };

    for_each<range_c<int, 0, 10> >(value_printer);
}

}


#include <pangolin/pangolin.h>
#include <Eigen/Core>
#include <unistd.h>
#include <fstream>

// 本例演示了如何画出一个预先存储的轨迹

using namespace std;
using namespace Eigen;

struct State{
  Eigen::Matrix3d Rwr;
  Eigen::Vector3d twr;
  Eigen::Vector3d vel;

  State(Eigen::Matrix3d& Rwr, Eigen::Vector3d& twr, Eigen::Vector3d& vel)
    : Rwr(Rwr), twr(twr), vel(vel) {}
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

std::vector<std::vector<double>> readSensorData(std::string);
std::vector<State, Eigen::aligned_allocator<State>> readStates(std::string);
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);
void DrawTrajectoryComparison(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>,
                              vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>>);

int main(int argc, char **argv) {
  if(argc < 3) {
    std::cout << "missing arg for the file" << std::endl;
  }

  // std::string path = argv[1];
  // std::vector<std::vector<double>> data = readSensorData(path);

  // path to trajectory file
  // string est_traj_file = "./results/est_states.txt";
  // string gt_traj_file = "./results/gt_states.txt";
  string est_traj_file = argv[1];
  string gt_traj_file = argv[2];

  std::vector<State, Eigen::aligned_allocator<State>> states = readStates(est_traj_file);
  std::vector<State, Eigen::aligned_allocator<State>> gt_states = readStates(gt_traj_file);

  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses;
  vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> gt_poses;

  // for (std::vector<double> &state : data) {
  //   cout << "state size: " << state.size() << endl;
  //   for (int i = 0; i < 16; i++) {
  //     cout << state[i] << " ";
  //   }
  //   cout << endl;
  //   Matrix3d Rwr;
  //   Vector3d twr;
  //   Rwr << state[0], state[1], state[2],
  //          state[4], state[5], state[6],
  //          state[8], state[9], state[10];
  //   twr << state[3], state[7], state[11];
  //   cout << "Rwr: " << Rwr << endl;
  //   cout << "twr: " << twr << endl;
  //   Isometry3d Twr(Rwr);
  //   Twr.pretranslate(twr);
  //   poses.push_back(Twr);
  // }

  // Isometry3d Twr(Eigen::Matrix3d::Identity());
  // Twr.pretranslate(Eigen::Vector3d::Zero());
  // poses.push_back(Twr);
  // gt_poses.push_back(Twr);

  int cnt = states.size();

  for (int i = 0; i < cnt; i++) {
    Isometry3d Twr(states[i].Rwr);
    Twr.pretranslate(states[i].twr);
    poses.push_back(Twr);
  }
  for (int i = 0; i < cnt; i++) {
    Isometry3d Twr(gt_states[i].Rwr);
    Twr.pretranslate(gt_states[i].twr);
    gt_poses.push_back(Twr);
  }

  /*
  ifstream fin(trajectory_file);
  if (!fin) {
    cout << "cannot find trajectory file at " << trajectory_file << endl;
    return 1;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Isometry3d Twr(Quaterniond(qw, qx, qy, qz));
    Twr.pretranslate(Vector3d(tx, ty, tz));
    poses.push_back(Twr);
  }

  */
  cout << "read total " << poses.size() << " pose entries" << endl;

  // draw trajectory in pangolin
  DrawTrajectoryComparison(poses, gt_poses);
  return 0;
}

std::vector<std::vector<double>> readSensorData(std::string path) {
  std::vector<std::vector<double>> ret;

  std::ifstream csvFile;
  csvFile.open(path);

  std::string line;
  while(std::getline(csvFile, line)) {
    std::vector<double> row;
    std::cout << "line:" << line << std::endl;
    std::istringstream s(line);
    std::string field;
    while (std::getline(s, field,',')) {
      std::cout << "field: " << field << std::endl;
      row.push_back(std::stod(field));
    }
    ret.push_back(row);
  }
  return ret;
}

std::vector<State, Eigen::aligned_allocator<State>> readStates(std::string filename) {
  std::vector<State, Eigen::aligned_allocator<State>> states;
  std::ifstream file(filename);
  if (!file) {
    cout << "Error opening the file: " << filename << endl;
    return states;
  }
  cout << "Start reading: " << filename << endl;
  if (file.is_open()) {
    for (int i = 0; i < 36; i++) {
      Eigen::Matrix3d Rwr;
      Eigen::Vector3d twr;
      Eigen::Vector3d vel;
      file >> Rwr(0, 0) >> Rwr(0, 1) >> Rwr(0, 2)
          >> Rwr(1, 0) >> Rwr(1, 1) >> Rwr(1, 2)
          >> Rwr(2, 0) >> Rwr(2, 1) >> Rwr(2, 2)
          >> twr(0) >> twr(1) >> twr(2)
          >> vel(0) >> vel(1) >> vel(2);
      states.push_back(State(Rwr, twr, vel));
    }
  }
  return states;
}

void DrawTrajectoryComparison(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses,
                              vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> gt_poses) {
    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(1.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < gt_poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = gt_poses[i].translation();
      Vector3d Xw = gt_poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = gt_poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = gt_poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < gt_poses.size(); i++) {
      glColor3f(0.0, 1.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = gt_poses[i], p2 = gt_poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

/*******************************************************************************************/
void DrawTrajectory(vector<Isometry3d, Eigen::aligned_allocator<Isometry3d>> poses) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
    pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

  pangolin::View &d_cam = pangolin::CreateDisplay()
    .SetBounds(0.0, 1.0, 0.0, 1.0, -1024.0f / 768.0f)
    .SetHandler(new pangolin::Handler3D(s_cam));

  while (pangolin::ShouldQuit() == false) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
    glLineWidth(2);
    for (size_t i = 0; i < poses.size(); i++) {
      // 画每个位姿的三个坐标轴
      Vector3d Ow = poses[i].translation();
      Vector3d Xw = poses[i] * (0.1 * Vector3d(1, 0, 0));
      Vector3d Yw = poses[i] * (0.1 * Vector3d(0, 1, 0));
      Vector3d Zw = poses[i] * (0.1 * Vector3d(0, 0, 1));
      glBegin(GL_LINES);
      glColor3f(1.0, 0.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Xw[0], Xw[1], Xw[2]);
      glColor3f(0.0, 1.0, 0.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Yw[0], Yw[1], Yw[2]);
      glColor3f(0.0, 0.0, 1.0);
      glVertex3d(Ow[0], Ow[1], Ow[2]);
      glVertex3d(Zw[0], Zw[1], Zw[2]);
      glEnd();
    }
    // 画出连线
    for (size_t i = 0; i < poses.size(); i++) {
      glColor3f(0.0, 0.0, 0.0);
      glBegin(GL_LINES);
      auto p1 = poses[i], p2 = poses[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}


#pragma once
#include <iostream>
#include <fstream>
#include <vector>
#include <tuple>
#include "Vector3D.hpp"
#include "Struct.hpp"
#include "FileReader.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>

void OutYUBA(const int& step, const std::vector<Vector3D>& elements, const std::string& file_name, const std::string& tag, bool trunc_f=true);
void OutYUBA(const int& step, const std::vector<Triangle>& elements, const std::string& file_name, const std::string& tag, bool trunc_f=true);

std::vector<Vector3D> get_next_Beads(      Getline& gl, const std::tuple<int,int>& tr);
std::vector<Vector3D> get_next_Coordinate( Getline& gl, const std::tuple<int,int>& tr);
std::vector<Triangle> get_next_Triangle(   Getline& gl, const std::tuple<int,int>& tr);


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MAIN all_of any_of none_of tests

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <vector>

#include <operators.h>
#include <rectangle.h>

using rectangle = rectangle_t<int>;

BOOST_AUTO_TEST_SUITE( all_any_none_of_test_suite )

BOOST_AUTO_TEST_CASE( empty_container_all_of )
{
    std::vector<rectangle> rects;

    /*! True regardless of the predicate, as all elements in the container
     * satisfy the predicate trivially. */
    BOOST_CHECK(std::all_of(std::begin(rects), std::end(rects), true_no_op()));
    BOOST_CHECK(std::all_of(std::begin(rects), std::end(rects), false_no_op()));
}

BOOST_AUTO_TEST_CASE( empty_container_any_of )
{
    std::vector<rectangle> rects;

    /*! True regardless of the predicate, as none of the elements in the
     * container make the predicate return false. */
    BOOST_CHECK(std::none_of(std::begin(rects), std::end(rects), true_no_op()));
    BOOST_CHECK(std::none_of(std::begin(rects), std::end(rects), false_no_op()));
}

BOOST_AUTO_TEST_CASE( empty_container_none_of )
{
    std::vector<rectangle> rects;

    /*! Note the definition for any_of: "returns true if the predicate returns
     * true for *at least one* element in the range. Since the range has no
     * elements, if must return false for any predicate, as there is no
     * 'witness'. */
    BOOST_CHECK(not std::any_of(std::begin(rects), std::end(rects), true_no_op()));
    BOOST_CHECK(not std::any_of(std::begin(rects), std::end(rects), false_no_op()));
}

BOOST_AUTO_TEST_CASE( no_op_all_of )
{
    const rects_variety_fixture f;

    BOOST_CHECK(std::all_of(std::begin(f.rects), std::end(f.rects), true_no_op()));
    BOOST_CHECK(not std::all_of(std::begin(f.rects), std::end(f.rects), false_no_op()));
    BOOST_CHECK(not std::all_of(std::begin(f.rects), std::end(f.rects), alternating_no_op()));
}

BOOST_AUTO_TEST_CASE( no_op_any_of )
{
    const rects_variety_fixture f;

    BOOST_CHECK(std::any_of(std::begin(f.rects), std::end(f.rects), true_no_op()));
    BOOST_CHECK(not std::any_of(std::begin(f.rects), std::end(f.rects), false_no_op()));

    /*! True, as the container has at least 2 elements. */
    BOOST_CHECK(std::any_of(std::begin(f.rects), std::end(f.rects), alternating_no_op()));
}

BOOST_AUTO_TEST_CASE( no_op_none_of )
{
    const rects_variety_fixture f;

    BOOST_CHECK(not std::none_of(std::begin(f.rects), std::end(f.rects), true_no_op()));
    BOOST_CHECK(std::none_of(std::begin(f.rects), std::end(f.rects), false_no_op()));
    BOOST_CHECK(not std::none_of(std::begin(f.rects), std::end(f.rects), alternating_no_op()));
}

BOOST_AUTO_TEST_CASE( is_valid_checks )
{
    const rects_variety_fixture f;

    BOOST_CHECK(not std::all_of(std::begin(f.rects), std::end(f.rects), is_valid<rects_variety_fixture::coord>));
    BOOST_CHECK(std::any_of(std::begin(f.rects), std::end(f.rects), is_valid<rects_variety_fixture::coord>));
    BOOST_CHECK(not std::none_of(std::begin(f.rects), std::end(f.rects), is_valid<rects_variety_fixture::coord>));
}

BOOST_AUTO_TEST_CASE( has_area_valid_rects_fixture )
{
    const valid_rects_fixture f;

    BOOST_REQUIRE(std::all_of(std::begin(f.rects), std::end(f.rects), is_valid<rects_variety_fixture::coord>));

    BOOST_CHECK(not std::all_of(std::begin(f.rects), std::end(f.rects), has_area<rects_variety_fixture::coord>));
    BOOST_CHECK(std::any_of(std::begin(f.rects), std::end(f.rects), has_area<rects_variety_fixture::coord>));
    BOOST_CHECK(not std::none_of(std::begin(f.rects), std::end(f.rects), has_area<rects_variety_fixture::coord>));
}

BOOST_AUTO_TEST_CASE( has_area_positive_area_rects_fixture )
{
    const positive_area_rects_fixture f;

    BOOST_REQUIRE(std::all_of(std::begin(f.rects), std::end(f.rects), is_valid<rects_variety_fixture::coord>));

    BOOST_CHECK(std::all_of(std::begin(f.rects), std::end(f.rects), has_area<rects_variety_fixture::coord>));
    BOOST_CHECK(std::any_of(std::begin(f.rects), std::end(f.rects), has_area<rects_variety_fixture::coord>));
    BOOST_CHECK(not std::none_of(std::begin(f.rects), std::end(f.rects), has_area<rects_variety_fixture::coord>));
}

BOOST_AUTO_TEST_SUITE_END( /* all_of_test_suite */ )


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

#include "unsupervised_data_reader.h"
#include "neural_network_exception.h"

#include <boost/format.hpp>

#include <vector>
#include <cmath>

namespace nnforge
{
	unsupervised_data_reader::unsupervised_data_reader()
	{
	}

	unsupervised_data_reader::~unsupervised_data_reader()
	{
	}

	size_t unsupervised_data_reader::get_input_neuron_elem_size() const
	{
		return neuron_data_type::get_input_size(get_input_type());
	}

	std::vector<feature_map_data_stat> unsupervised_data_reader::get_feature_map_input_data_stat_list()
	{
		std::vector<feature_map_data_stat> res;

		neuron_data_type::input_type type_code = get_input_type();

		if (type_code != neuron_data_type::type_float)
			throw neural_network_exception(((boost::format("Unable to stat data reader with input data type %1%") % type_code).str()));

		reset();

		unsigned int entry_count = get_entry_count();
		if (entry_count == 0)
			throw neural_network_exception("Unable to stat data reader with no entries");

		layer_configuration_specific input_configuration = get_input_configuration();
		res.resize(input_configuration.feature_map_count);

		for(std::vector<feature_map_data_stat>::iterator it = res.begin(); it != res.end(); ++it)
		{
			it->min = std::numeric_limits<float>::max();
			it->max = -std::numeric_limits<float>::max();
			it->average = 0.0F;
			it->std_dev = 0.0F;
		}

		std::vector<float> input_data(input_configuration.get_neuron_count());
		unsigned int neuron_count_per_feature_map = input_configuration.get_neuron_count_per_feature_map();

		double mult = 1.0 / ((double)entry_count * (double)neuron_count_per_feature_map);

		std::vector<double> sum(input_configuration.feature_map_count, 0.0);
		std::vector<double> sum_squared(input_configuration.feature_map_count, 0.0);
		while(read(&(*input_data.begin())))
		{
			std::vector<float>::const_iterator data_it = input_data.begin();
			std::vector<double>::iterator sum_it = sum.begin();
			std::vector<double>::iterator sum_squared_it = sum_squared.begin();
			for(std::vector<feature_map_data_stat>::iterator fm_it = res.begin(); fm_it != res.end(); ++fm_it, ++sum_it, ++sum_squared_it)
			{
				double current_sum = 0.0;
				double current_sum_squared = 0.0;
				for(unsigned int i = 0; i < neuron_count_per_feature_map; ++i)
				{
					float val = *data_it;
					fm_it->min = std::min(fm_it->min, val);
					fm_it->max = std::max(fm_it->max, val);
					current_sum += static_cast<double>(val);
					current_sum_squared += static_cast<double>(val * val);
					++data_it;
				}
				*sum_it += current_sum;
				*sum_squared_it += current_sum_squared;
			}
		}

		std::vector<double>::const_iterator sum_it = sum.begin();
		std::vector<double>::const_iterator sum_squared_it = sum_squared.begin();
		for(std::vector<feature_map_data_stat>::iterator it = res.begin(); it != res.end(); ++it, ++sum_it, ++sum_squared_it)
		{
			double avg = *sum_it * mult;
			it->average = static_cast<float>(avg);
			double avg_sq = *sum_squared_it * mult;
			it->std_dev = sqrtf(static_cast<float>(avg_sq - avg * avg));
		}

		return res;
	}

	void unsupervised_data_reader::next_epoch()
	{
		reset();
	}

	unsigned int unsupervised_data_reader::get_sample_count() const
	{
		return 1;
	}
}


#include <gtest/gtest.h>
#include <Eigen/Geometry>
#include <plucker/plane.h>
#include "gtest_helper.h"

namespace
{

template<typename T>
class PlaneTest
    : public ::testing::Test
{
protected:
    template<typename U = T>
    static constexpr typename std::enable_if<std::is_same<U, float>::value, U>::type
    absolute_tolerance(){ return 1e-4f; }

    template<typename U = T>
    static constexpr typename std::enable_if<std::is_same<U, double>::value, U>::type
    absolute_tolerance(){ return 1e-8; }

    template<typename U = T>
    static constexpr typename std::enable_if<std::is_same<U, float>::value, U>::type
    relative_tolerance(){ return 1e-5f; }

    template<typename U = T>
    static constexpr typename std::enable_if<std::is_same<U, double>::value, U>::type
    relative_tolerance(){ return 1e-5; }
};

using MyTypes = ::testing::Types<float, double>;
TYPED_TEST_SUITE(PlaneTest, MyTypes);

TYPED_TEST(PlaneTest, Constructor)
{
    using Plane = plucker::Plane<TypeParam>;
    using Vector4 = typename Plane::Vector4;

    constexpr auto atol = PlaneTest<TypeParam>::absolute_tolerance();

    const auto coord = Vector4(TypeParam(1), TypeParam(2), TypeParam(3), TypeParam(4));

    Plane res1(coord);
    Plane res2(coord.x(), coord.y(), coord.z(), coord.w());
    Plane res3(coord.head(3), coord.w());
    EXPECT_MAT_ALMOST_EQUAL(coord, res1.coord(), atol);
    EXPECT_MAT_ALMOST_EQUAL(coord, res2.coord(), atol);
    EXPECT_MAT_ALMOST_EQUAL(coord, res3.coord(), atol);
}

TYPED_TEST(PlaneTest, Accessor)
{
    using Plane = plucker::Plane<TypeParam>;
    using Vector3 = typename Plane::Vector3;
    using Vector4 = typename Plane::Vector4;

    constexpr auto atol = PlaneTest<TypeParam>::absolute_tolerance();

    const auto coord = Vector4(TypeParam(1), TypeParam(2), TypeParam(3), TypeParam(4));

    {
        const Plane res1(coord);
        EXPECT_MAT_ALMOST_EQUAL(coord, res1.coord(), atol);

        Plane res2;
        res2.coord() = coord;
        EXPECT_MAT_ALMOST_EQUAL(coord, res2.coord(), atol);
    }
    {
        const Plane res1(coord);
        EXPECT_ALMOST_EQUAL(coord.x(), res1.a(), atol);
        EXPECT_ALMOST_EQUAL(coord.y(), res1.b(), atol);
        EXPECT_ALMOST_EQUAL(coord.z(), res1.c(), atol);
        EXPECT_ALMOST_EQUAL(coord.w(), res1.d(), atol);

        Plane res2;
        res2.a() = coord.x();
        res2.b() = coord.y();
        res2.c() = coord.z();
        res2.d() = coord.w();
        EXPECT_ALMOST_EQUAL(coord.x(), res2.a(), atol);
        EXPECT_ALMOST_EQUAL(coord.y(), res2.b(), atol);
        EXPECT_ALMOST_EQUAL(coord.z(), res2.c(), atol);
        EXPECT_ALMOST_EQUAL(coord.w(), res2.d(), atol);
    }
    {
        const Vector3 n = coord.head(3);

        const Plane res1(coord);
        EXPECT_MAT_ALMOST_EQUAL(n, res1.normal(), atol);
        EXPECT_ALMOST_EQUAL(coord.w(), res1.d(), atol);

        Plane res2;
        res2.normal() = n;
        res2.d() = coord.w();
        EXPECT_MAT_ALMOST_EQUAL(n, res2.normal(), atol);
        EXPECT_ALMOST_EQUAL(coord.w(), res2.d(), atol);
    }
}

TYPED_TEST(PlaneTest, Alignment)
{
    using Plane = plucker::Plane<TypeParam>;
    using Vector4 = typename Plane::Vector4;

    if((sizeof(Vector4) % 16) != 0)
        GTEST_SKIP_("It does not need to be aligned.");

    {
        Plane* p = new Plane;
        const auto addr = reinterpret_cast<std::uintptr_t>(&(p->coord()));
        EXPECT_TRUE((addr % 16) == 0);
        delete p;
    }
    {
        Plane* p = new Plane[2];
        const auto addr0 = reinterpret_cast<std::uintptr_t>(&(p[0].coord()));
        const auto addr1 = reinterpret_cast<std::uintptr_t>(&(p[1].coord()));
        EXPECT_TRUE((addr0 % 16) == 0);
        EXPECT_TRUE((addr1 % 16) == 0);
        delete[] p;
    }
}

}   // namespace


// -------------------------------------------
//  @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;
}