#include <cstring>
#include <limits>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include <Eigen/Geometry>
#include <common_robotics_utilities/math.hpp>
#include <gtest/gtest.h>
#include <voxelized_geometry_tools/collision_map.hpp>
#include <voxelized_geometry_tools/pointcloud_voxelization.hpp>

namespace voxelized_geometry_tools
{
namespace
{
using pointcloud_voxelization::PointCloudVoxelizationFilterOptions;
using pointcloud_voxelization::PointCloudVoxelizationInterface;
using pointcloud_voxelization::PointCloudWrapper;
using pointcloud_voxelization::PointCloudWrapperPtr;

class VectorVector3dPointCloudWrapper : public PointCloudWrapper
{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  void PushBack(const Eigen::Vector3d& point)
  {
    points_.push_back(point);
  }

  double MaxRange() const override
  {
    return std::numeric_limits<double>::infinity();
  }

  int64_t Size() const override { return static_cast<int64_t>(points_.size()); }

  const Eigen::Isometry3d& GetPointCloudOriginTransform() const override
  {
    return origin_transform_;
  }

  void SetPointCloudOriginTransform(
      const Eigen::Isometry3d& origin_transform) override
  {
    origin_transform_ = origin_transform;
  }

private:
  void CopyPointLocationIntoDoublePtrImpl(
      const int64_t point_index, double* destination) const override
  {
    const Eigen::Vector3d& point = points_.at(static_cast<size_t>(point_index));
    std::memcpy(destination, point.data(), sizeof(double) * 3);
  }

  void CopyPointLocationIntoFloatPtrImpl(
      const int64_t point_index, float* destination) const override
  {
    const Eigen::Vector3f point =
        points_.at(static_cast<size_t>(point_index)).cast<float>();
    std::memcpy(destination, point.data(), sizeof(float) * 3);
  }

  common_robotics_utilities::math::VectorVector3d points_;
  Eigen::Isometry3d origin_transform_ = Eigen::Isometry3d::Identity();
};

void check_empty_voxelization(const CollisionMap& occupancy)
{
  // Make sure the grid is properly filled
  for (int64_t xidx = 0; xidx < occupancy.GetNumXCells(); xidx++)
  {
    for (int64_t yidx = 0; yidx < occupancy.GetNumYCells(); yidx++)
    {
      for (int64_t zidx = 0; zidx < occupancy.GetNumZCells(); zidx++)
      {
        // Check grid querying
        const auto occupancy_query = occupancy.GetImmutable(xidx, yidx, zidx);
        // Check grid values
        const float cmap_occupancy = occupancy_query.Value().Occupancy();
        // Check the bottom cells
        if (zidx == 0)
        {
          ASSERT_EQ(cmap_occupancy, 1.0f);
        }
        // All other cells should be marked "unknown"
        else
        {
          ASSERT_EQ(cmap_occupancy, 0.5f);
        }
      }
    }
  }
}

void check_voxelization(const CollisionMap& occupancy)
{
  // Make sure the grid is properly filled
  for (int64_t xidx = 0; xidx < occupancy.GetNumXCells(); xidx++)
  {
    for (int64_t yidx = 0; yidx < occupancy.GetNumYCells(); yidx++)
    {
      for (int64_t zidx = 0; zidx < occupancy.GetNumZCells(); zidx++)
      {
        // Check grid querying
        const auto occupancy_query = occupancy.GetImmutable(xidx, yidx, zidx);
        // Check grid values
        const float cmap_occupancy = occupancy_query.Value().Occupancy();
        // Check the bottom cells
        if (zidx == 0)
        {
          ASSERT_EQ(cmap_occupancy, 1.0f);
        }
        // Check a few "seen empty" cells
        if ((xidx == 3) && (yidx >= 3) && (zidx >= 1))
        {
          ASSERT_EQ(cmap_occupancy, 0.0f);
        }
        if ((xidx >= 3) && (yidx == 3) && (zidx >= 1))
        {
          ASSERT_EQ(cmap_occupancy, 0.0f);
        }
        // Check a few "seen filled" cells
        if ((xidx == 4) && (yidx >= 4) && (zidx >= 1))
        {
          ASSERT_EQ(cmap_occupancy, 1.0f);
        }
        if ((xidx >= 4) && (yidx == 4) && (zidx >= 1))
        {
          ASSERT_EQ(cmap_occupancy, 1.0f);
        }
        // Check shadowed cells
        if ((xidx > 4) && (yidx > 4) && (zidx >= 1))
        {
          ASSERT_EQ(cmap_occupancy, 0.5f);
        }
      }
    }
  }
}

GTEST_TEST(PointCloudVoxelizationTest, Test)
{
  // Make the static environment
  const Eigen::Isometry3d X_WG(Eigen::Translation3d(-1.0, -1.0, -1.0));
  // Grid 2m in each axis
  const double x_size = 2.0;
  const double y_size = 2.0;
  const double z_size = 2.0;
  // 1/4 meter resolution, so 8 cells/axis
  const double grid_resolution = 0.25;
  const double step_size_multiplier = 0.5;
  const CollisionCell empty_cell(0.0f);
  const CollisionCell filled_cell(1.0f);

  const common_robotics_utilities::voxel_grid::GridSizes grid_size(
      grid_resolution, x_size, y_size, z_size);
  CollisionMap static_environment(X_WG, "world", grid_size, empty_cell);

  // Set the bottom cells filled
  for (int64_t xidx = 0; xidx < static_environment.GetNumXCells(); xidx++)
  {
    for (int64_t yidx = 0; yidx < static_environment.GetNumYCells(); yidx++)
    {
      static_environment.SetValue(xidx, yidx, 0, filled_cell);
    }
  }

  // Make some test pointclouds
  // Make the physical->optical frame transform
  const Eigen::Isometry3d X_CO = Eigen::Translation3d(0.0, 0.0, 0.0) *
      Eigen::Quaterniond(Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitZ()) *
                         Eigen::AngleAxisd(-M_PI_2, Eigen::Vector3d::UnitX()));

  // Camera 1 pose
  const Eigen::Isometry3d X_WC1(Eigen::Translation3d(-2.0, 0.0, 0.0));
  const Eigen::Isometry3d X_WC1O = X_WC1 * X_CO;
  PointCloudWrapperPtr cam1_cloud(new VectorVector3dPointCloudWrapper());
  static_cast<VectorVector3dPointCloudWrapper*>(cam1_cloud.get())
      ->SetPointCloudOriginTransform(X_WC1O);
  for (double x = -2.0; x <= 2.0; x += 0.03125)
  {
    for (double y = -2.0; y <= 2.0; y += 0.03125)
    {
      const double z = (x <= 0.0) ? 2.125 : 4.0;
      static_cast<VectorVector3dPointCloudWrapper*>(cam1_cloud.get())->PushBack(
          Eigen::Vector3d(x, y, z));
    }
  }

  // Camera 2 pose
  const Eigen::Isometry3d X_WC2 = Eigen::Translation3d(0.0, -2.0, 0.0) *
      Eigen::Quaterniond(Eigen::AngleAxisd(M_PI_2, Eigen::Vector3d::UnitZ()));
  const Eigen::Isometry3d X_WC2O = X_WC2 * X_CO;
  PointCloudWrapperPtr cam2_cloud(new VectorVector3dPointCloudWrapper());
  static_cast<VectorVector3dPointCloudWrapper*>(cam2_cloud.get())
      ->SetPointCloudOriginTransform(X_WC2O);
  for (double x = -2.0; x <= 2.0; x += 0.03125)
  {
    for (double y = -2.0; y <= 2.0; y += 0.03125)
    {
      const double z = (x >= 0.0) ? 2.125 : 4.0;
      static_cast<VectorVector3dPointCloudWrapper*>(cam2_cloud.get())->PushBack(
          Eigen::Vector3d(x, y, z));
    }
  }

  // Camera 3 pose
  const Eigen::Isometry3d X_WC3 = Eigen::Isometry3d::Identity();
  const Eigen::Isometry3d X_WC3O = X_WC3 * X_CO;
  // Cloud 3 is empty
  PointCloudWrapperPtr cam3_cloud(new VectorVector3dPointCloudWrapper());
  static_cast<VectorVector3dPointCloudWrapper*>(cam3_cloud.get())
      ->SetPointCloudOriginTransform(X_WC3O);

  // Make control parameters
  // We require that 100% of points from the camera see through to see a voxel
  // as free.
  const double percent_seen_free = 1.0;
  // We don't worry about outliers.
  const int32_t outlier_points_threshold = 1;
  // We only need one camera to see a voxel as free.
  const int32_t num_cameras_seen_free = 1;
  const PointCloudVoxelizationFilterOptions filter_options(
      percent_seen_free, outlier_points_threshold, num_cameras_seen_free);

  // Run all available voxelizers
  const auto available_backends =
      pointcloud_voxelization::GetAvailableBackends();

  for (size_t idx = 0; idx < available_backends.size(); idx++)
  {
    const auto& available_backend = available_backends.at(idx);
    std::cout << "Trying voxelizer [" << idx << "] "
              << available_backend.DeviceName() << std::endl;

    auto voxelizer =
        pointcloud_voxelization::MakePointCloudVoxelizer(available_backend);

    const auto empty_voxelized = voxelizer->VoxelizePointClouds(
        static_environment, step_size_multiplier, filter_options, {});
    check_empty_voxelization(empty_voxelized);

    const auto voxelized = voxelizer->VoxelizePointClouds(
        static_environment, step_size_multiplier, filter_options,
        {cam1_cloud, cam2_cloud, cam3_cloud});
    check_voxelization(voxelized);
  }
}
}  // namespace
}  // namespace voxelized_geometry_tools

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}


/****************************************************************************
 *  hipipe library
 *  Copyright (c) 2017, Cognexa Solutions s.r.o.
 *  Copyright (c) 2018, Iterait a.s.
 *  Author(s) Filip Matzner
 *
 *  This file is distributed under the MIT License.
 *  See the accompanying file LICENSE.txt for the complete license agreement.
 ****************************************************************************/

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE groups_test

#include "common.hpp"

#include <hipipe/core/groups.hpp>

#include <boost/test/unit_test.hpp>
#include <range/v3/action/sort.hpp>
#include <range/v3/view/filter.hpp>

#include <random>
#include <vector>

namespace hp = hipipe;

// test with a seeded random generator
std::mt19937 prng{1000003};

std::size_t n_groups(const std::vector<std::size_t>& groups, std::size_t group)
{
    return
      (groups
         | rgv::filter([group](std::size_t l) { return l == group; })
         | rg::to_vector
      ).size();
}

BOOST_AUTO_TEST_CASE(test_generate_groups)
{
    std::vector<std::size_t> groups = hp::generate_groups(10, {1.5, 1.5}, prng);
    BOOST_TEST(groups.size() == 10UL);
    BOOST_TEST(n_groups(groups, 0) == 5UL);
    BOOST_TEST(n_groups(groups, 1) == 5UL);

    auto sorted_groups = groups;
    sorted_groups |= rga::sort;
    BOOST_CHECK(sorted_groups != groups);
}

BOOST_AUTO_TEST_CASE(test_generate_groups_zero_ratio)
{
    std::vector<std::size_t> groups = hp::generate_groups(10, {1.5, 0, 1.5}, prng);
    BOOST_TEST(groups.size() == 10UL);
    BOOST_TEST(n_groups(groups, 0) == 5UL);
    BOOST_TEST(n_groups(groups, 1) == 0UL);
    BOOST_TEST(n_groups(groups, 2) == 5UL);

    auto sorted_groups = groups;
    sorted_groups |= rga::sort;
    BOOST_CHECK(sorted_groups != groups);
}

BOOST_AUTO_TEST_CASE(test_generate_groups_not_divisible)
{
    std::vector<std::size_t> groups = hp::generate_groups(11, {0, 8, 3, 3, 0}, prng);
    BOOST_TEST(groups.size() == 11UL);
    BOOST_TEST(n_groups(groups, 0) == 0UL);

    // the first non-zero one gets round(11 / 14 * 8) = 6
    BOOST_TEST(n_groups(groups, 1) == 6UL);
    // the second non-zero one gets round(11 / 14 * 3) = 2
    BOOST_TEST(n_groups(groups, 2) == 2UL);
    // the third non-zero one gets the rest = 3
    BOOST_TEST(n_groups(groups, 3) == 3UL);

    BOOST_TEST(n_groups(groups, 4) == 0UL);
}

BOOST_AUTO_TEST_CASE(test_generate_many_groups)
{
    std::vector<std::vector<std::size_t>> groups =
      hp::generate_groups(2, 20, {0.3, 0.3}, {0.2, 0.2}, prng);

    BOOST_TEST(groups.size() == 2UL);
    BOOST_TEST(groups[0].size() == 20UL);
    BOOST_TEST(groups[1].size() == 20UL);

    // check that the groups have correct ratio
    for (std::size_t j = 0; j < 2; ++j) {
        BOOST_TEST(n_groups(groups[j], 0) == 6UL);
        BOOST_TEST(n_groups(groups[j], 1) == 6UL);
        BOOST_TEST(n_groups(groups[j], 2) == 4UL);
        BOOST_TEST(n_groups(groups[j], 3) == 4UL);
    }

    // check that all the fixed groups (i.e., 2+) are the same
    for (std::size_t i = 0; i < 10; ++i) {
        if (groups[0][i] >= 2UL) BOOST_TEST(groups[0][i] == groups[0][i]);
    }

    // check that the groups differ
    BOOST_CHECK(groups[0] != groups[1]);
}

BOOST_AUTO_TEST_CASE(test_generate_many_groups_zero_ratio)
{
    std::vector<std::vector<std::size_t>> groups =
      hp::generate_groups(2, 20, {0.3, 0, 0, 0.3}, {0.2, 0, 0, 0.2}, prng);

    BOOST_TEST(groups.size() == 2UL);
    BOOST_TEST(groups[0].size() == 20UL);
    BOOST_TEST(groups[1].size() == 20UL);

    // check that the groups have correct ratio
    for (std::size_t j = 0; j < 2; ++j) {
        BOOST_TEST(n_groups(groups[j], 0) == 6UL);
        BOOST_TEST(n_groups(groups[j], 3) == 6UL);
        BOOST_TEST(n_groups(groups[j], 4) == 4UL);
        BOOST_TEST(n_groups(groups[j], 7) == 4UL);
    }

    // check that all the fixed groups (i.e., 4+) are the same
    for (std::size_t i = 0; i < 10; ++i) {
        if (groups[0][i] >= 4UL) BOOST_TEST(groups[0][i] == groups[0][i]);
    }

    // check that the groups differ
    BOOST_CHECK(groups[0] != groups[1]);
}


//
// Copyright 2016 Pixar
//
// Licensed under the Apache License, Version 2.0 (the "Apache License")
// with the following modification; you may not use this file except in
// compliance with the Apache License and the following modification to it:
// Section 6. Trademarks. is deleted and replaced with:
//
// 6. Trademarks. This License does not grant permission to use the trade
//    names, trademarks, service marks, or product names of the Licensor
//    and its affiliates, except as required to comply with Section 4(c) of
//    the License and to reproduce the content of the NOTICE file.
//
// You may obtain a copy of the Apache License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the Apache License with the above modification is
// distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
// KIND, either express or implied. See the Apache License for the specific
// language governing permissions and limitations under the Apache License.
//
///
/// LayerOffset.cpp

#include "pxr/pxr.h"
#include "pxr/usd/sdf/layerOffset.h"
#include "pxr/usd/sdf/timeCode.h"
#include "pxr/base/gf/math.h"

#include "pxr/base/tf/type.h"

#include <boost/functional/hash/hash.hpp>

#include <climits>
#include <limits>
#include <ostream>
#include <vector>

#define EPSILON (1e-6)

PXR_NAMESPACE_OPEN_SCOPE

TF_REGISTRY_FUNCTION(TfType) {
    TfType::Define<SdfLayerOffset>();
    TfType::Define<std::vector<SdfLayerOffset>>();
}

bool
SdfLayerOffset::IsIdentity() const
{
    // Construct a static instance to avoid a default construction on every call
    // to SdfLayerOffset::IsIdentity().
    static SdfLayerOffset identityOffset;

    // Use operator==() for fuzzy compare (i.e. GfIsClose).
    return *this == identityOffset;
}

SdfLayerOffset::SdfLayerOffset(double offset, double scale) :
    _offset(offset),
    _scale(scale)
{
}

bool
SdfLayerOffset::IsValid() const
{
    return std::isfinite(_offset) && std::isfinite(_scale);
}

SdfLayerOffset SdfLayerOffset::GetInverse() const
{
    if (IsIdentity()) {
        return *this;
    }

    double newScale;
    if (_scale != 0.0) {
        newScale = 1.0 / _scale;
    } else {
        newScale = std::numeric_limits<double>::infinity();
    }
    return SdfLayerOffset( -_offset * newScale, newScale );
}

SdfLayerOffset
SdfLayerOffset::operator*(const SdfLayerOffset &rhs) const
{
    return SdfLayerOffset( _scale * rhs._offset + _offset,
                           _scale * rhs._scale );
}

double
SdfLayerOffset::operator*(double rhs) const
{
    return ( rhs * _scale + _offset );
}

SdfTimeCode
SdfLayerOffset::operator*(const SdfTimeCode &rhs) const
{
    return SdfTimeCode( (*this) * double(rhs) );
}

bool
SdfLayerOffset::operator==(const SdfLayerOffset &rhs) const
{
    // Use EPSILON so that 0 == -0, for example.
    return (!IsValid() && !rhs.IsValid()) ||
           (GfIsClose(_offset, rhs._offset, EPSILON) &&
            GfIsClose(_scale, rhs._scale, EPSILON));
}

bool
SdfLayerOffset::operator<(const SdfLayerOffset &rhs) const
{
    if (!IsValid()) {
        return false;
    }
    if (!rhs.IsValid()) {
        return true;
    }
    if (GfIsClose(_scale, rhs._scale, EPSILON)) {
        if (GfIsClose(_offset, rhs._offset, EPSILON)) {
            return false;
        }
        else {
            return _offset < rhs._offset;
        }
    }
    else {
        return _scale < rhs._scale;
    }
}

size_t
SdfLayerOffset::GetHash() const
{
    size_t hash = 0;
    boost::hash_combine(hash, _offset);
    boost::hash_combine(hash, _scale);
    return hash;
}

std::ostream & operator<<( std::ostream &out,
                           const SdfLayerOffset &layerOffset )
{
    return out << "SdfLayerOffset("
        << layerOffset.GetOffset() << ", "
        << layerOffset.GetScale() << ")";
}

PXR_NAMESPACE_CLOSE_SCOPE


// Copyright 2018 Hans Dembinski
//
// 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 <array>
#include <boost/core/lightweight_test.hpp>
#include <boost/histogram.hpp>
#include <boost/histogram/accumulators/weighted_sum.hpp>
#include <boost/histogram/algorithm/sum.hpp>
#include <boost/histogram/axis/integer.hpp>
#include <unordered_map>
#include <vector>
#include "utility_histogram.hpp"

using namespace boost::histogram;
using namespace boost::histogram::literals; // to get _c suffix
using boost::histogram::algorithm::sum;

template <typename Tag>
void run_tests() {
  auto ax = axis::integer<>(0, 100);

  auto h1 = make(Tag(), ax);
  for (unsigned i = 0; i < 100; ++i) h1(i);
  BOOST_TEST_EQ(sum(h1), 100);

  auto h2 = make_s(Tag(), std::vector<double>(), ax, ax);
  for (unsigned i = 0; i < 100; ++i)
    for (unsigned j = 0; j < 100; ++j) h2(i, j);
  BOOST_TEST_EQ(sum(h2), 10000);

  auto h3 = make_s(Tag(), std::array<int, 102>(), ax);
  for (unsigned i = 0; i < 100; ++i) h3(i);
  BOOST_TEST_EQ(sum(h3), 100);

  auto h4 = make_s(Tag(), std::unordered_map<std::size_t, int>(), ax);
  for (unsigned i = 0; i < 100; ++i) h4(i);
  BOOST_TEST_EQ(sum(h4), 100);

  // auto h5 =
  //     make_s(Tag(), std::vector<accumulators::weighted_sum<>>(), axis::integer<>(0, 1),
  //            axis::integer<int, axis::null_type, axis::option::none_t>(2, 4));
  // h5(weight(2), 0, 2);
  // h5(-1, 2);
  // h5(1, 3);

  // const auto v = algorithm::sum(h5);
  // BOOST_TEST_EQ(v.value(), 4);
  // BOOST_TEST_EQ(v.variance(), 6);
}

int main() {
  run_tests<static_tag>();
  run_tests<dynamic_tag>();

  return boost::report_errors();
}


#include <boost/test/unit_test.hpp>
#include <templ/solvers/temporal/TemporalAssertion.hpp>
#include <templ/solvers/temporal/Event.hpp>
#include <templ/solvers/temporal/PersistenceCondition.hpp>
#include <templ/solvers/temporal/point_algebra/QualitativeTimePoint.hpp>
#include <templ/solvers/temporal/QualitativeTemporalConstraintNetwork.hpp>
#include <templ/symbols/ObjectVariable.hpp>
#include <templ/symbols/StateVariable.hpp>
#include <templ/symbols/Value.hpp>
#include <templ/symbols/values/Int.hpp>

using namespace templ;
using namespace templ::symbols;
using namespace templ::solvers;
using namespace templ::solvers::temporal;

BOOST_AUTO_TEST_SUITE(temporal_assertion)

BOOST_AUTO_TEST_CASE(disjoint)
{
    StateVariable stateVariable("robot_location", "robot_0");
    Value::Ptr fromValue(new values::Int(0));
    Value::Ptr toValue(new values::Int(10));
    point_algebra::TimePoint::Ptr t0(new point_algebra::QualitativeTimePoint("t0"));
    point_algebra::TimePoint::Ptr t0_alias(new point_algebra::QualitativeTimePoint("t0"));
    point_algebra::TimePoint::Ptr t1(new point_algebra::QualitativeTimePoint("t1"));

    TemporalAssertion::Ptr event0a(new Event(stateVariable, fromValue, toValue, t0));
    TemporalAssertion::Ptr event0b(new Event(stateVariable, fromValue, toValue, t0));
    TemporalAssertion::Ptr event0a_alias(new Event(stateVariable, fromValue, toValue, t0_alias));
    TemporalAssertion::Ptr event1(new Event(stateVariable, fromValue, toValue, t1));

    QualitativeTemporalConstraintNetwork::Ptr qtcn(new QualitativeTemporalConstraintNetwork());
    point_algebra::TimePointComparator comparator(qtcn);
    BOOST_REQUIRE_MESSAGE(!event0a->isDisjointFrom(event0b, comparator), "Identical events are not disjoint" );
    BOOST_REQUIRE_MESSAGE(!event0a->isDisjointFrom(event0a_alias, comparator), "Identical events are not disjoint" );
    BOOST_REQUIRE_MESSAGE(event0a->isDisjointFrom(event1, comparator), "Disjoint events are disjoint" );
}

BOOST_AUTO_TEST_CASE(same_value)
{
    QualitativeTemporalConstraintNetwork::Ptr qtcn(new QualitativeTemporalConstraintNetwork());
    point_algebra::TimePointComparator comparator(qtcn);

    StateVariable stateVariable("robot_location", "robot_0");
    Value::Ptr fromValue0(new values::Int(0));
    Value::Ptr toValue0(new values::Int(10));
    Value::Ptr fromValue1(new values::Int(0));
    Value::Ptr toValue1(new values::Int(10));

    point_algebra::TimePoint::Ptr t0(new point_algebra::QualitativeTimePoint("t0"));
    point_algebra::TimePoint::Ptr t1(new point_algebra::QualitativeTimePoint("t1"));

    TemporalAssertion::Ptr event0(new Event(stateVariable, fromValue0, toValue0, t0));
    TemporalAssertion::Ptr event1(new Event(stateVariable, fromValue1, toValue1, t1));

    BOOST_REQUIRE_MESSAGE(event0->isReferringToSameValue(event1, comparator), "Two events refer to the same value");

    Value::Ptr fromValue2(new values::Int(0));
    Value::Ptr toValue2(new values::Int(2));
    TemporalAssertion::Ptr event2(new Event(stateVariable, fromValue2, toValue2, t0));
    BOOST_REQUIRE_MESSAGE(!event0->isReferringToSameValue(event2, comparator), "Two events refer to the same value");
    BOOST_REQUIRE_MESSAGE(!event1->isReferringToSameValue(event2, comparator), "Two events refer to the same value");

    {
        Value::Ptr valueFrom(new values::Int(10));
        Value::Ptr valueTo(new values::Int(30));

        point_algebra::TimePoint::Ptr timepoint0 = point_algebra::TimePoint::create("Initial timepoint");

        StateVariable stateVariable("location", "sherpa");

        point_algebra::TimePointComparator comparator;

        Event::Ptr event0(new Event(stateVariable, valueFrom, valueTo, timepoint0));
        Event::Ptr event1(new Event(stateVariable, valueFrom, valueTo, timepoint0));

        BOOST_REQUIRE_MESSAGE(event0->isReferringToSameValue(event1, comparator), "Same event refers to same value");

        Value::Ptr valueFrom2(new values::Int(20));
        Value::Ptr valueTo2(new values::Int(40));

        Event::Ptr event2(new Event(stateVariable, valueFrom2, valueTo2, timepoint0));
        BOOST_REQUIRE_MESSAGE(!event0->isReferringToSameValue(event2, comparator), "Different events refer not to the same value");
    }
}

BOOST_AUTO_TEST_CASE(persistence_condition)
{
    StateVariable stateVariable("robot_location","robot0");
    {
        // rloc(robot0)@[t2,t3) : l1
        ObjectVariable::Ptr objectVariable0(new ObjectVariable("l1"));
        point_algebra::TimePoint::Ptr fromT(new point_algebra::QualitativeTimePoint("t2"));
        point_algebra::TimePoint::Ptr toT(new point_algebra::QualitativeTimePoint("t3"));

        PersistenceCondition::Ptr assertion0(new PersistenceCondition(stateVariable, objectVariable0, fromT, toT));

        // rloc(robot0)@[t2,t3) : l3
        ObjectVariable::Ptr objectVariable1(new ObjectVariable("l3"));
        PersistenceCondition::Ptr assertion1(new PersistenceCondition(stateVariable, objectVariable1, fromT, toT));

        QualitativeTemporalConstraintNetwork::Ptr qtcn(new QualitativeTemporalConstraintNetwork());
        qtcn->addQualitativeConstraint(fromT, toT, point_algebra::QualitativeTimePointConstraint::LessOrEqual);

        qtcn->addTimePoint(fromT);
        qtcn->addTimePoint(toT);

        point_algebra::TimePointComparator comparator(qtcn);
        BOOST_REQUIRE_MESSAGE(!assertion0->isDisjointFrom(assertion1, comparator), "Overlapping persistence conditions are not disjoint");

        // Check if there is no connecting constraint between two intervals
        point_algebra::TimePoint::Ptr fromT2(new point_algebra::QualitativeTimePoint("t4"));
        point_algebra::TimePoint::Ptr toT2(new point_algebra::QualitativeTimePoint("t5"));
        qtcn->addQualitativeConstraint(fromT2, toT2, point_algebra::QualitativeTimePointConstraint::LessOrEqual);

        PersistenceCondition::Ptr assertion2(new PersistenceCondition(stateVariable, objectVariable1, fromT2, toT2));
        BOOST_REQUIRE_MESSAGE(assertion0->isDisjointFrom(assertion2, comparator), "Non overlapping persistence conditions are disjoint");

        qtcn->addQualitativeConstraint(fromT2, fromT, point_algebra::QualitativeTimePointConstraint::Equal);
        BOOST_REQUIRE_MESSAGE(!assertion0->isDisjointFrom(assertion2, comparator), "Overlapping persistence conditions are not disjoint");
    }

    {
        ObjectVariable::Ptr objectVariable = make_shared<ObjectVariable>("l0");
        point_algebra::TimePoint::Ptr t0 = point_algebra::TimePoint::create("t0");
        point_algebra::TimePoint::Ptr t1 = point_algebra::TimePoint::create("t1");
        point_algebra::TimePoint::Ptr t2 = point_algebra::TimePoint::create("t2");

        PersistenceCondition pc0(stateVariable, objectVariable, t0, t1);
        PersistenceCondition pc1(stateVariable, objectVariable, t1, t2);
        PersistenceCondition pc2(stateVariable, objectVariable, t1, t2);

        BOOST_REQUIRE_MESSAGE(pc0 != pc1, "PersistenceConditions are not equal");
        BOOST_REQUIRE_MESSAGE(pc1 == pc2, "PersistenceConditions are equal");
    }
}

BOOST_AUTO_TEST_SUITE_END()


#include "multi_body_icp.h"

#include "vox_grid.h"
#include "frames.h"
#include "model.h"
#include "focal_grid.h"

#include <boost/thread/thread.hpp>
#include <boost/make_shared.hpp>
#include <string>
#include <stdint.h>
#include <iostream>
#include <iomanip>
#include <math.h>
#include <chrono>
#include <vector>
#include <thread>
#include <algorithm>
#include <dirent.h>
#include <armadillo>
#include <nlopt.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/registration/icp.h>
#include <Eigen/Core>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/common/time.h>
#include <pcl/console/print.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/features/fpfh_omp.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/sample_consensus_prerejective.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>

using namespace std;

// Types
typedef pcl::PointXYZ PointT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointCloud<PointNT> PointCloudT;
typedef pcl::FPFHSignature33 FeatureT;
typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
typedef pcl::PointCloud<FeatureT> FeatureCloudT;

// Define a new point representation for < x, y, z, curvature >
class MyPointRepresentation : public pcl::PointRepresentation <PointNT>
{
  using pcl::PointRepresentation<PointNT>::nr_dimensions_;
public:
  MyPointRepresentation ()
  {
    // Define the number of dimensions
    nr_dimensions_ = 4;
  }

  // Override the copyToFloatArray method to define our feature vector
  virtual void copyToFloatArray (const PointNT &p, float * out) const
  {
    // < x, y, z, curvature >
    out[0] = p.x;
    out[1] = p.y;
    out[2] = p.z;
    out[3] = p.curvature;
  }
};

MultiBodyICP::MultiBodyICP() {
  // empty
}

/*
vector<arma::Mat<double>> MultiBodyICP::SingleFrameICP(frames &frame_in, model &mod) {

  vector<arma::Mat<double>> M_align_vec;

  arma::Mat<double> body_pcl = frame_in.pcl_init_single_frame[0];
  arma::Mat<double> wing_L_pcl = frame_in.pcl_init_single_frame[1];
  arma::Mat<double> wing_R_pcl = frame_in.pcl_init_single_frame[2];

  arma::Mat<double> M_body_init = frame_in.M_init_single_frame[0];
  arma::Mat<double> M_wing_L_init = frame_in.M_init_single_frame[1];
  arma::Mat<double> M_wing_R_init = frame_in.M_init_single_frame[2];

  // Calculate transformation matrices for all initial orientations:

  // body:

  arma::Mat<double> M_thorax_init = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  pcl::PointCloud<pcl::PointXYZ> thorax_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_thorax_init, 0);
  PointCloudT thorax_src = MultiBodyICP::TransferPointXYZ2PointNT(thorax_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> head_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_head_init, 1);
  PointCloudT head_src = MultiBodyICP::TransferPointXYZ2PointNT(head_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> abdomen_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_abdomen_init, 2);
  PointCloudT abdomen_src = MultiBodyICP::TransferPointXYZ2PointNT(abdomen_src_pcl);

  pcl::PointCloud<pcl::PointXYZ> body_dest_pcl = MultiBodyICP::Convert_Mat_2_PCL_XYZ(body_pcl);
  PointCloudT body_dest = MultiBodyICP::TransferPointXYZ2PointNT(body_dest_pcl);

  // Perform ICP on thorax
  //tuple<arma::Mat<double>,double> icp_result_thorax = MultiBodyICP::ICP_on_single_segment(body_dest_pcl, thorax_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_head = MultiBodyICP::ICP_on_single_segment(body_dest_pcl, head_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_abdomen = MultiBodyICP::ICP_on_single_segment(body_dest_pcl, abdomen_src_pcl);
  tuple<arma::Mat<double>,double> icp_result_thorax = MultiBodyICP::RobustPoseEstimation(body_dest, thorax_src);
  tuple<arma::Mat<double>,double> icp_result_head = MultiBodyICP::RobustPoseEstimation(body_dest, head_src);
  tuple<arma::Mat<double>,double> icp_result_abdomen = MultiBodyICP::RobustPoseEstimation(body_dest, abdomen_src);

  M_align_vec.push_back(get<0>(icp_result_thorax));
  M_align_vec.push_back(get<0>(icp_result_head));
  M_align_vec.push_back(get<0>(icp_result_abdomen));

  // left wing:

  arma::Mat<double> M_wing_L1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,0,3,3), 3);
  arma::Mat<double> M_wing_L2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,4,3,7), 3);
  arma::Mat<double> M_wing_L3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,8,3,11), 3);
  arma::Mat<double> M_wing_L4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,12,3,15), 3);

  pcl::PointCloud<pcl::PointXYZ> wing_L1_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_L1_init, 3);
  PointCloudT wing_L1_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_L1_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_L2_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_L2_init, 3);
  PointCloudT wing_L2_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_L2_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_L3_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_L3_init, 3);
  PointCloudT wing_L3_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_L3_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_L4_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_L4_init, 3);
  PointCloudT wing_L4_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_L4_src_pcl);

  pcl::PointCloud<pcl::PointXYZ> wing_L_dest_pcl = MultiBodyICP::Convert_Mat_2_PCL_XYZ(wing_L_pcl);
  PointCloudT wing_L_dest = MultiBodyICP::TransferPointXYZ2PointNT(wing_L_dest_pcl);

  // Perform ICP on the left wing:

  //tuple<arma::Mat<double>,double> icp_result_wing_L1 = MultiBodyICP::ICP_on_single_segment(wing_L_dest_pcl, wing_L1_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_L2 = MultiBodyICP::ICP_on_single_segment(wing_L_dest_pcl, wing_L2_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_L3 = MultiBodyICP::ICP_on_single_segment(wing_L_dest_pcl, wing_L3_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_L4 = MultiBodyICP::ICP_on_single_segment(wing_L_dest_pcl, wing_L4_src_pcl);
  tuple<arma::Mat<double>,double> icp_result_wing_L1 = MultiBodyICP::RobustPoseEstimation(wing_L_dest, wing_L1_src);
  tuple<arma::Mat<double>,double> icp_result_wing_L2 = MultiBodyICP::RobustPoseEstimation(wing_L_dest, wing_L2_src);
  tuple<arma::Mat<double>,double> icp_result_wing_L3 = MultiBodyICP::RobustPoseEstimation(wing_L_dest, wing_L3_src);
  tuple<arma::Mat<double>,double> icp_result_wing_L4 = MultiBodyICP::RobustPoseEstimation(wing_L_dest, wing_L4_src);

  arma::Col<double> score_vec_L = {get<1>(icp_result_wing_L1),get<1>(icp_result_wing_L2),get<1>(icp_result_wing_L3),get<1>(icp_result_wing_L4)};

  if (score_vec_L.index_max()==0) {
    M_align_vec.push_back(get<0>(icp_result_wing_L1));
  }
  else if (score_vec_L.index_max()==1) {
    M_align_vec.push_back(get<0>(icp_result_wing_L2));
  }
  else if (score_vec_L.index_max()==2) {
    M_align_vec.push_back(get<0>(icp_result_wing_L3));
  }
  else if (score_vec_L.index_max()==3) {
    M_align_vec.push_back(get<0>(icp_result_wing_L4));
  }
  else {
    arma::Mat<double> zero_mat(4,4);
    zero_mat.zeros();
    M_align_vec.push_back(zero_mat);
  }

  // right wing:

  arma::Mat<double> M_wing_R1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,0,3,3), 4);
  arma::Mat<double> M_wing_R2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,4,3,7), 4);
  arma::Mat<double> M_wing_R3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,8,3,11), 4);
  arma::Mat<double> M_wing_R4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,12,3,15), 4);

  pcl::PointCloud<pcl::PointXYZ> wing_R1_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_R1_init, 4);
  PointCloudT wing_R1_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_R1_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_R2_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_R2_init, 4);
  PointCloudT wing_R2_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_R2_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_R3_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_R3_init, 4);
  PointCloudT wing_R3_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_R3_src_pcl);
  pcl::PointCloud<pcl::PointXYZ> wing_R4_src_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_wing_R4_init, 4);
  PointCloudT wing_R4_src = MultiBodyICP::TransferPointXYZ2PointNT(wing_R4_src_pcl);

  pcl::PointCloud<pcl::PointXYZ> wing_R_dest_pcl = MultiBodyICP::Convert_Mat_2_PCL_XYZ(wing_R_pcl);
  PointCloudT wing_R_dest = MultiBodyICP::TransferPointXYZ2PointNT(wing_R_dest_pcl);

  // Perform ICP on the right wing:

  //tuple<arma::Mat<double>,double> icp_result_wing_R1 = MultiBodyICP::ICP_on_single_segment(wing_R_dest_pcl, wing_R1_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_R2 = MultiBodyICP::ICP_on_single_segment(wing_R_dest_pcl, wing_R2_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_R3 = MultiBodyICP::ICP_on_single_segment(wing_R_dest_pcl, wing_R3_src_pcl);
  //tuple<arma::Mat<double>,double> icp_result_wing_R4 = MultiBodyICP::ICP_on_single_segment(wing_R_dest_pcl, wing_R4_src_pcl);
  tuple<arma::Mat<double>,double> icp_result_wing_R1 = MultiBodyICP::RobustPoseEstimation(wing_R_dest, wing_R1_src);
  tuple<arma::Mat<double>,double> icp_result_wing_R2 = MultiBodyICP::RobustPoseEstimation(wing_R_dest, wing_R2_src);
  tuple<arma::Mat<double>,double> icp_result_wing_R3 = MultiBodyICP::RobustPoseEstimation(wing_R_dest, wing_R3_src);
  tuple<arma::Mat<double>,double> icp_result_wing_R4 = MultiBodyICP::RobustPoseEstimation(wing_R_dest, wing_R4_src);

  arma::Col<double> score_vec_R = {get<1>(icp_result_wing_R1),get<1>(icp_result_wing_R2),get<1>(icp_result_wing_R3),get<1>(icp_result_wing_R4)};

  if (score_vec_R.index_max()==0) {
    M_align_vec.push_back(get<0>(icp_result_wing_R1));
  }
  else if (score_vec_R.index_max()==1) {
    M_align_vec.push_back(get<0>(icp_result_wing_R2));
  }
  else if (score_vec_R.index_max()==2) {
    M_align_vec.push_back(get<0>(icp_result_wing_R3));
  }
  else if (score_vec_R.index_max()==3) {
    M_align_vec.push_back(get<0>(icp_result_wing_R4));
  }
  else {
    arma::Mat<double> zero_mat(4,4);
    zero_mat.zeros();
    M_align_vec.push_back(zero_mat);
  }

  return M_align_vec;
}
*/

pcl::PointCloud<pcl::PointXYZ> MultiBodyICP::GetModelSRCPointCloud(model &mod, arma::Mat<double> M_init, int seg_ind) {

  pcl::PolygonMesh part_mesh = mod.parts[seg_ind];

  // Scale the part:

  double part_scale = mod.scale[seg_ind];

  arma::Mat<double> M_part = {{M_init(0,0)*part_scale, M_init(0,1)*part_scale, M_init(0,2)*part_scale, M_init(0,3)},
    {M_init(1,0)*part_scale, M_init(1,1)*part_scale, M_init(1,2)*part_scale, M_init(1,3)},
    {M_init(2,0)*part_scale, M_init(2,1)*part_scale, M_init(2,2)*part_scale, M_init(2,3)},
    {0.0, 0.0, 0.0, 1.0}};

  // Transform the mesh:

  pcl::PolygonMesh trans_mesh = MultiBodyICP::TransformMesh(part_mesh, M_part);

  // Get Pointcloud of the mesh:

  pcl::PointCloud<pcl::PointXYZ> pcl_out = MultiBodyICP::GetPointCloudFromMesh(trans_mesh);

  return pcl_out;
}

arma::Mat<double> MultiBodyICP::CalculateMeshTransform(model &mod, arma::Mat<double> M_init, int seg_ind) {

  arma::Mat<double> R_init = M_init.submat(0,0,2,2);
  arma::Col<double> T_init = {M_init(0,3), M_init(1,3), M_init(2,3)};

  double seg_scale = mod.scale[seg_ind];

  arma::Col<double> bbox_config = mod.bounding_box_config[seg_ind];
  arma::Mat<double> M_bbox = MultiBodyICP::GetM(bbox_config);
  arma::Mat<double> R_bbox = M_bbox.submat(0,0,2,2);
  arma::Col<double> T_bbox = {M_bbox(0,3)*seg_scale, M_bbox(1,3)*seg_scale, M_bbox(2,3)*seg_scale};

  arma::Mat<double> R_out = R_init*R_bbox;
  arma::Col<double> T_out = R_init*T_bbox+T_init;
  arma::Mat<double> M_out = {{R_out(0,0), R_out(0,1), R_out(0,2), T_out(0)},
    {R_out(1,0), R_out(1,1), R_out(1,2), T_out(1)},
    {R_out(2,0), R_out(2,1), R_out(2,2), T_out(2)},
    {0.0, 0.0, 0.0, 1.0}};
  mod.M_vec.push_back(M_out);

  return M_out;
}

tuple<arma::Mat<double>,double> MultiBodyICP::RobustPoseEstimation(PointCloudT &dest_pcl, PointCloudT &src_pcl) {

  int max_iter = 50000;
  double leaf_size = 0.10;
  double search_radius = 0.25;
  int N_samples = 3;
  int N_features = 5;

  // Pointclouds:
  PointCloudT::Ptr src (new PointCloudT);
  PointCloudT::Ptr src_aligned (new PointCloudT);
  PointCloudT::Ptr dest (new PointCloudT);
  FeatureCloudT::Ptr src_features (new FeatureCloudT);
  FeatureCloudT::Ptr dest_features (new FeatureCloudT);

  *src = src_pcl;
  *dest = dest_pcl;

  // Downsampling:
  pcl::VoxelGrid<PointNT> grid;
  grid.setLeafSize (leaf_size, leaf_size, leaf_size);
  grid.setInputCloud (src);
  grid.filter (*src);
  grid.setInputCloud (dest);
  grid.filter (*dest);

  // Estimate normals:
  pcl::NormalEstimationOMP<PointNT,PointNT> nest;
  nest.setRadiusSearch (search_radius);
  nest.setInputCloud (src);
  nest.compute (*src);
  nest.setInputCloud (dest);
  nest.compute (*dest);

  // Estimate features:
  FeatureEstimationT fest;
  fest.setRadiusSearch (search_radius);
  fest.setInputCloud (src);
  fest.setInputNormals (src);
  fest.compute (*src_features);
  fest.setInputCloud (dest);
  fest.setInputNormals (dest);
  fest.compute (*dest_features);

  // Perform alignment:
  pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
  align.setInputSource (src);
  align.setSourceFeatures (src_features);
  align.setInputTarget (dest);
  align.setTargetFeatures (dest_features);
  align.setMaximumIterations (max_iter); // Number of RANSAC iterations
  align.setNumberOfSamples (N_samples); // Number of points to sample for generating/prejecting a pose
  align.setCorrespondenceRandomness (N_features); // Number of nearest features to use
  align.setSimilarityThreshold (0.5f); // Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (5.0f * leaf_size); // Inlier threshold
  align.setInlierFraction (0.1f); // Required inlier fraction ofr accepting a pose hypothesis

  {
    pcl::ScopeTime t("Alignment");
    align.align (*src_aligned);
  }

  arma::Mat<double> M_out(4,4);
  double score;

  if (align.hasConverged ()) {

    Eigen::Matrix4f transformation = align.getFinalTransformation ();

    M_out(0,0) = transformation(0,0);
    M_out(0,1) = transformation(0,1);
    M_out(0,2) = transformation(0,2);
    M_out(0,3) = transformation(0,3);
    M_out(1,0) = transformation(1,0);
    M_out(1,1) = transformation(1,1);
    M_out(1,2) = transformation(1,2);
    M_out(1,3) = transformation(1,3);
    M_out(2,0) = transformation(2,0);
    M_out(2,1) = transformation(2,1);
    M_out(2,2) = transformation(2,2);
    M_out(2,3) = transformation(2,3);
    M_out(3,0) = transformation(3,0);
    M_out(3,1) = transformation(3,1);
    M_out(3,2) = transformation(3,2);
    M_out(3,3) = transformation(3,3);

    score = 1.0;

    cout << "transformation matrix" << endl;
    cout << M_out << endl;
    cout << "inliers" << endl;
    cout << align.getInliers().size() << endl;
    cout << "" << endl;

  }
  else {
    cout << "aligned failed" << endl;
  }

  return make_tuple(M_out,score);
}

tuple<arma::Mat<double>,double> MultiBodyICP::ICP_on_single_segment(pcl::PointCloud<pcl::PointXYZ> &dest_pcl, pcl::PointCloud<pcl::PointXYZ> &src_pcl) {

  pcl::PointCloud<pcl::PointXYZ>::Ptr dest_cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr src_cloud (new pcl::PointCloud<pcl::PointXYZ>);

  *dest_cloud = dest_pcl;
  *src_cloud = src_pcl;

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  icp.setInputCloud(src_cloud);
  icp.setInputTarget(dest_cloud);

  pcl::PointCloud<pcl::PointXYZ> Final;
  icp.align(Final);

  int icp_converged = icp.hasConverged();
  double icp_score = icp.getFitnessScore();
  Eigen::Matrix4f M_final = icp.getFinalTransformation();

  cout << "has converged:" << icp_converged << " score: " << icp_score << endl;
  cout << M_final << endl;

  arma::Mat<double> M_out(4,4);

  M_out(0,0) = M_final(0,0);
  M_out(0,1) = M_final(0,1);
  M_out(0,2) = M_final(0,2);
  M_out(0,3) = M_final(0,3);
  M_out(1,0) = M_final(1,0);
  M_out(1,1) = M_final(1,1);
  M_out(1,2) = M_final(1,2);
  M_out(1,3) = M_final(1,3);
  M_out(2,0) = M_final(2,0);
  M_out(2,1) = M_final(2,1);
  M_out(2,2) = M_final(2,2);
  M_out(2,3) = M_final(2,3);
  M_out(3,0) = M_final(3,0);
  M_out(3,1) = M_final(3,1);
  M_out(3,2) = M_final(3,2);
  M_out(3,3) = M_final(3,3);

  return make_tuple(M_out,icp_score);

}

vector<arma::Mat<int>> MultiBodyICP::ReturnProjectedImages(frames &frame_in, model &mod, vox_grid &vox, FocalGrid &fcg) {

  // Compute initial orientation matrices:

  arma::Mat<double> M_body_init = frame_in.M_init_single_frame[0];
  arma::Mat<double> M_wing_L_init = frame_in.M_init_single_frame[1];
  arma::Mat<double> M_wing_R_init = frame_in.M_init_single_frame[2];

  // body:
  arma::Mat<double> M_thorax_init  = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init    = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  // left wing:
  arma::Mat<double> M_wing_L1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,0,3,3), 3);

  // right wing:
  arma::Mat<double> M_wing_R1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,0,3,3), 4);

  // Project the model segments from 3d to 2d and back to 3d:

  vector<arma::Col<int>> mdl_pcl_thorax  = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_thorax_init, 0, 10);
  vector<arma::Col<int>> mdl_pcl_head    = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_head_init, 1, 10);
  vector<arma::Col<int>> mdl_pcl_abdomen = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_abdomen_init, 2, 10);
  vector<arma::Col<int>> mdl_pcl_wing_L1 = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_wing_L1_init, 3, 3);
  vector<arma::Col<int>> mdl_pcl_wing_R1 = MultiBodyICP::GetProjectedImages(mod, vox, fcg, M_wing_R1_init, 4, 3);

  vector<arma::Mat<int>> proj_frames;

  int N_frames = mdl_pcl_thorax.size();

  for (int i=0; i<N_frames; i++) {

    arma::Col<int> frame = mdl_pcl_thorax[i]+mdl_pcl_head[i]+mdl_pcl_abdomen[i]+mdl_pcl_wing_L1[i]+mdl_pcl_wing_R1[i];

    int N_row = get<0>(frame_in.image_size[i]);
    int N_col = get<1>(frame_in.image_size[i]);

    arma::Mat<int> frame_mat(N_row,N_col);

    for (int j=0; j<N_col; j++) {
      for (int k=0; k<N_row; k++) {
        if (frame(k*N_col+j)>10) {
          frame_mat(k,j) = 10;
        }
        else {
          frame_mat(k,j) = frame(k*N_col+j);
        }
      }
    }

    proj_frames.push_back(frame_mat);

  }

  return proj_frames;
}

vector<arma::Col<int>> MultiBodyICP::GetProjectedImages(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> M_init, int seg_ind, int int_val) {

  vector<arma::Mat<double>> proj_images;

  pcl::PointCloud<pcl::PointXYZ> seg_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);

  vector<tuple<double,double,double,int>> seg_pcl_vec = MultiBodyICP::Convert_PCL_XYZ_2_Vec(seg_pcl, int_val);

  vector<arma::Col<int>> proj_frames = fcg.ProjectCloud2Image(seg_pcl_vec);

  return proj_frames;
}

arma::Mat<double> MultiBodyICP::ReturnSRCPCL(frames &frame_in, model &mod, vox_grid &vox, FocalGrid &fcg) {

  // Compute initial orientation matrices:

  arma::Mat<double> M_body_init   = frame_in.M_init_single_frame[0];
  arma::Mat<double> M_wing_L_init = frame_in.M_init_single_frame[1];
  arma::Mat<double> M_wing_R_init = frame_in.M_init_single_frame[2];

  // body:
  arma::Mat<double> M_thorax_init  = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init    = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  // left wing:
  arma::Mat<double> M_wing_L1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,0,3,3), 3);
  arma::Mat<double> M_wing_L2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,4,3,7), 3);
  arma::Mat<double> M_wing_L3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,8,3,11), 3);
  arma::Mat<double> M_wing_L4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,12,3,15), 3);

  // right wing:
  arma::Mat<double> M_wing_R1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,0,3,3), 4);
  arma::Mat<double> M_wing_R2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,4,3,7), 4);
  arma::Mat<double> M_wing_R3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,8,3,11), 4);
  arma::Mat<double> M_wing_R4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,12,3,15), 4);

  // Project the model segments from 3d to 2d and back to 3d:

  arma::Mat<double> mdl_pcl_thorax  = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_thorax_init,  0, 1);
  arma::Mat<double> mdl_pcl_head    = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_head_init,    1, 1);
  arma::Mat<double> mdl_pcl_abdomen = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_abdomen_init, 2, 1);
  arma::Mat<double> mdl_pcl_wing_L1 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L1_init, 3, 1);
  arma::Mat<double> mdl_pcl_wing_L2 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L2_init, 3, 1);
  arma::Mat<double> mdl_pcl_wing_L3 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L3_init, 3, 1);
  arma::Mat<double> mdl_pcl_wing_L4 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L4_init, 3, 1);
  arma::Mat<double> mdl_pcl_wing_R1 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R1_init, 4, 1);
  arma::Mat<double> mdl_pcl_wing_R2 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R2_init, 4, 1);
  arma::Mat<double> mdl_pcl_wing_R3 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R3_init, 4, 1);
  arma::Mat<double> mdl_pcl_wing_R4 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R4_init, 4, 1);

  arma::Mat<double> pcl_mat_out = mdl_pcl_thorax;
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_head);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_abdomen);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_L1);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_L2);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_L3);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_L4);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_R1);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_R2);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_R3);
  pcl_mat_out = arma::join_rows(pcl_mat_out,mdl_pcl_wing_R4);

  return pcl_mat_out;
}

arma::Mat<double> MultiBodyICP::SingleFrameICP(frames &frame_in, model &mod, vox_grid &vox, FocalGrid &fcg) {

  clock_t start;
  double duration;

  start = clock();

  arma::Mat<double> body_pcl = frame_in.pcl_init_single_frame[0];
  arma::Mat<double> M_body_init   = frame_in.M_init_single_frame[0];

  arma::Mat<double> M_thorax_init  = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init    = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  tuple<arma::Mat<double>,arma::Mat<double>,double> thorax_icp_results  = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, M_thorax_init,  0, 1);
  tuple<arma::Mat<double>,arma::Mat<double>,double> head_icp_results    = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, M_head_init,  1, 3);
  tuple<arma::Mat<double>,arma::Mat<double>,double> abdomen_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, M_abdomen_init,  2, 5);

  arma::Mat<double> pcl_mat_out = get<0>(thorax_icp_results);
  pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(head_icp_results));
  pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(abdomen_icp_results));

  duration = (clock()-start)/ (double) CLOCKS_PER_SEC;

  cout << "ICP took" << endl;
  cout << duration << endl;

  return pcl_mat_out;

}

/*
arma::Mat<double> MultiBodyICP::SingleFrameICP(frames &frame_in, model &mod, vox_grid &vox, FocalGrid &fcg) {

  // Compute initial orientation matrices:

  arma::Mat<double> body_pcl = frame_in.pcl_init_single_frame[0];
  arma::Mat<double> wing_L_pcl = frame_in.pcl_init_single_frame[1];
  arma::Mat<double> wing_R_pcl = frame_in.pcl_init_single_frame[2];

  arma::Mat<double> M_body_init   = frame_in.M_init_single_frame[0];
  arma::Mat<double> M_wing_L_init = frame_in.M_init_single_frame[1];
  arma::Mat<double> M_wing_R_init = frame_in.M_init_single_frame[2];

  // body:
  arma::Mat<double> M_thorax_init  = MultiBodyICP::CalculateMeshTransform(mod, M_body_init, 0);
  arma::Mat<double> M_head_init    = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 1);
  arma::Mat<double> M_abdomen_init = MultiBodyICP::CalculateMeshTransform(mod, M_thorax_init, 2);

  // left wing:
  arma::Mat<double> M_wing_L1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,0,3,3), 3);
  arma::Mat<double> M_wing_L2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,4,3,7), 3);
  arma::Mat<double> M_wing_L3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,8,3,11), 3);
  arma::Mat<double> M_wing_L4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_L_init.submat(0,12,3,15), 3);

  // right wing:
  arma::Mat<double> M_wing_R1_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,0,3,3), 4);
  arma::Mat<double> M_wing_R2_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,4,3,7), 4);
  arma::Mat<double> M_wing_R3_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,8,3,11), 4);
  arma::Mat<double> M_wing_R4_init = MultiBodyICP::CalculateMeshTransform(mod, M_wing_R_init.submat(0,12,3,15), 4);

  // Project the model segments from 3d to 2d and back to 3d:

  arma::Mat<double> mdl_pcl_thorax  = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_thorax_init,  0, 1);
  arma::Mat<double> mdl_pcl_head    = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_head_init,    1, 1);
  arma::Mat<double> mdl_pcl_abdomen = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_abdomen_init, 2, 1);
  arma::Mat<double> mdl_pcl_wing_L1 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L1_init, 3, 2);
  arma::Mat<double> mdl_pcl_wing_L2 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L2_init, 3, 2);
  arma::Mat<double> mdl_pcl_wing_L3 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L3_init, 3, 2);
  arma::Mat<double> mdl_pcl_wing_L4 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_L4_init, 3, 2);
  arma::Mat<double> mdl_pcl_wing_R1 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R1_init, 4, 3);
  arma::Mat<double> mdl_pcl_wing_R2 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R2_init, 4, 3);
  arma::Mat<double> mdl_pcl_wing_R3 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R3_init, 4, 3);
  arma::Mat<double> mdl_pcl_wing_R4 = MultiBodyICP::GetProjectedModelPCL(mod, vox, fcg, M_wing_R4_init, 4, 3);

  // Perform ICP:
  tuple<arma::Mat<double>,arma::Mat<double>,double> thorax_icp_results  = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, mdl_pcl_thorax,  M_thorax_init,  0, 1);
  tuple<arma::Mat<double>,arma::Mat<double>,double> head_icp_results    = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, mdl_pcl_head,     M_head_init,    1, 2);
  tuple<arma::Mat<double>,arma::Mat<double>,double> abdomen_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, body_pcl, mdl_pcl_abdomen, M_abdomen_init, 2, 3);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_L1_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_L_pcl, mdl_pcl_wing_L1, M_wing_L1_init, 3, 5);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_L2_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_L_pcl, mdl_pcl_wing_L2, M_wing_L2_init, 3, 5);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_L3_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_L_pcl, mdl_pcl_wing_L3, M_wing_L1_init, 3, 5);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_L4_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_L_pcl, mdl_pcl_wing_L4, M_wing_L2_init, 3, 5);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_R1_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_R_pcl, mdl_pcl_wing_R1, M_wing_R1_init, 4, 10);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_R2_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_R_pcl, mdl_pcl_wing_R2, M_wing_R2_init, 4, 10);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_R3_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_R_pcl, mdl_pcl_wing_R3, M_wing_R1_init, 4, 10);
  tuple<arma::Mat<double>,arma::Mat<double>,double> wing_R4_icp_results = MultiBodyICP::ICPAlgorithm(mod, vox, fcg, wing_R_pcl, mdl_pcl_wing_R4, M_wing_R2_init, 4, 10);

  // Find best fit:
  arma::Mat<double> pcl_mat_out = get<0>(thorax_icp_results);
  pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(head_icp_results));
  pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(abdomen_icp_results));

  arma::Row<double> score_vec_L = {get<2>(wing_L1_icp_results), get<2>(wing_L2_icp_results),get<2>(wing_L3_icp_results), get<2>(wing_L4_icp_results)};
  arma::Row<double> score_vec_R = {get<2>(wing_R1_icp_results), get<2>(wing_R2_icp_results),get<2>(wing_R3_icp_results), get<2>(wing_R4_icp_results)};

  if (score_vec_L.index_min()==0) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_L1_icp_results));
  }
  else if (score_vec_L.index_min()==1) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_L2_icp_results));
  }
  else if (score_vec_L.index_min()==2) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_L3_icp_results));
  }
  else if (score_vec_L.index_min()==3) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_L4_icp_results));
  }

  if (score_vec_R.index_min()==0) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_R1_icp_results));
  }
  else if (score_vec_R.index_min()==1) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_R2_icp_results));
  }
  else if (score_vec_R.index_min()==2) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_R3_icp_results));
  }
  else if (score_vec_R.index_min()==3) {
    pcl_mat_out = arma::join_rows(pcl_mat_out,get<0>(wing_R4_icp_results));
  }

  return pcl_mat_out;
}
*/

//tuple<arma::Mat<double>,double> MultiBodyICP::PairAlign(arma::Mat<double> &dest_pcl, arma::Mat<double> &src_pcl) {
tuple<arma::Mat<double>,double> MultiBodyICP::PairAlign(arma::Mat<double> &dest_pcl, model &mod, arma::Mat<double> M_init, int seg_ind) {

  //PointCloud cloud_src = MultiBodyICP::Convert_Mat_2_PCL_XYZ(src_pcl);
  //PointCloud cloud_tgt = MultiBodyICP::Convert_Mat_2_PCL_XYZ(dest_pcl);

  PointCloud cloud_src = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);
  PointCloud cloud_tgt = MultiBodyICP::Convert_Mat_2_PCL_XYZ(dest_pcl);

  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);

  PointCloud::Ptr cloud_src2 (new PointCloud);
  PointCloud::Ptr cloud_tgt2 (new PointCloud);

  *cloud_src2 = cloud_src;
  *cloud_tgt2 = cloud_tgt;

  pcl::VoxelGrid<PointT> grid;

  grid.setLeafSize (0.06, 0.06, 0.06);
  grid.setInputCloud (cloud_src2);
  grid.filter (*src);

  grid.setInputCloud (cloud_tgt2);
  grid.filter (*tgt);

  pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
  //pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icp;

  icp.setMaxCorrespondenceDistance(0.2);
  icp.setMaximumIterations(50);
  icp.setTransformationEpsilon(0.001);
  //icp.setEuclideanFitnessEpsilon(1);
  icp.setRANSACIterations(50);
  icp.setRANSACOutlierRejectionThreshold(0.06);

  icp.setInputCloud(src);
  icp.setInputTarget(tgt);

  pcl::PointCloud<pcl::PointXYZ> Final;

  icp.align(Final);

  double score = icp.getFitnessScore();

    // Get the transformation from target to source
  //targetToSource = Ti.inverse();

  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity ();

  Ti = icp.getFinalTransformation();

  arma::Mat<double> M_transform(4,4);

  M_transform(0,0) = Ti(0,0);
  M_transform(0,1) = Ti(0,1);
  M_transform(0,2) = Ti(0,2);
  M_transform(0,3) = Ti(0,3);
  M_transform(1,0) = Ti(1,0);
  M_transform(1,1) = Ti(1,1);
  M_transform(1,2) = Ti(1,2);
  M_transform(1,3) = Ti(1,3);
  M_transform(2,0) = Ti(2,0);
  M_transform(2,1) = Ti(2,1);
  M_transform(2,2) = Ti(2,2);
  M_transform(2,3) = Ti(2,3);
  M_transform(3,0) = 0.0;
  M_transform(3,1) = 0.0;
  M_transform(3,2) = 0.0;
  M_transform(3,3) = 1.0;

  return make_tuple(M_transform,score);

}

/*
//tuple<arma::Mat<double>,double> MultiBodyICP::PairAlign(arma::Mat<double> &dest_pcl, arma::Mat<double> &src_pcl) {
tuple<arma::Mat<double>,double> MultiBodyICP::PairAlign(arma::Mat<double> &dest_pcl, model &mod, arma::Mat<double> M_init, int seg_ind) {

  //PointCloud cloud_src = MultiBodyICP::Convert_Mat_2_PCL_XYZ(src_pcl);
  PointCloud cloud_src = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);
  PointCloud cloud_tgt = MultiBodyICP::Convert_Mat_2_PCL_XYZ(dest_pcl);

  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);

  PointCloud::Ptr cloud_src2 (new PointCloud);
  PointCloud::Ptr cloud_tgt2 (new PointCloud);

  *cloud_src2 = cloud_src;
  *cloud_tgt2 = cloud_tgt;

  //*src = cloud_src;
  //*tgt = cloud_tgt;

  pcl::VoxelGrid<PointT> grid;

  grid.setLeafSize (0.06, 0.06, 0.06);
  grid.setInputCloud (cloud_src2);
  grid.filter (*src);

  grid.setInputCloud (cloud_tgt2);
  grid.filter (*tgt);

  // Compute surface normals and curvature
  PointCloudT::Ptr points_with_normals_src (new PointCloudT);
  PointCloudT::Ptr points_with_normals_tgt (new PointCloudT);

  //pcl::NormalEstimation<PointT, PointNT> norm_est;
  pcl::NormalEstimationOMP<PointT, PointNT> norm_est;
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  norm_est.setSearchMethod (tree);
  norm_est.setKSearch (30);

  norm_est.setInputCloud (src);
  norm_est.compute (*points_with_normals_src);
  pcl::copyPointCloud (*src, *points_with_normals_src);

  norm_est.setInputCloud (tgt);
  norm_est.compute (*points_with_normals_tgt);
  pcl::copyPointCloud (*tgt, *points_with_normals_tgt);

  // Instantiate our custom point representation (defined above) ...
  MyPointRepresentation point_representation;
  // ... and weight the 'curvature' dimension so that it is balanced against x, y, and z
  float alpha[4] = {1.0, 1.0, 1.0, 1.0};
  point_representation.setRescaleValues (alpha);

  // Align
  pcl::IterativeClosestPointNonLinear<PointNT, PointNT> reg;
  reg.setTransformationEpsilon (0.001);
  //reg.setEuclideanFitnessEpsilon(1e-2);
  // Set the maximum distance between two correspondences (src<->tgt) to 10cm
  // Note: adjust this based on the size of your datasets
  reg.setMaxCorrespondenceDistance (0.2);
  // Set the point representation
  reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));

  reg.setInputSource (points_with_normals_src);
  reg.setInputTarget (points_with_normals_tgt);

  // Run the same optimization in a loop and visualize the results
  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;
  PointCloudT::Ptr reg_result = points_with_normals_src;
  reg.setMaximumIterations (30);

  double score = 1.0;

  clock_t start;
  double duration;

  start = clock();

  for (int i = 0; i < 30; ++i) {
    // save cloud for visualization purpose
    points_with_normals_src = reg_result;

    // Estimate
    reg.setInputSource (points_with_normals_src);
    reg.align (*reg_result);

    //accumulate transformation between each Iteration
    Ti = reg.getFinalTransformation () * Ti;

    //if the difference between this transformation and the previous one
    //is smaller than the threshold, refine the process by reducing
    //the maximal correspondence distance
    if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())
      reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);

    prev = reg.getLastIncrementalTransformation ();

    score = reg.getFitnessScore();
  }

  duration = (clock()-start)/ (double) CLOCKS_PER_SEC;

  cout << "iteration time" << endl;
  cout << duration << endl;

  // Get the transformation from target to source
  //targetToSource = Ti.inverse();

  arma::Mat<double> M_transform(4,4);

  M_transform(0,0) = Ti(0,0);
  M_transform(0,1) = Ti(0,1);
  M_transform(0,2) = Ti(0,2);
  M_transform(0,3) = Ti(0,3);
  M_transform(1,0) = Ti(1,0);
  M_transform(1,1) = Ti(1,1);
  M_transform(1,2) = Ti(1,2);
  M_transform(1,3) = Ti(1,3);
  M_transform(2,0) = Ti(2,0);
  M_transform(2,1) = Ti(2,1);
  M_transform(2,2) = Ti(2,2);
  M_transform(2,3) = Ti(2,3);
  M_transform(3,0) = 0.0;
  M_transform(3,1) = 0.0;
  M_transform(3,2) = 0.0;
  M_transform(3,3) = 1.0;

  return make_tuple(M_transform,score);
}

/*
tuple<arma::Mat<double>,arma::Mat<double>,double> MultiBodyICP::ICPAlgorithm(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> &dest_pcl, arma::Mat<double> M_init, int seg_ind, int int_val) {

  arma::Mat<double> src_pcl_i;

  PointCloud cloud_src;
  PointCloud cloud_tgt;

  PointCloud::Ptr src (new PointCloud);
  PointCloud::Ptr tgt (new PointCloud);

  // Non-linear closest point:

  pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> icp;

  icp.setMaxCorrespondenceDistance(0.3);
  icp.setMaximumIterations(30);
  icp.setTransformationEpsilon(0.001);
  icp.setEuclideanFitnessEpsilon(1);
  icp.setRANSACIterations(30);
  icp.setRANSACOutlierRejectionThreshold(0.04);

  // Set target:
  cloud_tgt = MultiBodyICP::Convert_Mat_2_PCL_XYZ(dest_pcl);
  *tgt = cloud_tgt;
  icp.setInputTarget(tgt);

  // Iteration parameters:
  int max_iter = 10;
  double prev_score = 1e6;
  int i = 0;

  // Output parameters:
  arma::Mat<double> M_out;
  double score = 1.0;
  pcl::PointCloud<pcl::PointXYZ> Final;
  Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity ();
  arma::Mat<double> M_delta(4,4);
  arma::Mat<double> M_update;
  arma::Mat<double> src_pcl_out;
  M_update = M_init;

  while ((i < max_iter) && ((score/prev_score)<1.02)) {

    // Set src:
    src_pcl_i = MultiBodyICP::GetProjectedModelPCL(mod,vox,fcg,M_update,seg_ind,int_val);
    cloud_src = MultiBodyICP::Convert_Mat_2_PCL_XYZ(src_pcl_i);
    *src = cloud_src;
    icp.setInputCloud(src);

    // Align:
    icp.align(Final);

    score = icp.getFitnessScore();

    Ti = icp.getFinalTransformation();

    M_delta(0,0) = Ti(0,0);
    M_delta(0,1) = Ti(0,1);
    M_delta(0,2) = Ti(0,2);
    M_delta(0,3) = Ti(0,3);
    M_delta(1,0) = Ti(1,0);
    M_delta(1,1) = Ti(1,1);
    M_delta(1,2) = Ti(1,2);
    M_delta(1,3) = Ti(1,3);
    M_delta(2,0) = Ti(2,0);
    M_delta(2,1) = Ti(2,1);
    M_delta(2,2) = Ti(2,2);
    M_delta(2,3) = Ti(2,3);
    M_delta(3,0) = 0.0;
    M_delta(3,1) = 0.0;
    M_delta(3,2) = 0.0;
    M_delta(3,3) = 1.0;

    M_update = M_delta*M_update;

    if ((score/prev_score)<1.02) {
      src_pcl_out = src_pcl_i;
      M_out = M_update;
      prev_score = score;
    }
    else {
      i = max_iter;
    }

    i++;
  }

  return make_tuple(src_pcl_out,M_out,score);
}
*/

tuple<arma::Mat<double>,arma::Mat<double>,double> MultiBodyICP::ICPAlgorithm(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> &dest_pcl, arma::Mat<double> M_init, int seg_ind, int int_val) {

  arma::Mat<double> M_update;
  M_update = M_init;

  tuple<arma::Mat<double>,double> pair_update = MultiBodyICP::PairAlign(dest_pcl, mod, M_init, seg_ind);

  double score = get<1>(pair_update);

  cout << "score" << endl;
  cout << score << endl;

  arma::Mat<double> M_delta = get<0>(pair_update);

  cout << M_delta << endl;

  M_update.submat(0,0,2,2) = M_delta.submat(0,0,2,2)*M_update.submat(0,0,2,2);
  M_update(0,3) = M_update(0,3)+M_delta(0,3);
  M_update(1,3) = M_update(1,3)+M_delta(1,3);
  M_update(2,3) = M_update(2,3)+M_delta(2,3);

  arma::Mat<double> src_pcl_out = MultiBodyICP::GetProjectedModelPCL(mod,vox,fcg,M_update,seg_ind,int_val);

  return make_tuple(src_pcl_out,M_update,score);
}

/*
tuple<arma::Mat<double>,arma::Mat<double>,double> MultiBodyICP::ICPAlgorithm(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> &dest_pcl, arma::Mat<double> M_init, int seg_ind, int int_val) {

  double score = 1.0;
  arma::Mat<double> src_pcl_i;
  arma::Mat<double> src_pcl_out;
  arma::Mat<double> M_update;
  M_update = M_init;
  arma::Mat<double> M_out;
  M_out.eye(4,4);

  int max_iter = 30;
  double prev_score = 1e6;
  int i = 0;

  //for (int i=0; i<20; i++) {
  while ((i < max_iter) && ((score/prev_score)<1.05)) {

    cout << "iteration" << endl;
    cout << i << endl;

    src_pcl_i = MultiBodyICP::GetProjectedModelPCL(mod,vox,fcg,M_update,seg_ind,int_val);

    tuple<arma::Mat<double>,double> pair_update = MultiBodyICP::PairAlign(dest_pcl, src_pcl_i);

    //score = get<1>(pair_update);
    score = get<1>(pair_update);
    cout << "score" << endl;
    cout << score << endl;

    arma::Mat<double> M_delta = get<0>(pair_update);

    M_update.submat(0,0,2,2) = M_update.submat(0,0,2,2)*M_delta.submat(0,0,2,2);
    M_update(0,3) = M_update(0,3)+M_delta(0,3);
    M_update(1,3) = M_update(1,3)+M_delta(1,3);
    M_update(2,3) = M_update(2,3)+M_delta(2,3);

    if ((score/prev_score)<1.05) {

      M_out = M_update;
      src_pcl_out = src_pcl_i;
      prev_score = score;

    }
    else {
      i = max_iter;
    }

    i++;
  }

  return make_tuple(src_pcl_out,M_out,score);
}
*/

double MultiBodyICP::ProjectedModelScore(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> &dest_pcl, arma::Mat<double> M_init, int seg_ind, int int_val) {

  pcl::PointCloud<pcl::PointXYZ> seg_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);

  vector<tuple<double,double,double,int>> seg_pcl_vec = MultiBodyICP::Convert_PCL_XYZ_2_Vec(seg_pcl, int_val);

  vector<tuple<double,double,double,int>> dest_pcl_vec = MultiBodyICP::Convert_Mat_2_PCL_Vec(dest_pcl, int_val);

  vector<arma::Col<int>> proj_mdl_frames = fcg.ProjectCloud2Image(seg_pcl_vec);

  vector<arma::Col<int>> proj_dest_frames = fcg.ProjectCloud2Image(dest_pcl_vec);

  int N_views = proj_mdl_frames.size();

  double score = 0.0;

  for (int i=0; i<N_views; i++) {

    arma::Col<int> frame_diff = arma::abs(proj_dest_frames[i]-proj_mdl_frames[i]);

    score += (1.0*arma::sum(frame_diff))/(1.0*arma::sum(proj_dest_frames[i]));

  }

  return score;

}

arma::Mat<double> MultiBodyICP::GetProjectedModelPCL(model &mod, vox_grid &vox, FocalGrid &fcg, arma::Mat<double> M_init, int seg_ind, int int_val) {

  // Project model pointcloud to 2d images:

  pcl::PointCloud<pcl::PointXYZ> seg_pcl = MultiBodyICP::GetModelSRCPointCloud(mod, M_init, seg_ind);

  vector<tuple<double,double,double,int>> seg_pcl_vec = MultiBodyICP::Convert_PCL_XYZ_2_Vec(seg_pcl, int_val);

  vector<arma::Col<int>> proj_mdl_frames = fcg.ProjectCloud2Image(seg_pcl_vec);

  // Project 2d images to pointcloud:

  vector<tuple<int,double,double,double,double,double,double>> proj_pcl = fcg.ProjectImage2Cloud(proj_mdl_frames, vox);

  arma::Mat<double> pcl_out = MultiBodyICP::Convert_PCL_Vec_2_Mat(proj_pcl);

  return pcl_out;
}

pcl::PolygonMesh MultiBodyICP::TransformMesh(pcl::PolygonMesh mesh_in, arma::Mat<double> transform_mat) {

  pcl::PCLHeader header = mesh_in.header;

  pcl::PCLPointCloud2 cloud2_in = mesh_in.cloud;

  vector<pcl::Vertices> polygons = mesh_in.polygons;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::fromPCLPointCloud2(cloud2_in, *cloud_in);

  // Construct the transformation matrix

  Eigen::Matrix4f trans_mat = Eigen::Matrix4f::Identity();

  trans_mat(0,0) = transform_mat(0,0);
  trans_mat(0,1) = transform_mat(0,1);
  trans_mat(0,2) = transform_mat(0,2);
  trans_mat(0,3) = transform_mat(0,3);
  trans_mat(1,0) = transform_mat(1,0);
  trans_mat(1,1) = transform_mat(1,1);
  trans_mat(1,2) = transform_mat(1,2);
  trans_mat(1,3) = transform_mat(1,3);
  trans_mat(2,0) = transform_mat(2,0);
  trans_mat(2,1) = transform_mat(2,1);
  trans_mat(2,2) = transform_mat(2,2);
  trans_mat(2,3) = transform_mat(2,3);

  // Transform the pointcloud

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

  pcl::transformPointCloud(*cloud_in, *cloud_out, trans_mat);

  pcl::PCLPointCloud2::Ptr cloud2_out (new pcl::PCLPointCloud2);

  pcl::toPCLPointCloud2(*cloud_out,*cloud2_out);

  pcl::PolygonMesh mesh_out;

  mesh_out.header = header;
  mesh_out.polygons = polygons;
  mesh_out.cloud = *cloud2_out;

  return mesh_out;

}

pcl::PointCloud<pcl::PointXYZ> MultiBodyICP::GetPointCloudFromMesh(pcl::PolygonMesh mesh_in) {

  pcl::PointCloud<pcl::PointXYZ> cloud_out;

  pcl::PCLPointCloud2 cloud2_in = mesh_in.cloud;

  pcl::fromPCLPointCloud2(cloud2_in, cloud_out);

  return cloud_out;
}

arma::Mat<double> MultiBodyICP::MultiplyMat(arma::Mat<double> M_a, arma::Mat<double> M_b) {

  arma::Mat<double> mult_mat;

  mult_mat.zeros(4,4);

  mult_mat.submat(0,0,2,2) = M_a.submat(0,0,2,2)*M_b.submat(0,0,2,2);

  mult_mat.submat(0,0,2,0) = M_a.submat(0,0,2,2)*M_b.submat(0,0,2,0)+M_a.submat(0,0,2,0);

  //mult_mat(0,3) = M_a(0,3)+M_b(0,3);
  //mult_mat(1,3) = M_a(1,3)+M_b(1,3);
  //mult_mat(2,3) = M_a(2,3)+M_b(2,3);

  mult_mat(3,3) = 1.0;

  return mult_mat;

}

arma::Mat<double> MultiBodyICP::TransformMat(arma::Col<double> q_in, double scale) {

  arma::Mat<double> start_mat;

  start_mat.zeros(4,4);

  start_mat(0,0) = (2.0*pow(q_in(0),2)-1.0+2.0*pow(q_in(1),2));
  start_mat(0,1) = (2.0*q_in(1)*q_in(2)+2.0*q_in(0)*q_in(3));
  start_mat(0,2) = (2.0*q_in(1)*q_in(3)-2.0*q_in(0)*q_in(2));
  start_mat(0,3) = scale*(q_in(4));
  start_mat(1,0) = (2.0*q_in(1)*q_in(2)-2.0*q_in(0)*q_in(3));
  start_mat(1,1) = (2.0*pow(q_in(0),2)-1.0+2.0*pow(q_in(2),2));
  start_mat(1,2) = (2.0*q_in(2)*q_in(3)+2.0*q_in(0)*q_in(1));
  start_mat(1,3) = scale*(q_in(5));
  start_mat(2,0) = (2.0*q_in(1)*q_in(3)+2.0*q_in(0)*q_in(2));
  start_mat(2,1) = (2.0*q_in(2)*q_in(3)-2.0*q_in(0)*q_in(1));
  start_mat(2,2) = (2.0*pow(q_in(0),2)-1.0+2.0*pow(q_in(3),2));
  start_mat(2,3) = scale*(q_in(6));
  start_mat(3,3) = 1.0;

  return start_mat;

}

arma::Mat<double> MultiBodyICP::Convert_PCL_Vec_2_Mat(vector<tuple<int,double,double,double,double,double,double>> pcl_in) {

  int N_points = pcl_in.size();

  arma::Mat<double> pcl_out(7,N_points);

  for (int i=0; i<N_points; i++) {

    pcl_out(0,i) = (double) get<0>(pcl_in[i]);
    pcl_out(1,i) = get<1>(pcl_in[i]);
    pcl_out(2,i) = get<2>(pcl_in[i]);
    pcl_out(3,i) = get<3>(pcl_in[i]);
    pcl_out(4,i) = get<4>(pcl_in[i]);
    pcl_out(5,i) = get<5>(pcl_in[i]);
    pcl_out(6,i) = get<6>(pcl_in[i]);

  }

  return pcl_out;
}

vector<tuple<double,double,double,int>> MultiBodyICP::Convert_PCL_XYZ_2_Vec(pcl::PointCloud<pcl::PointXYZ> pcl_in, int ind) {

  vector<tuple<double,double,double,int>> pcl_out;

  for (int i=0; i<pcl_in.points.size(); i++) {

    pcl_out.push_back(make_tuple(pcl_in.points[i].x,pcl_in.points[i].y,pcl_in.points[i].z,ind));

  }

  return pcl_out;
}

vector<tuple<double,double,double,int>> MultiBodyICP::Convert_Mat_2_PCL_Vec(arma::Mat<double> &pcl_in, int ind) {

  vector<tuple<double,double,double,int>> pcl_out;

  for (int i=0; i<pcl_in.n_cols; i++) {

    pcl_out.push_back(make_tuple(pcl_in(1,i),pcl_in(2,i),pcl_in(3,i),ind));

  }

  return pcl_out;
}

pcl::PointCloud<pcl::PointXYZ> MultiBodyICP::Convert_Mat_2_PCL_XYZ(arma::Mat<double> pcl_mat) {

  // Convert an armadillo matrix to a PointCloud vector

  pcl::PointCloud<pcl::PointXYZ> pcl_vec;

  int N_points = pcl_mat.n_cols;

  for (int i=0; i<N_points; i++) {

    pcl::PointXYZ point;
    point.x = pcl_mat(1,i);
    point.y = pcl_mat(2,i);
    point.z = pcl_mat(3,i);

    pcl_vec.push_back(point);

  }

  return pcl_vec;

}

arma::Mat<double> MultiBodyICP::GetM(arma::Col<double> state_in) {

  arma::Col<double> q_vec = {state_in(0),state_in(1),state_in(2),state_in(3)};
  q_vec = q_vec/sqrt(pow(q_vec(0),2)+pow(q_vec(1),2)+pow(q_vec(2),2)+pow(q_vec(3),2));

  double q0 = q_vec(0);
  double q1 = q_vec(1);
  double q2 = q_vec(2);
  double q3 = q_vec(3);
  double tx = state_in(4);
  double ty = state_in(5);
  double tz = state_in(6);

  arma::Mat<double> M = {{2.0*pow(q0,2.0)-1.0+2.0*pow(q1,2.0), 2.0*q1*q2-2.0*q0*q3, 2.0*q1*q3+2.0*q0*q2, tx},
    {2.0*q1*q2+2.0*q0*q3, 2.0*pow(q0,2.0)-1.0+2.0*pow(q2,2.0), 2.0*q2*q3-2.0*q0*q1, ty},
    {2.0*q1*q3-2.0*q0*q2, 2.0*q2*q3+2.0*q0*q1, 2.0*pow(q0,2.0)-1.0+2.0*pow(q3,2.0), tz},
    {0.0, 0.0, 0.0, 1.0}};

  return M;

}

PointCloudT MultiBodyICP::TransferPointXYZ2PointNT(pcl::PointCloud<pcl::PointXYZ> pcl_in) {

  PointCloudT pcl_out;

  for (int i=0; i<pcl_in.points.size(); i++) {

    PointNT point_out;

    point_out.x = pcl_in.points[i].x;
    point_out.y = pcl_in.points[i].y;
    point_out.z = pcl_in.points[i].z;
    point_out.normal_x = 1.0;
    point_out.normal_y = 0.0;
    point_out.normal_z = 0.0;

    pcl_out.push_back(point_out);

  }
  return pcl_out;
}

arma::Mat<double> MultiBodyICP::TransferPointCloudT2Mat(PointCloudT pcl_in, int int_val) {

  int N_points = pcl_in.points.size();

  arma::Mat<double> pcl_out(7,N_points);

  for (int i=0; i<N_points; i++) {
    pcl_out(0,i) = (double) int_val;
    pcl_out(1,i) = pcl_in.points[i].x;
    pcl_out(2,i) = pcl_in.points[i].y;
    pcl_out(3,i) = pcl_in.points[i].z;
    pcl_out(4,i) = 1.0;
    pcl_out(5,i) = 0.0;
    pcl_out(6,i) = 0.0;
  }

  return pcl_out;
}


/*
  [auto_generated]
  boost/numeric/odeint/external/vexcl/vexcl_norm_inf.hpp

  [begin_description]
  vector_space_norm_inf specialization for vexcl
  [end_description]

  Copyright 2009-2013 Karsten Ahnert
  Copyright 2009-2013 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_NORM_INF_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_NORM_INF_HPP_DEFINED

#include <map>
#include <algorithm>

#include <vexcl/vector.hpp>
#include <vexcl/multivector.hpp>
#include <vexcl/reductor.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>

namespace boost {
namespace numeric {
namespace odeint {

namespace detail {

// norm_inf specializations for vexcl types need an instance of
// vex::Reductor<T> to be available. This function returns reference
// to such an instance:
template <typename T>
const vex::Reductor<T, vex::MAX>&
vexcl_reductor(const std::vector<cl::CommandQueue> &queue)
{
    // We will hold one static reductor per set of queues (or, rather, contexts):
    static std::map< std::vector<cl_context>, vex::Reductor<T, vex::MAX> > cache;

    // Extract OpenCL context handles from command queues:
    std::vector<cl_context> ctx;
    ctx.reserve(queue.size());
    for(auto q = queue.begin(); q != queue.end(); ++q)
        ctx.push_back( vex::qctx(*q)() );

    // See if there is suitable instance of reductor already:
    auto r = cache.find(ctx);

    // If not, create new instance and move it to the cache.
    if (r == cache.end())
        r = cache.insert( std::make_pair(
                    std::move(ctx), vex::Reductor<T, vex::MAX>(queue)
                    ) ).first;

    return r->second;
}

} // namespace detail

// specialization for vexcl vector
template <typename T>
struct vector_space_norm_inf< vex::vector<T> > {
    typedef T result_type;

    T operator()( const vex::vector<T> &x ) const {
        auto max = detail::vexcl_reductor<T>(x.queue_list());

        return max( fabs(x) );
    }
};

// specialization for vexcl multivector
template <typename T, size_t N>
struct vector_space_norm_inf< vex::multivector<T, N> > {
    typedef T result_type;

    T operator()( const vex::multivector<T, N> &x ) const {
        auto max = detail::vexcl_reductor<T>(x.queue_list());

        // Reducing a multivector results in std::array<T, N>:
        auto m = max( fabs(x) );

        // We will need to reduce it even further:
        return *std::max_element(m.begin(), m.end());
    }
};

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_VEXCL_VEXCL_NORM_INF_HPP_DEFINED


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

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <boost/math/complex/atan.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/four.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/pio_2.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>
#include <boost/math/complex/atan.hpp>

NT2_TEST_CASE_TPL ( atan_real,  NT2_REAL_TYPES)
{
  using nt2::atan;
  using nt2::tag::atan_;
  typedef typename std::complex<T> cT;
  typedef typename nt2::meta::call<atan_(cT)>::type r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, cT);

  // specific values tests
#ifndef BOOST_SIMD_NO_INVALIDS
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Nan <T>(),  nt2::Zero<T>())) , boost::math::atan(cT(nt2::Nan <T>(),  nt2::Zero<T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Nan  <T>(), nt2::One<T>()))  , boost::math::atan(cT(nt2::Nan  <T>(), nt2::One<T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Nan  <T>(), nt2::Inf<T>()))  , boost::math::atan(cT(nt2::Nan  <T>(), nt2::Inf<T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Nan  <T>(), nt2::Nan<T>()))  , boost::math::atan(cT(nt2::Nan  <T>(), nt2::Nan<T>())) , 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Zero <T>(), nt2::Nan <T>())) , boost::math::atan(cT(nt2::Zero <T>(), nt2::Nan <T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::One  <T>(), nt2::Nan <T>())) , boost::math::atan(cT(nt2::One  <T>(), nt2::Nan <T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Inf  <T>(), nt2::Nan<T>()))  , boost::math::atan(cT(nt2::Inf  <T>(), nt2::Nan<T>())) , 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::One  <T>(), nt2::Inf <T>())) , boost::math::atan(cT(nt2::One  <T>(), nt2::Inf <T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Inf  <T>(), nt2::One <T>())) , boost::math::atan(cT(nt2::Inf  <T>(), nt2::One <T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Inf  <T>(), nt2::Inf<T>()))  , boost::math::atan(cT(nt2::Inf  <T>(), nt2::Inf<T>())) , 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::Nan  <T>(), nt2::One<T>())) , boost::math::atan(cT(-nt2::Nan  <T>(), nt2::One<T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::Nan  <T>(), nt2::Inf<T>())) , boost::math::atan(cT(-nt2::Nan  <T>(), nt2::Inf<T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::Nan  <T>(), nt2::Nan<T>())) , boost::math::atan(cT(-nt2::Nan  <T>(), nt2::Nan<T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::Zero <T>(), nt2::Nan <T>())), boost::math::atan(cT(-nt2::Zero <T>(), nt2::Nan <T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::One  <T>(), nt2::Nan <T>())), boost::math::atan(cT(-nt2::One  <T>(), nt2::Nan <T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::Inf  <T>(), nt2::Nan<T>())) , boost::math::atan(cT(-nt2::Inf  <T>(), nt2::Nan<T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::One  <T>(), nt2::Inf <T>())), boost::math::atan(cT(-nt2::One  <T>(), nt2::Inf <T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::Inf  <T>(), nt2::One <T>())), boost::math::atan(cT(-nt2::Inf  <T>(), nt2::One <T>())), 0.75);
 NT2_TEST_ULP_EQUAL(nt2::atan(cT(-nt2::Inf  <T>(), nt2::Inf<T>())) , boost::math::atan(cT(-nt2::Inf  <T>(), nt2::Inf<T>())), 0.75);
#endif
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Zero<T>(),  nt2::Zero<T>())), nt2::Zero<cT>(), 0);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::One  <T>(), nt2::One <T>())),   cT(1.0172219678978513535,0.40235947810852507045), 2);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::One  <T>(), nt2::Mone <T>())),  cT(1.0172219678978513535,-0.40235947810852507045), 2);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Mone  <T>(), nt2::One <T>())),  cT(-1.0172219678978513535,0.40235947810852507045), 2);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(nt2::Mone  <T>(), nt2::Mone <T>())), cT(-1.0172219678978513535,-0.40235947810852507045), 2);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(1, 1)),  boost::math::atan(cT(1, 1)), 1);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(1, 10)),  boost::math::atan(cT(1, 10)), 1);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(10, 10)),  boost::math::atan(cT(10, 10)), 1);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(10, 1)),  boost::math::atan(cT(10, 1)), 1);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(0, 1)),  boost::math::atan(cT(0, 1)), 1);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(1, 0)),  boost::math::atan(cT(1, 0)), 1);
  NT2_TEST_ULP_EQUAL(nt2::atan(cT(0, 0)),  boost::math::atan(cT(0, 0)), 1);
}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2018 Tiago de Paula Peixoto <tiago@skewed.de>
//
// 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 3
// 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/>.

#include "graph_filtering.hh"

#include <boost/python.hpp>

#include "graph.hh"
#include "graph_selectors.hh"
#include "graph_eigentrust.hh"

using namespace std;
using namespace graph_tool;

size_t eigentrust(GraphInterface& g, boost::any c, boost::any t,
                  double epslon, size_t max_iter)
{
    if (!belongs<writable_edge_scalar_properties>()(c))
        throw ValueException("edge property must be writable");
    if (!belongs<vertex_floating_properties>()(t))
        throw ValueException("vertex property must be of floating point"
                             " value type");

    size_t iter = 0;
    run_action<>()
        (g, bind(get_eigentrust(),
                 _1, g.get_vertex_index(), g.get_edge_index(), _2,
                 _3, epslon, max_iter, ref(iter)),
         writable_edge_scalar_properties(),
         vertex_floating_properties())(c,t);
    return iter;
}

void export_eigentrust()
{
    using namespace boost::python;
    def("get_eigentrust", &eigentrust);
}


// Copyright (c) 2020 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <pybind11/numpy.h>
#include <Eigen/Core>
#include "pyinterp/detail/broadcast.hpp"
#include "pyinterp/detail/geometry/box.hpp"
#include "pyinterp/detail/thread.hpp"
#include "pyinterp/geodetic/point.hpp"

namespace pyinterp::geodetic {

// Defines a box made of two describing points.
template <typename T>
class Box2D : public boost::geometry::model::box<Point2D<T>> {
 public:
  /// @brief Default constructor
  Box2D() = default;

  /// @brief Constructor taking the minimum corner point and the maximum corner
  /// point.
  /// @param min_corner the minimum corner point
  /// @param max_corner the maximum corner point
  Box2D(const Point2D<T>& min_corner, const Point2D<T>& max_corner)
      : boost::geometry::model::box<Point2D<T>>(min_corner, max_corner) {}

  /// @brief Get a box that covers the entire Earth. In other words, a box that
  /// covers all positions, whatever they may be.
  ///
  // @return a box that covers the entire Earth
  static auto entire_earth() -> Box2D { return Box2D({-180, -90}, {180, 90}); }

  /// @brief Test if the given point is inside or on border of this instance
  ///
  /// @param pt Point to test
  //  @return True if the given point is inside or on border of this Box
  auto covered_by(const Point2D<T>& pt) const -> bool {
    return boost::geometry::covered_by(pt, *this);
  }

  /// @brief Test if the coordinates of the points provided are located inside
  /// or at the edge of this box.
  ///
  /// @param lon Longitudes coordinates in degrees to check
  /// @param lat Latitude coordinates in degrees to check
  /// @return Returns a vector containing a flag equal to 1 if the coordinate is
  /// located in the box or at the edge otherwise 0.
  [[nodiscard]] auto covered_by(const Eigen::Ref<const Eigen::VectorXd>& lon,
                                const Eigen::Ref<const Eigen::VectorXd>& lat,
                                const size_t num_threads) const
      -> pybind11::array_t<int8_t> {
    detail::check_eigen_shape("lon", lon, "lat", lat);
    auto size = lon.size();
    auto result =
        pybind11::array_t<int8_t>(pybind11::array::ShapeContainer{{size}});
    auto _result = result.template mutable_unchecked<1>();

    {
      pybind11::gil_scoped_release release;

      // Captures the detected exceptions in the calculation function
      // (only the last exception captured is kept)
      auto except = std::exception_ptr(nullptr);

      detail::dispatch(
          [&](size_t start, size_t end) {
            try {
              for (size_t ix = start; ix < end; ++ix) {
                _result(ix) = static_cast<int8_t>(
                    covered_by(Point2D<T>(lon(ix), lat(ix))));
              }
            } catch (...) {
              except = std::current_exception();
            }
          },
          size, num_threads);

      if (except != nullptr) {
        std::rethrow_exception(except);
      }
    }
    return result;
  }

  /// Converts a Box2D into a string with the same meaning as that of this
  /// instance.
  [[nodiscard]] auto to_string() const -> std::string {
    std::stringstream ss;
    ss << boost::geometry::dsv(*this);
    return ss.str();
  }

  /// Get a tuple that fully encodes the state of this instance
  [[nodiscard]] auto getstate() const -> pybind11::tuple {
    return pybind11::make_tuple(this->min_corner().getstate(),
                                this->max_corner().getstate());
  }

  /// Create a new instance from a registered state of an instance of this
  /// object.
  static auto setstate(const pybind11::tuple& state) -> Box2D<T> {
    if (state.size() != 2) {
      throw std::runtime_error("invalid state");
    }
    return Box2D<T>(Point2D<T>::setstate(state[0].cast<pybind11::tuple>()),
                    Point2D<T>::setstate(state[1].cast<pybind11::tuple>()));
  }
};

}  // namespace pyinterp::geodetic

// BOOST specialization to accept pyinterp::geodectic::Box2D as a geometry
// entity
namespace boost::geometry::traits {

namespace pg = pyinterp::geodetic;

/// Box tag
template <typename T>
struct tag<pg::Box2D<T>> {
  using type = box_tag;
};

/// Type of a point
template <typename T>
struct point_type<pg::Box2D<T>> {
  using type = pg::Point2D<T>;
};

template <typename T, std::size_t Dimension>
struct indexed_access<pg::Box2D<T>, min_corner, Dimension> {
  /// get corner of box
  static inline auto get(pg::Box2D<T> const& box) -> double {
    return geometry::get<Dimension>(box.min_corner());
  }

  /// set corner of box
  static inline void set(pg::Box2D<T>& box,  // NOLINT
                         double const& value) {
    geometry::set<Dimension>(box.min_corner(), value);
  }
};

template <typename T, std::size_t Dimension>
struct indexed_access<pg::Box2D<T>, max_corner, Dimension> {
  /// get corner of box
  static inline auto get(pg::Box2D<T> const& box) -> double {
    return geometry::get<Dimension>(box.max_corner());
  }

  /// set corner of box
  static inline void set(pg::Box2D<T>& box,  // NOLINT
                         double const& value) {
    geometry::set<Dimension>(box.max_corner(), value);
  }
};

}  // namespace boost::geometry::traits


#define BOOST_TEST_MODULE systems
#include <boost/test/included/unit_test.hpp>
#include "systems/controller.hpp"
#include "enhance/output_utility.hpp"

BOOST_AUTO_TEST_SUITE(systems)

BOOST_AUTO_TEST_CASE(periodic_box)
{
    Box<PERIODIC::ON> periodic;
    periodic.setParameters(Parameters());
    periodic.setLengthX(10);
    periodic.setLengthY(10);
    periodic.setLengthZ(10);

    Eigen::Vector3f a (1,1,1);
    Eigen::Vector3f b (9,1,1);

    BOOST_CHECK_MESSAGE(periodic.contains(a), "contains a");
    BOOST_CHECK_MESSAGE(periodic.contains(b), "contains b");

    // the next two should be true, because Box::contains calls Box::scaleDown
    BOOST_CHECK_MESSAGE(periodic.contains(-a), "not contains -a");
    BOOST_CHECK_MESSAGE(periodic.contains(-b), "not contains -b");

    BOOST_CHECK_MESSAGE(periodic.distance(a,b)-2 < 1e-3, "distance");
}

BOOST_AUTO_TEST_CASE(normal_box)
{
    Box<PERIODIC::OFF> normal;
    normal.setParameters(Parameters());
    normal.setLengthX(10);
    normal.setLengthY(10);
    normal.setLengthZ(10);

    Eigen::Vector3f a (1,1,1);
    Eigen::Vector3f b (9,1,1);

    BOOST_CHECK_MESSAGE(normal.contains(a), "contains a");
    BOOST_CHECK_MESSAGE(normal.contains(b), "contains b");
    BOOST_CHECK_MESSAGE(!normal.contains(-a), "not contains -a");
    BOOST_CHECK_MESSAGE(!normal.contains(-b), "not contains -b");
    BOOST_CHECK_MESSAGE(normal.distance(a,b)-8 < 1e-3, "distance");
}

BOOST_AUTO_TEST_CASE(system)
{
    System system;

    BOOST_CHECK_MESSAGE(system.getTime() < 1e-3, "time before");
    system.addTime(0.5);
    BOOST_CHECK_MESSAGE(system.getTime()-0.5 < 1e-3, "time after");
}

BOOST_AUTO_TEST_CASE(system_distribute_from_gro)
{
    const char* argv[3] = {nullptr,"--config","../../tests/test_config.ini"};

    System system;
    {
        Parameters prms;
        prms.read(3,argv);
        prms.setup();
        system.setParameters(prms);
    }

    system.addParticles(ParticleFactory<ParticleMobile>(system.getParameters().mobile));
    TrajectoryDistributorGro dist;
    dist.setParameters(system.getParameters());
    dist(&system.getParticles());

    BOOST_CHECK_MESSAGE( system.getParticles().size() == 2, "number of particles "+std::to_string(system.getParticles().size()) );

    {
        const std::unique_ptr<Particle>& particle = system.getParticles().at(0);
        vesDEBUG("position " << particle->coords().format(ROWFORMAT))
        BOOST_CHECK_MESSAGE( particle->coords().isApprox(Eigen::Vector3f(1,1,1),1e-3), "position is "+enhance::streamBindableToString(particle->coords().format(ROWFORMAT)) );
        BOOST_CHECK_MESSAGE( particle->orientation().isApprox(Eigen::Vector3f(1,1,1).normalized(),1e-3), "orientation is "+enhance::streamBindableToString(particle->orientation().format(ROWFORMAT)) );
    }

    {
        const std::unique_ptr<Particle>& particle = system.getParticles().at(1);
        vesDEBUG("position " << particle->coords().format(ROWFORMAT))
        BOOST_CHECK_MESSAGE( particle->coords().isApprox(Eigen::Vector3f(9,9,9),1e-3), "position is "+enhance::streamBindableToString(particle->coords().format(ROWFORMAT)) );
        BOOST_CHECK_MESSAGE( particle->orientation().isApprox(Eigen::Vector3f(-1,-1,-1).normalized(),1e-3), "orientation is "+enhance::streamBindableToString(particle->orientation().format(ROWFORMAT)) );
    }
}

BOOST_AUTO_TEST_SUITE_END()