/*
Copyright 2020 The OneFlow Authors. All rights reserved.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/
#include <gtest/gtest.h>
#include "oneflow/core/ep/test/primitive/primitive_test.h"
#include "oneflow/core/ep/include/primitive/memcpy.h"
#include "oneflow/core/ep/include/primitive/elementwise_unary.h"
#include <Eigen/Core>
namespace oneflow {

namespace ep {

namespace primitive {

namespace test {

template<typename Src, typename Dst>
struct ReluFunctor {
  Dst operator()(Src src) {
    if (src > zero_val) { return src; }
    return zero_val;
  }

  Src zero_val = static_cast<Src>(0.0);
};

template<typename Src, typename Dst>
struct GeluFunctor {
  Dst operator()(Src src) {
    return static_cast<Dst>(0.5) * src * (static_cast<Src>(1.0) + std::erf(inv_sqrt2 * src));
  }
  Src inv_sqrt2 = std::sqrt(0.5);
};

template<typename Src, typename Dst>
struct TanhFunctor {
  Dst operator()(Src src) { return static_cast<Dst>(std::tanh(src)); }
};

template<typename Src, typename Dst>
struct LogicalNotFunctor {
  Dst operator()(Src src) { return static_cast<Dst>(!src); }
};

template<typename Src, typename Dst, typename FunctorT>
void EigenElementwise(FunctorT functor, Src* src, Dst* dst, const size_t elem_cnt) {
  for (int idx = 0; idx < elem_cnt; idx++) { dst[idx] = functor(src[idx]); }
}

template<typename Src, typename Dst, DataType SrcType, DataType DstType,
         ep::primitive::UnaryOp unary_op, template<typename A, typename B> class FunctorClass>
void TestElementwise(DeviceManagerRegistry* registry, const std::set<DeviceType>& device_types,
                     const size_t elem_cnt, Scalar attr0 = Scalar(), Scalar attr1 = Scalar()) {
  for (const auto& device_type : device_types) {
    auto device = registry->GetDevice(device_type, 0);
    using EigenSrcVec = Eigen::Matrix<Src, 1, Eigen::Dynamic>;
    using EigenDstVec = Eigen::Matrix<Dst, 1, Eigen::Dynamic>;

    const size_t src_data_size = elem_cnt * sizeof(Src);
    const size_t dst_data_size = elem_cnt * sizeof(Dst);
    EigenSrcVec eigen_src(elem_cnt);
    EigenDstVec eigen_dst(elem_cnt);
    eigen_src.setRandom();
    eigen_dst.setZero();

    ep::test::PinnedMemoryGuard host_src(device.get(), elem_cnt * sizeof(Src));
    ep::test::PinnedMemoryGuard host_dst(device.get(), elem_cnt * sizeof(Dst));
    ep::test::DeviceMemoryGuard device_src(device.get(), elem_cnt * sizeof(Src));
    ep::test::DeviceMemoryGuard device_dst(device.get(), elem_cnt * sizeof(Dst));

    ep::test::StreamGuard stream(device.get());
    std::unique_ptr<ElementwiseUnary> elementwise_primitive = NewPrimitive<ElementwiseUnaryFactory>(
        device_type, unary_op, /*src_type=*/SrcType, /*dst_type=*/DstType, attr0, attr1);
    ASSERT_TRUE(elementwise_primitive.operator bool());
    std::unique_ptr<Memcpy> h2d = NewPrimitive<MemcpyFactory>(device_type, MemcpyKind::kHtoD);
    std::unique_ptr<Memcpy> d2h = NewPrimitive<MemcpyFactory>(device_type, MemcpyKind::kDtoH);
    ASSERT_TRUE(d2h.operator bool());
    ASSERT_TRUE(h2d.operator bool());
    Src* eigen_src_data = eigen_src.data();
    std::memcpy(host_src.ptr(), eigen_src_data, src_data_size);
    h2d->Launch(stream.stream(), device_src.ptr<Src>(), host_src.ptr<Src>(), src_data_size);
    elementwise_primitive->Launch(stream.stream(), device_src.ptr<Dst>(), device_dst.ptr<Dst>(),
                                  elem_cnt);
    d2h->Launch(stream.stream(), host_dst.ptr<Dst>(), device_dst.ptr<Dst>(), dst_data_size);
    CHECK_JUST(stream.stream()->Sync());

    FunctorClass<Src, Dst> functor{};
    EigenElementwise<Src, Dst, FunctorClass<Src, Dst>>(functor, eigen_src.data(), eigen_dst.data(),
                                                       elem_cnt);
    auto elementwise_primitive_res =
        Eigen::Map<EigenDstVec, Eigen::Unaligned>(host_dst.ptr<Dst>(), elem_cnt);
    ASSERT_TRUE(eigen_dst.template isApprox(elementwise_primitive_res));
  }
}

TEST_F(PrimitiveTest, TestElementwisePrimitive) {
  // Test Relu
  TestElementwise<float, float, DataType::kFloat, DataType::kFloat, ep::primitive::UnaryOp::kRelu,
                  ReluFunctor>(&device_manager_registry_, available_device_types_, 16);
  TestElementwise<double, double, DataType::kDouble, DataType::kDouble,
                  ep::primitive::UnaryOp::kRelu, ReluFunctor>(&device_manager_registry_,
                                                              available_device_types_, 32);
  TestElementwise<int32_t, int32_t, DataType::kInt32, DataType::kInt32,
                  ep::primitive::UnaryOp::kRelu, ReluFunctor>(&device_manager_registry_,
                                                              available_device_types_, 64);
  TestElementwise<int64_t, int64_t, DataType::kInt64, DataType::kInt64,
                  ep::primitive::UnaryOp::kRelu, ReluFunctor>(&device_manager_registry_,
                                                              available_device_types_, 128);

  // Test Gelu
  TestElementwise<float, float, DataType::kFloat, DataType::kFloat, ep::primitive::UnaryOp::kGelu,
                  GeluFunctor>(&device_manager_registry_, available_device_types_, 32);
  TestElementwise<double, double, DataType::kDouble, DataType::kDouble,
                  ep::primitive::UnaryOp::kGelu, GeluFunctor>(&device_manager_registry_,
                                                              available_device_types_, 128);

  // Test Tanh
  TestElementwise<float, float, DataType::kFloat, DataType::kFloat, ep::primitive::UnaryOp::kTanh,
                  TanhFunctor>(&device_manager_registry_, available_device_types_, 32);
  TestElementwise<double, double, DataType::kDouble, DataType::kDouble,
                  ep::primitive::UnaryOp::kTanh, TanhFunctor>(&device_manager_registry_,
                                                              available_device_types_, 128);

  // Test Logical Not
  TestElementwise<float, bool, DataType::kFloat, DataType::kBool,
                  ep::primitive::UnaryOp::kLogicalNot, LogicalNotFunctor>(
      &device_manager_registry_, available_device_types_, 32);
  TestElementwise<double, bool, DataType::kDouble, DataType::kBool,
                  ep::primitive::UnaryOp::kLogicalNot, LogicalNotFunctor>(
      &device_manager_registry_, available_device_types_, 64);
  TestElementwise<int8_t, bool, DataType::kInt8, DataType::kBool,
                  ep::primitive::UnaryOp::kLogicalNot, LogicalNotFunctor>(
      &device_manager_registry_, available_device_types_, 16);
  TestElementwise<int32_t, bool, DataType::kInt32, DataType::kBool,
                  ep::primitive::UnaryOp::kLogicalNot, LogicalNotFunctor>(
      &device_manager_registry_, available_device_types_, 128);
  TestElementwise<int64_t, bool, DataType::kInt64, DataType::kBool,
                  ep::primitive::UnaryOp::kLogicalNot, LogicalNotFunctor>(
      &device_manager_registry_, available_device_types_, 96);
}

}  // namespace test

}  // namespace primitive

}  // namespace ep

}  // namespace oneflow


/**
 * @author Alessandro Bianco
 */

/**
 * Unit tests for the DFN Registration3D/IcpMatcher
 */

/**
 * @addtogroup DFNsTest
 * @{
 */

#include <catch.hpp>
#include <Registration3D/IcpMatcher.hpp>
#include <Converters/PclPointCloudToPointCloudConverter.hpp>
#include <Errors/Assert.hpp>

#include <boost/make_shared.hpp>

using namespace CDFF::DFN::Registration3D;
using namespace Converters;
using namespace PointCloudWrapper;
using namespace PoseWrapper;

#define REQUIRE_CLOSE(a,b) REQUIRE(a < b + 0.001); REQUIRE(a > b - 0.001);

TEST_CASE( "Call to process (Registration 3D Icp Matcher)", "[process]" )
{
	PRINT_TO_LOG("Running Icp matcher", "");

	// Prepare input data (a sphere)
	pcl::PointCloud<pcl::PointXYZ>::Ptr inputCloud = boost::make_shared<pcl::PointCloud<pcl::PointXYZ> >();
	for (float alpha = 0; alpha < 2 * M_PI; alpha += 0.1)
	{
		for (float beta = 0; beta < 2 * M_PI; beta += 0.1)
		{
			pcl::PointXYZ spherePoint;
			spherePoint.x = std::cos(alpha) * std::cos(beta);
			spherePoint.y = std::sin(alpha);
			spherePoint.z = std::sin(beta);
			inputCloud->points.push_back(spherePoint);
		}
	}
	PclPointCloudToPointCloudConverter pclPointCloudToPointCloud;
	PointCloudConstPtr sourcePointCloud = pclPointCloudToPointCloud.Convert(inputCloud);
	PointCloudConstPtr sinkPointCloud = pclPointCloudToPointCloud.Convert(inputCloud);

	// Instantiate DFN
	IcpMatcher *icp = new IcpMatcher;

	// Setup DFN
	icp->setConfigurationFile("../tests/ConfigurationFiles/DFNs/Registration3D/IcpMatcher_Conf1.yaml");
	icp->configure();

	// Send input data to DFN
	icp->sourceCloudInput(*sourcePointCloud);
	icp->sinkCloudInput(*sinkPointCloud);

	PRINT_TO_LOG("Running Icp matcher", "");
	// Run DFN
	icp->process();

	// Query output data from DFN
	const Pose3D& transform = icp->transformOutput();
	bool success = icp->successOutput();

	REQUIRE_CLOSE( GetXPosition(transform), 0);
	REQUIRE_CLOSE( GetYPosition(transform), 0);
	REQUIRE_CLOSE( GetZPosition(transform), 0);
	REQUIRE_CLOSE( GetXOrientation(transform), 0);
	REQUIRE_CLOSE( GetYOrientation(transform), 0);
	REQUIRE_CLOSE( GetZOrientation(transform), 0);
	REQUIRE_CLOSE( GetWOrientation(transform), 1);

	// Cleanup
	delete icp;
	delete sourcePointCloud;
	delete sinkPointCloud;
}

TEST_CASE( "Call to configure (Registration 3D Icp Matcher)", "[configure]" )
{
	// Instantiate DFN
	IcpMatcher *icp = new IcpMatcher;

	// Setup DFN
	icp->setConfigurationFile("../tests/ConfigurationFiles/DFNs/Registration3D/IcpMatcher_Conf1.yaml");
	icp->configure();

	// Cleanup
	delete icp;
}

/** @} */


#include <iostream>
#include <armadillo>
#include "Plague.h"
#include "Plague.cpp"

using namespace std;
using namespace arma;

int main() {

	vector<double> vec{1,2,3,4};
	dmat mat = BuildDistribMatCheat(vec);

	cout << mat;

     return 0;
}


#include <sparse.h>

#include <sparse_fill.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(SPARSE);

BOOST_AUTO_TEST_CASE(transpose_block_test)
{

  // scalar case
  typedef sparse::Block<1,1,float> scalar_block_type;
  scalar_block_type s1(5);
  scalar_block_type s2(0);
  sparse::transpose(s1,s2);
  BOOST_CHECK(s2[0] == 5);

  // block case
  typedef sparse::Block<4,3,float> block_type;
  typedef sparse::Block<3,4,float> transposed_block_type;

  block_type a;
  sparse::fill(a);
  transposed_block_type b;
  sparse::transpose(a,b);
  for (size_t i = 0; i < block_type::nrows(); ++i)
  {
    for (size_t j = 0; j < block_type::ncols(); ++j)
    {
      BOOST_CHECK(a(i,j) == b(j,i));
    }
  }

}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/*!

  Copyright 2015 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/floor.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/pi.hpp>
#include <boost/simd/constant/three.hpp>

STF_CASE_TPL (" floor real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;

  using bs::floor;
  using r_t = decltype(floor(T()));

  STF_TYPE_IS(r_t, T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(floor(bs::Inf<T>()), bs::Inf<T>());
  STF_EQUAL(floor(bs::Minf<T>()), bs::Minf<T>());
  STF_IEEE_EQUAL(floor(bs::Nan<T>()), bs::Nan<T>());
#endif
  STF_EQUAL(floor(bs::One<T>()), bs::One<T>());
  STF_EQUAL(floor(bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(floor(bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(floor(bs::Pi<T>()), bs::Three<T>());
  STF_EQUAL(floor(T(-1.1)), r_t(-2));
  STF_EQUAL(floor(T(1.1)), r_t(1));
} // end of test for floating_

STF_CASE_TPL (" floor unsigned_int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::floor;

  // return type conformity test
  STF_EXPR_IS(floor(T()), T);

  // specific values tests
  STF_EQUAL(floor(bs::One<T>()), bs::One<T>());
  STF_EQUAL(floor(bs::Zero<T>()), bs::Zero<T>());
} // end of test for unsigned_int_

STF_CASE_TPL (" floor signed_int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;

  using bs::floor;

  // return type conformity test
   STF_EXPR_IS(floor(T()), T);

  // specific values tests
  STF_EQUAL(floor(bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(floor(bs::One<T>()), bs::One<T>());
  STF_EQUAL(floor(bs::Zero<T>()), bs::Zero<T>());
} // end of test for signed_int_


#ifdef STAN_OPENCL

#include <stan/math/opencl/kernel_generator.hpp>
#include <stan/math/opencl/matrix_cl.hpp>
#include <stan/math/opencl/copy.hpp>
#include <stan/math/prim/fun.hpp>
#include <Eigen/Dense>
#include <gtest/gtest.h>

#define EXPECT_MATRIX_NEAR(A, B, DELTA) \
  for (int i = 0; i < A.size(); i++)    \
    EXPECT_NEAR(A(i), B(i), DELTA);

TEST(KernelGenerator, append_row_errors) {
  using Eigen::MatrixXd;
  using stan::math::append_row;
  using stan::math::matrix_cl;

  MatrixXd m1(1, 1);
  m1 << 1;
  MatrixXd rv(1, 3);
  rv << 1, 2.5, 3;
  MatrixXd m2(2, 2);
  m2 << 1, 2.5, 3, 4;
  MatrixXd m3(3, 3);
  m3 << 1, 2.5, 3, 4, 5, 6.3, 7, -8, -9.5;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);
  matrix_cl<double> m3_cl(m3);
  matrix_cl<double> rv_cl(rv);

  EXPECT_NO_THROW(append_row(m3_cl, rv_cl));
  EXPECT_NO_THROW(append_row(m3_cl, stan::math::rowwise_broadcast(m1_cl)));
  EXPECT_NO_THROW(append_row(stan::math::rowwise_broadcast(m1_cl), m3_cl));
  EXPECT_THROW(append_row(m3_cl, stan::math::colwise_broadcast(rv_cl)),
               std::invalid_argument);
  EXPECT_THROW(append_row(stan::math::colwise_broadcast(rv_cl), m3_cl),
               std::invalid_argument);
  EXPECT_THROW(append_row(m2_cl, m3_cl), std::invalid_argument);
}

TEST(KernelGenerator, append_row_test) {
  using Eigen::MatrixXd;
  using stan::math::append_row;
  using stan::math::matrix_cl;
  MatrixXd m1(2, 3);
  m1 << 1, 2.5, 3, 4, 5, 6.3;
  MatrixXd m2(1, 3);
  m2 << 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  matrix_cl<double> res_cl = append_row(m1_cl, m2_cl);
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = append_row(m1, m2);
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, append_row_multiple_operations_test) {
  using Eigen::MatrixXd;
  using stan::math::append_row;
  using stan::math::matrix_cl;
  MatrixXd m1(2, 3);
  m1 << 1, 2.5, 3, 4, 5, 6.3;
  MatrixXd m2(1, 3);
  m2 << 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  matrix_cl<double> res_cl
      = append_row(append_row(m1_cl, m1_cl), append_row(m2_cl, m2_cl));
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = append_row(append_row(m1, m1), append_row(m2, m2));
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, append_row_multiple_operations_accept_lvalue_test) {
  using Eigen::MatrixXd;
  using stan::math::append_row;
  using stan::math::matrix_cl;
  MatrixXd m1(2, 3);
  m1 << 1, 2.5, 3, 4, 5, 6.3;
  MatrixXd m2(1, 3);
  m2 << 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  auto a = append_row(m1_cl, m1_cl);
  auto b = append_row(m2_cl, m2_cl);
  matrix_cl<double> res_cl = append_row(a, b);
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = append_row(append_row(m1, m1), append_row(m2, m2));
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, append_col_errors) {
  using Eigen::MatrixXd;
  using stan::math::append_col;
  using stan::math::matrix_cl;

  MatrixXd m1(1, 1);
  m1 << 1;
  MatrixXd v(3, 1);
  v << 1, 2.5, 3;
  MatrixXd m2(2, 2);
  m2 << 1, 2.5, 3, 4;
  MatrixXd m3(3, 3);
  m3 << 1, 2.5, 3, 4, 5, 6.3, 7, -8, -9.5;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);
  matrix_cl<double> m3_cl(m3);
  matrix_cl<double> v_cl(v);

  EXPECT_NO_THROW(append_col(m3_cl, v_cl));
  EXPECT_NO_THROW(append_col(m3_cl, stan::math::colwise_broadcast(m1_cl)));
  EXPECT_NO_THROW(append_col(stan::math::colwise_broadcast(m1_cl), m3_cl));
  EXPECT_THROW(append_col(m3_cl, stan::math::rowwise_broadcast(v_cl)),
               std::invalid_argument);
  EXPECT_THROW(append_col(stan::math::rowwise_broadcast(v_cl), m3_cl),
               std::invalid_argument);
  EXPECT_THROW(append_col(m2_cl, m3_cl), std::invalid_argument);
}

TEST(KernelGenerator, append_col_test) {
  using Eigen::MatrixXd;
  using stan::math::append_col;
  using stan::math::matrix_cl;
  MatrixXd m1(3, 2);
  m1 << 1, 2.5, 3, 4, 5, 6.3;
  MatrixXd m2(3, 1);
  m2 << 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  matrix_cl<double> res_cl = append_col(m1_cl, m2_cl);
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = append_col(m1, m2);
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, append_col_multiple_operations_test) {
  using Eigen::MatrixXd;
  using stan::math::append_col;
  using stan::math::matrix_cl;
  MatrixXd m1(3, 2);
  m1 << 1, 2.5, 3, 4, 5, 6.3;
  MatrixXd m2(3, 1);
  m2 << 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  matrix_cl<double> res_cl
      = append_col(append_col(m1_cl, m1_cl), append_col(m2_cl, m2_cl));
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = append_col(append_col(m1, m1), append_col(m2, m2));
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

TEST(KernelGenerator, append_col_multiple_operations_accept_lvalue_test) {
  using Eigen::MatrixXd;
  using stan::math::append_col;
  using stan::math::matrix_cl;
  MatrixXd m1(3, 2);
  m1 << 1, 2.5, 3, 4, 5, 6.3;
  MatrixXd m2(3, 1);
  m2 << 2, 4, 8;

  matrix_cl<double> m1_cl(m1);
  matrix_cl<double> m2_cl(m2);

  auto a = append_col(m1_cl, m1_cl);
  auto b = append_col(m2_cl, m2_cl);
  matrix_cl<double> res_cl = append_col(a, b);
  MatrixXd res = stan::math::from_matrix_cl(res_cl);

  MatrixXd correct = append_col(append_col(m1, m1), append_col(m2, m2));
  EXPECT_MATRIX_NEAR(res, correct, 1e-9);
}

#endif


#include <fstream>
#include <sstream>
#include <iostream>

#include "read.h"
#include "write.h"
#include "util.h"
#include "matrixgen.h"
#include "mathfns.h"

#include <iomanip>

#ifdef ARMA
#include <armadillo>
#endif // ARMA

void writeSamples(char const* sample_name, char const* trial_name, int vDim, std::vector<SAMPLE_DIMS> sample_dims, sample_fn fn) {

	if (!manage_folder_structure(SAMPLE_PATH, sample_name, trial_name))
		return;

	std::ofstream myfile;
	std::string post_file_name1, post_file_name2, file_name1, file_name2;

	char* file_extension1 = "1.csv", * file_extension2 = "2.csv";
	post_file_name1 = create_file_name(sample_name, trial_name, file_extension1);
	post_file_name2 = create_file_name(sample_name, trial_name, file_extension2);

	int thread_ID = 0;

	for (int s = 0; s < sample_dims.size(); s++) {

		std::stringstream ss1, ss2;
		ss1 << s + 1 << post_file_name1;
		file_name1 = ss1.str();
		ss2 << s + 1 << post_file_name2;
		file_name2 = ss2.str();

#ifdef MKL
		std::unique_ptr<double[]> ns1 = std::make_unique<double[]>(sample_dims[s].m1 * vDim);
		std::unique_ptr<double[]> ns2 = std::make_unique<double[]>(sample_dims[s].m2 * vDim);

		nRandomVectorsCreateMKL(sample_dims[s].m1, vDim, ns1.get(), fn, thread_ID);
		nRandomVectorsCreateMKL(sample_dims[s].m2, vDim, ns2.get(), fn, thread_ID);
#endif // MKL
#ifdef ARMA
		dmat ns1 = nRandomVectorsCreate(nsamples_ vDim, unifSampleFromDSphere);
		dmat ns2 = nRandomVectorsCreate(nsamples_, vDim, unifSampleFromDSphere);
#endif // ARMA

		int prec = std::numeric_limits<double>::digits10 + 2;
		/* Write First Dataset */
		if (!myfile.is_open()) {
			myfile.open(file_name1);
		}
		std::cout << "Start writing " << file_name1 << std::endl;

		for (int i = 0; i < sample_dims[s].m1; i++)
		{
			for (int j = 0; j < vDim; j++)
			{
				if (j < vDim - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns1.get()[i * vDim + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns1(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns1.get()[i * vDim + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns1(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << file_name1 << std::endl;

		/* Write Second Dataset*/
		if (!myfile.is_open()) {
			myfile.open(file_name2);
		}
		std::cout << "Start writing " << file_name1 << std::endl;

		for (int i = 0; i < sample_dims[s].m2; i++)
		{
			for (int j = 0; j < vDim; j++)
			{
				if (j < vDim - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns2.get()[i * vDim + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns2(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns2.get()[i * vDim + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns2(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << file_name2 << "\n" << std::endl;
	}
}

void writeSamples(char const* sample_name, char const* trial_name, int vDim, std::vector<SAMPLE_DIMS> sample_dims, sample_fn fn, double scale) {

	if (!manage_folder_structure(SAMPLE_PATH, sample_name, trial_name))
		return;

	std::ofstream myfile;
	std::string post_file_name1, post_file_name2, file_name1, file_name2;

	char* file_extension1 = "1.csv", * file_extension2 = "2.csv";
	post_file_name1 = create_file_name(sample_name, trial_name, file_extension1);
	post_file_name2 = create_file_name(sample_name, trial_name, file_extension2);

	int thread_ID = 0;

	for (int s = 0; s < sample_dims.size(); s++) {

		std::stringstream ss1, ss2;
		ss1 << s + 1 << post_file_name1;
		file_name1 = ss1.str();
		ss2 << s + 1 << post_file_name2;
		file_name2 = ss2.str();

#ifdef MKL
		std::unique_ptr<double[]> ns1 = std::make_unique<double[]>(sample_dims[s].m1 * vDim);
		std::unique_ptr<double[]> ns2 = std::make_unique<double[]>(sample_dims[s].m2 * vDim);

		nRandomVectorsCreateMKL(sample_dims[s].m1, vDim, ns1.get(), fn, thread_ID);
		nRandomVectorsCreateMKL(sample_dims[s].m2, vDim, ns2.get(), fn, thread_ID);
#endif // MKL
#ifdef ARMA
		dmat ns1 = nRandomVectorsCreate(nsamples_ vDim, unifSampleFromDSphere);
		dmat ns2 = nRandomVectorsCreate(nsamples_, vDim, unifSampleFromDSphere);
#endif // ARMA

		int prec = std::numeric_limits<double>::digits10 + 2;
		/* Write First Dataset */
		if (!myfile.is_open()) {
			myfile.open(file_name1);
		}
		std::cout << "Start writing " << file_name1 << std::endl;

		for (int i = 0; i < sample_dims[s].m1; i++)
		{
			for (int j = 0; j < vDim; j++)
			{
				if (j < vDim - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns1.get()[i * vDim + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns1(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns1.get()[i * vDim + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns1(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << file_name1 << std::endl;

		/* Write Second Dataset*/
		if (!myfile.is_open()) {
			myfile.open(file_name2);
		}
		std::cout << "Start writing " << file_name1 << std::endl;

		for (int i = 0; i < sample_dims[s].m2; i++)
		{
			for (int j = 0; j < vDim; j++)
			{
				if (j < vDim - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns2.get()[i * vDim + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns2(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << ns2.get()[i * vDim + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << ns2(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << file_name2 << "\n" << std::endl;
	}
}

void writeNLVMs(char const* nlvm_name, char const* Exn, char const* sample_name, char const* trial_name, std::vector<NLVM_DIMS> nlvm_dims,
	nlvmf fn) {

	std::ofstream myfile;
	std::string post_rfile_name1, rfile_name1, post_rfile_name2, rfile_name2, post_wfile_name, wfile_name;

	char* rfile_extension1 = "1.csv", * rfile_extension2 = "2.csv", * wfile_extension = ".csv";

	int thread_ID = 0;

	for (int s = 0; s < nlvm_dims.size(); s++) {

		post_rfile_name1 = create_file_name(sample_name, trial_name, rfile_extension1);
		post_rfile_name2 = create_file_name(sample_name, trial_name, rfile_extension2);

		std::stringstream ss1, ss2;
		ss1 << s + 1 << post_rfile_name1;
		rfile_name1 = ss1.str();
		ss2 << s + 1 << post_rfile_name2;
		rfile_name2 = ss2.str();

#ifdef MKL
		std::unique_ptr<double[]> nlvm_mat = std::make_unique<double[]>(nlvm_dims[s].m * nlvm_dims[s].n);
#ifdef EXTERN
		nlvm(nlvm_dims[s].m, nlvm_dims[s].n, nlvm_mat.get(), fn, sample_name, trial_name, rfile_name1.c_str(), rfile_name2.c_str(), thread_ID);
#endif // EXTERN
#endif // MKL

#ifdef ARMA
#endif // ARMA

		post_wfile_name = create_file_name(Exn, nlvm_name, sample_name, trial_name, wfile_extension);

		std::stringstream ss;
		ss << s + 1 << post_wfile_name;
		wfile_name = ss.str();

		int prec = std::numeric_limits<double>::digits10 + 2;
		if (!manage_folder_structure(EXAMPLE_PATH, Exn, trial_name))
			return;

		/* Write NLVM Matrix */
		if (!myfile.is_open()) {
			myfile.open(wfile_name);
		}
		std::cout << "Start writing " << wfile_name << std::endl;

		for (int i = 0; i < nlvm_dims[s].m; i++)
		{
			for (int j = 0; j < nlvm_dims[s].n; j++)
			{
				if (j < nlvm_dims[s].n - 1) {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << nlvm_mat.get()[i * nlvm_dims[s].n + j] << ",";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << nlvm_mat(i, j) << ",";
#endif // ARMA
				}
				else {
#ifdef MKL
					myfile << std::fixed << std::setprecision(prec) << nlvm_mat.get()[i * nlvm_dims[s].n + j] << "\n";
#endif // MKL
#ifdef ARMA
					myfile << std::fixed << std::setprecision(prec) << nlvm_mat(i, j) << "\n";
#endif // ARMA
				}
			}
		}
		myfile.close();
		std::cout << "Finished writing " << wfile_name << std::endl;
	}
}

void writeResults(char const* nlvm_name, char const* Exn, char const* result_name, char const* sample_name, char const* trial_name,
	std::vector<NLVM_DIMS> nlvm_dims) {

	std::ofstream myfile;
	std::string rfile_name, post_rfile_name, wfile_name;

	char* file_extension = ".csv";

	for (int s = 0; s < nlvm_dims.size(); s++)
	{
		std::unique_ptr<double[]> NLVM = std::make_unique<double[]>(nlvm_dims[s].m * nlvm_dims[s].n);
		std::unique_ptr<double[]> NLVM_CMPR = std::make_unique<double[]>(nlvm_dims[s].m * nlvm_dims[s].n);

		post_rfile_name = create_file_name(Exn, nlvm_name, sample_name, trial_name, file_extension);

		std::stringstream ss;
		ss << s + 1 << post_rfile_name;
		rfile_name = ss.str();

		if (changeFolder(EX, sample_name, trial_name,Exn) < 0)
			return;
		if (!check_if_file_exists(rfile_name.c_str()))
			return;

		std::string dummy =  get_current_dir();

		int thread_ID = 0;
		std::cout << "Start reading " << rfile_name << std::endl;
		std::vector<std::vector<double>> nlvm_vecvec{ readCSV(rfile_name.c_str(),thread_ID) };
		dVecVecToCArr(nlvm_vecvec, NLVM.get(), thread_ID);
		nlvm_vecvec.swap(std::vector<std::vector<double>>()); // ?
		std::cout << "Reading complete" << std::endl;

		//dmat armaX0 = vecVecToArmaMat(X);

		if (s < 1)
		{
			wfile_name = create_file_name(Exn, nlvm_name, result_name, sample_name, trial_name, file_extension);

			if (!manage_folder_structure(RESULTS_PATH, Exn, sample_name))
				return;
			if (check_if_file_exists(wfile_name.c_str()))
				return;
		}

		std::cout << "Start analyzing and writing " << std::endl;
		if (!myfile.is_open()) {
			myfile.open(wfile_name);
			myfile << "nrows,ncols,eps,epsRank,flag\n";
		}

		unsigned int epsRank;
		int no_conv_flag = 0;
		double tol = 0.001;
		std::vector<float> epsVec = { 0.2, 0.1, 0.07, 0.03, 0.01, 0.001 }; //SED
		epsVec = { 0.1, 0.07, 0.03, 0.01, 0.005, 0.001 }; //LINEARK
		epsVec = { 0.03, 0.01, 0.007, 0.005,0.0025, 0.001 }; //POLYNOMIALK d=2
		epsVec = { 0.25, 0.15, 0.05,0.1, 0.01,0.001 }; //COSINEK
		epsVec = { 0.1, 0.06, 0.03, 0.02, 0.01, 0.005}; ; //POLYNOMIALK d=3
		epsVec = {150000,100000,50000,25000, 10000, 1000}; //CHISQRK
		epsVec = { 2, 1, 0.5, 0.3, 0.17, 0.03 }; //L1LOSSMKL
		//epsVec = { 1.3, 1.0, 0.7, 0.4, 0.2, 0.05 }; // 01LOSSMKL

		for (auto eps : epsVec)
		{
			std::memcpy(NLVM_CMPR.get(), NLVM.get(), nlvm_dims[s].m * nlvm_dims[s].n * sizeof(double));
			epsRank = getEpsRank(nlvm_dims[s].m, nlvm_dims[s].n, NLVM_CMPR.get(), eps, tol, no_conv_flag);
			myfile << nlvm_dims[s].m << "," << nlvm_dims[s].n << "," << eps << "," << epsRank << "," << no_conv_flag << "\n";
		}
	}
	myfile.close();
}


#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <tuple>
#include <vector>

#include <Eigen/Core>
#include <pcl/common/angles.h>
#include <pcl/common/distances.h>
#include <pcl/filters/random_sample.h>
#include <pcl/filters/uniform_sampling.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <vector_types.h>

#include "impl/cycle_iterator.hpp"
#include "impl/scene_generation.hpp"
#include "impl/util.hpp"
#include "kernel.h"
#include "linalg.h"
#include "ppf.h"
#include "vector_ops.h"

#include <pybind11/eigen.h>
#include <pybind11/pybind11.h>
#include <pybind11/pytypes.h>
#include <pybind11/stl.h>

namespace py = pybind11;

template <typename Point>
typename pcl::PointCloud<Point>::Ptr loadPLYFile(const std::string& file_name)
{
  typename pcl::PointCloud<Point>::Ptr cloud(new pcl::PointCloud<Point>);
  pcl::io::loadPLYFile<Point>(file_name, *cloud);
  return cloud;
}

template <typename Point>
typename pcl::PointCloud<Point>::Ptr randomDownsample(typename pcl::PointCloud<Point>::Ptr cloud, float p)
{
  pcl::RandomSample<Point> rs;
  typename pcl::PointCloud<Point>::Ptr filtered_cloud(new pcl::PointCloud<Point>);
  rs.setSample(p * cloud->size());
  rs.setInputCloud(cloud);
  rs.filter(*filtered_cloud);
  return filtered_cloud;
}
template <typename Point>
typename pcl::PointCloud<Point>::Ptr sequentialDownsample(typename pcl::PointCloud<Point>::Ptr cloud, int n)
{
  typename pcl::PointCloud<Point>::Ptr filtered_cloud(new pcl::PointCloud<Point>);
  int i = 0;
  for (typename pcl::PointCloud<Point>::iterator it = cloud->begin(); it != cloud->end(); ++it, ++i)
  {
    if (i % n == 0)
    {
      filtered_cloud->push_back(*it);
    }
  }
  return filtered_cloud;
}

template <typename Point>
typename pcl::PointCloud<Point>::Ptr voxelGridDownsample(typename pcl::PointCloud<Point>::Ptr cloud, float leaf)
{
  typename pcl::PointCloud<Point>::Ptr filtered_cloud(new pcl::PointCloud<Point>);
  pcl::VoxelGrid<Point> sor;
  sor.setInputCloud(cloud);
  sor.setLeafSize(leaf, leaf, leaf);
  sor.filter(*filtered_cloud);
  return filtered_cloud;
}

namespace ppf_registration_spatial_hashing
{
using Point = pcl::PointNormal;
using PointCloud = pcl::PointCloud<pcl::PointNormal>;
}  // namespace ppf_registration_spatial_hashing

namespace ppf = ppf_registration_spatial_hashing;

PYBIND11_DECLARE_HOLDER_TYPE(T, boost::shared_ptr<T>);

PYBIND11_MODULE(ppf_registration_spatial_hashing, m)
{
  py::class_<ppf::Point>(m, "Point")
      .def_readonly("x", &ppf::Point::x)
      .def_readonly("y", &ppf::Point::y)
      .def_readonly("z", &ppf::Point::z)
      .def("__repr__", [](const ppf::Point& self) {
        std::stringstream ss;
        ss << self;
        return ss.str();
      });

  auto point_cloud = py::class_<ppf::PointCloud, ppf::PointCloud::Ptr>(m, "PointCloud")
                         .def_readonly("width", &ppf::PointCloud::width)
                         .def_readonly("height", &ppf::PointCloud::height)
                         .def_readonly("points", &ppf::PointCloud::points)
                         .def_readonly("sensor_origin", &ppf::PointCloud::sensor_origin_)
                         .def_readonly("sensor_orientation", &ppf::PointCloud::sensor_orientation_)
                         .def("array", [](const ppf::PointCloud& self) { return self.getMatrixXfMap(); });

  m.def("load_ply_file", &loadPLYFile<ppf::Point>);
  m.def("random_downsample", &randomDownsample<ppf::Point>);
  m.def("sequential_downsample", &sequentialDownsample<ppf::Point>);
  m.def("voxelgrid_downsample", &voxelGridDownsample<ppf::Point>);

  m.def("min_max_3d", [](const ppf::PointCloud& point_cloud) {
    pcl::PointNormal minPt, maxPt;
    // Finding the max pairwise distance is expensive, so
    // approximate it with the max difference between coords.
    pcl::getMinMax3D(point_cloud, minPt, maxPt);
    return std::make_tuple(minPt, maxPt);
  });

  m.def("transform_point_cloud", [](const ppf::PointCloud& point_cloud, const Eigen::Matrix4f& transform) {
    ppf::PointCloud transformed_point_cloud;
    pcl::transformPointCloudWithNormals(point_cloud, transformed_point_cloud, transform);
    return transformed_point_cloud;
  });

  m.def("icp", [](ppf::PointCloud::Ptr scene, ppf::PointCloud::Ptr model) {
    pcl::IterativeClosestPoint<ppf::Point, ppf::Point> icp;
    icp.setInputSource(model);
    icp.setInputTarget(scene);
    ppf::PointCloud final;
    icp.align(final);
    BOOST_LOG_TRIVIAL(info) << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore()
                            << std::endl;
    return icp.getFinalTransformation();
  });

  m.def("ppf_registration",
        [](pcl::PointCloud<pcl::PointNormal>::Ptr scene_cloud, pcl::PointCloud<pcl::PointNormal>::Ptr model_cloud,
           float model_d_dist, unsigned int ref_point_downsample_factor, float vote_count_threshold,
           bool cpu_clustering, bool use_l1_norm, bool use_averaged_clusters, int devUse) {
          Eigen::Matrix4f result;
          result = ppf_registration(std::vector<pcl::PointCloud<pcl::PointNormal>::Ptr>{ scene_cloud },
                                    std::vector<pcl::PointCloud<pcl::PointNormal>::Ptr>{ model_cloud },
                                    std::vector<float>{ model_d_dist }, ref_point_downsample_factor,
                                    vote_count_threshold, cpu_clustering, use_l1_norm, use_averaged_clusters, devUse);
          return result;
        },
        py::arg("scene_cloud"), py::arg("model_cloud"), py::arg("model_d_dist"), py::arg("ref_point_downsample_factor"),
        py::arg("vote_count_threshold"), py::arg("cpu_clustering") = false, py::arg("use_l1_norm") = false,
        py::arg("use_averaged_clusters") = false, py::arg("device") = 0, py::return_value_policy::copy);
}


//  Copyright 2013 Eric Niebler. 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)
//
// This is an example of how to specify an action externally so
// that a single grammar can be used to drive multiple differnt
// calculations. In particular, it defines a calculator grammar
// that computes the result of an expression with either checked
// or non-checked division.

#include <iostream>
#include <boost/mpl/int.hpp>
#include <boost/proto/v5/proto.hpp>
#include "./unit_test.hpp"

namespace mpl = boost::mpl;
namespace proto = boost::proto;
namespace fusion = boost::fusion;
using proto::_;

// The argument placeholder type
template<typename I>
struct placeholder
  : I, proto::env_tag<placeholder<I>>
{
    using proto::env_tag<placeholder<I>>::operator=;
};

// Give each rule in the grammar a "name". This is so that we
// can easily dispatch on it later.
struct calc_grammar;
struct divides_rule
  : proto::def<proto::divides(calc_grammar, calc_grammar)>
  , proto::env_tag<divides_rule>
{
    using proto::env_tag<divides_rule>::operator=;
};

// Use external transforms in calc_gramar
struct calc_grammar
  : proto::def<
        proto::match(
            proto::case_(
                proto::terminal(placeholder<_>)
              , proto::get_env(proto::_value)
            )
          , proto::case_(
                proto::terminal(proto::convertible_to<double>)
              , proto::_value
            )
          , proto::case_(
                proto::plus(calc_grammar, calc_grammar)
              , proto::eval_with(calc_grammar)
            )
          , proto::case_(
                proto::minus(calc_grammar, calc_grammar)
              , proto::eval_with(calc_grammar)
            )
          , proto::case_(
                proto::multiplies(calc_grammar, calc_grammar)
              , proto::eval_with(calc_grammar)
            )
            // Note that we don't specify how division nodes are
            // handled here. proto::external is a placeholder
            // for an actual action.
          , proto::case_(
                divides_rule
              , proto::external
            )
        )
    >
{};

template<typename ExprDesc>
struct calc_expr
  : proto::expr<calc_expr<ExprDesc>>
{
    using proto::expr<calc_expr<ExprDesc>>::expr;
};

constexpr calc_expr<proto::terminal(placeholder<mpl::int_<0>>)> _1 {};
constexpr calc_expr<proto::terminal(placeholder<mpl::int_<1>>)> _2 {};

struct non_checked_division
  : proto::def<proto::eval_with(calc_grammar)>
{};

struct division_by_zero
  : std::exception
{};

struct do_checked_divide
{
    int operator()(int left, int right) const
    {
        if (right == 0) throw division_by_zero();
        return left / right;
    }
};

// Use proto::external_transforms again, this time to map the divides_rule
// to a transforms that performs checked division.
struct checked_division
  : proto::def<do_checked_divide(calc_grammar(proto::_left), calc_grammar(proto::_right))>
{};

void test_external_actions()
{
    placeholder<mpl::int_<0>> _1_; // BUGBUG would be nice to just say _1 = 2;
    placeholder<mpl::int_<1>> _2_;
    int result1 = calc_grammar()(_1 / _2, (_1_ = 6, _2_ = 2, divides_rule() = non_checked_division()));
    BOOST_CHECK_EQUAL(result1, 3);

    try
    {
        // This should throw
        int result2 = calc_grammar()(_1 / _2, (_1_ = 6, _2_ = 0, divides_rule() = checked_division()));
        BOOST_PROTO_IGNORE_UNUSED(result2);
        BOOST_CHECK(!"Didn't throw an exception"); // shouldn't get here!
    }
    catch(division_by_zero)
    {
        ; // OK
    }
    catch(...)
    {
        BOOST_CHECK(!"Unexpected exception"); // shouldn't get here!
    }
}

using namespace boost::unit_test;
///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("test for external actions");

    test->add(BOOST_TEST_CASE(&test_external_actions));

    return test;
}


// Copyright 2016 by Glukhov V. O. All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#include "pooling_layer.hpp"
#include <armadillo>
#include <cmath>

namespace cnn
{
	namespace nn
	{
		void MaxPoolingLayer::Forward(std::shared_ptr<arma::Cube<double>> input)
		{
			using namespace arma;

			input_ = input;
			uword output_height = (input_->n_rows - kernel_size_.height);
			uword output_width = (input_->n_cols - kernel_size_.width);

#ifndef NDEBUG
			assert(output_height % stride_ == 0);
			assert(output_width % stride_ == 0);
			//TODO: for release build in case of working in another thread
			//TODO: we should use std::exception_ptr for thread-safety
#endif
			output_height = output_height / stride_ + 1;
			output_width = output_width / stride_ + 1;
			if (!receptiveField_) {
				receptiveField_ = std::make_shared<Cube<double>>(output_height, output_width,
				                                                input_->n_slices, fill::zeros);
			} else {
				receptiveField_->set_size(output_height, output_width, input_->n_slices);
				receptiveField_->zeros();
			}

			connectIndexes_.set_size(input_->n_rows, input_->n_cols, input_->n_slices);
			connectIndexes_.zeros();
			double maxVal;
			uword rowIdx, colIdx;

			for (uword d = 0; d < input_->n_slices; ++d) {
				uword out_col = 0;
				for (uword c = 0; c < input_->n_cols; c += stride_) {
					uword out_row = 0;
					for (uword r = 0; r < input_->n_rows; r += stride_) {
						maxVal = input_->slice(d)(span(r, r + kernel_size_.height - 1),
						                          span(c, c + kernel_size_.height - 1)).max(rowIdx, colIdx);
						(*receptiveField_)(out_row, out_col, d) = maxVal;
						connectIndexes_(r + rowIdx, c + colIdx, d) = 1;
						++out_row;
					}
					++out_col;
				}
			}

			// currently common to use the activation function after convolution layer
			// instead of a subsample layer
			if (!activFunc_) {
				output_ = receptiveField_;
			} else {
				if (!output_) {
					output_ = std::make_shared<Cube<double>>(output_height, output_width,
					                                        input_->n_slices);
				} else {
					output_->set_size(output_height, output_width, input_->n_slices);
				}
				activFunc_->Compute(receptiveField_, output_);
			}
		}

		std::pair<tensor4d, tensor4d> MaxPoolingLayer::Backward(
			const std::shared_ptr<arma::Cube<double>>& prevLocalLoss)
		{
			using namespace arma;
#ifndef NDEBUG
			assert(prevLocalLoss);
#endif
			// top layer was 1d. we need reshape error to 3d
			if (prevLocalLoss->n_slices == 1 && prevLocalLoss->n_cols == 1) {
				(*prevLocalLoss) = unvectorise(prevLocalLoss->get_ref(), output_->n_rows,
				                               output_->n_cols, output_->n_slices);
			}

			if (!localLoss_) {
				localLoss_ = std::make_shared<Cube<double>>(
					input_->n_rows, input_->n_cols, input_->n_slices, fill::zeros);
			} else {
				localLoss_->set_size(input_->n_rows, input_->n_cols, input_->n_slices);
				localLoss_->zeros();
			}
			uword rowIdx, colIdx;
			for (uword d = 0; d < input_->n_slices; ++d) {
				uword lossCol = 0;
				for (uword c = 0; c < input_->n_cols; c += stride_) {
					uword lossRow = 0;
					for (uword r = 0; r < input_->n_rows; r += stride_) {
						// from top to bottom propagates only connected losses
						connectIndexes_.slice(d)(span(r, r + kernel_size_.height - 1),
						                         span(c, c + kernel_size_.height - 1)
						               ).max(rowIdx, colIdx);
						(*localLoss_)(r + rowIdx, c + colIdx, d) = (*prevLocalLoss)(
							lossRow, lossCol, d);
						++lossRow;
					}
					++lossCol;
				}
			}

			if (activFunc_) {
				localLoss_->transform([&] (double value) {
					if (!value) {
						return value;
					} else
						return activFunc_->Derivative(value);
				});
			}

			// pool layer doesn't has weights
			return
					std::make_pair(tensor4d(), tensor4d());
		}

		std::pair<tensor4d, tensor4d> MaxPoolingLayer::Backward2nd(
			const std::shared_ptr<arma::Cube<double>>& prevLocalLoss)
		{
			using namespace arma;
#ifndef NDEBUG
			assert(prevLocalLoss);
#endif
			// top layer was 1d. we need reshape error to 3d
			if (prevLocalLoss->n_slices == 1 && prevLocalLoss->n_cols == 1) {
				(*prevLocalLoss) = unvectorise(prevLocalLoss->get_ref(), output_->n_rows,
											   output_->n_cols, output_->n_slices);
			}

			if (!localLoss_) {
				localLoss_ = std::make_shared<Cube<double>>(
					input_->n_rows, input_->n_cols, input_->n_slices, fill::zeros);
			} else {
				localLoss_->set_size(input_->n_rows, input_->n_cols, input_->n_slices);
				localLoss_->zeros();
			}
			uword rowIdx, colIdx;
			for (uword d = 0; d < input_->n_slices; ++d) {
				uword lossCol = 0;
				for (uword c = 0; c < input_->n_cols; c += stride_) {
					uword lossRow = 0;
					for (uword r = 0; r < input_->n_rows; r += stride_) {
						// from top to bottom propagates only connected losses
						connectIndexes_.slice(d)(span(r, r + kernel_size_.height - 1),
												 span(c, c + kernel_size_.height - 1)
												 ).max(rowIdx, colIdx);
						(*localLoss_)(r + rowIdx, c + colIdx, d) = (*prevLocalLoss)(
							lossRow, lossCol, d);
						++lossRow;
					}
					++lossCol;
				}
			}

			if (activFunc_) {
				localLoss_->transform([&](double value) {
					if (!value) {
						return value;
					} else
						return std::pow(activFunc_->Derivative(value), 2);
				});
			}

			// pool layer doesn't has weights
			return std::make_pair(tensor4d(), tensor4d());
		}
	}
}


#include "../point_distributor.hpp"

#include <boost/test/unit_test.hpp>
#include <boost/test/test_case_template.hpp>

#include <algorithm>

BOOST_AUTO_TEST_SUITE(point_distributor_tests)

BOOST_AUTO_TEST_CASE(example)
{
    //                      6
    //    1                /
    //   / \              /
    //  /(0)\            /(1)
    // 0 (4) 2  4       /
    //       | / \ (2) /
    //       |/   \(3)/
    //       3      5
    poly_line line {0,
        std::vector<coordinate> {
            coordinate {0, 0},
            coordinate {1, 1},
            coordinate {2, 0},
            coordinate {2, -1},
            coordinate {3, 0},
            coordinate {4, -1},
            coordinate {5, 2}
        }
    };

    std::vector<point> points = {
        point {point::NO_LINE_ID, 0u, coordinate {0.5, 0.5}},
        point {point::NO_LINE_ID, 1u, coordinate {5, 0.5}},
        point {point::NO_LINE_ID, 2u, coordinate {3.75, -0.6}},
        point {point::NO_LINE_ID, 3u, coordinate {3.75, -0.5}},
        point {point::NO_LINE_ID, 4u, coordinate {0.5, 0}}
    };

    std::vector<shortcut> tangents_from_first {
        shortcut {0, 3, 1, shortcut::type::MINIMAL_TANGENT},
        shortcut {0, 4, 2, shortcut::type::MAXIMAL_TANGENT},
        shortcut {0, 5, 3, shortcut::type::MINIMAL_TANGENT},
        shortcut {0, 6, 1, shortcut::type::MAXIMAL_TANGENT},
    };

    point_distributor distributor(line, std::move(points));
    auto assignments = distributor(0, tangents_from_first);

    BOOST_CHECK_EQUAL(assignments.size(), 2);
    BOOST_CHECK_EQUAL(assignments[0].first.id, 0);
    BOOST_CHECK_EQUAL(assignments[0].second, 0);
    BOOST_CHECK_EQUAL(assignments[1].first.id, 2);
    BOOST_CHECK_EQUAL(assignments[1].second, 3);
}

BOOST_AUTO_TEST_CASE(example_line_test)
{
    //
    // 0         a
    // |b
    // |
    // |     d
    // |    3----4
    // |    |c
    // 1----2
    //
    poly_line line;
    line.coordinates = {
        coordinate {0, 2},
        coordinate {0, 0},
        coordinate {1, 0},
        coordinate {1, 1},
        coordinate {2, 1}
    };

    std::vector<point> points = {
        {point::NO_LINE_ID, 0, coordinate {2.75, 2.25}}, // a
        {point::NO_LINE_ID, 1, coordinate {0.05, 1.9}},  // b
        {point::NO_LINE_ID, 2, coordinate {1.1, 0.9}},   // c
        {point::NO_LINE_ID, 3, coordinate {1.1, 1.1}},   // d
    };

    point_distributor distributor(line, std::move(points));

    {
        auto assignments = distributor(0, { shortcut {0, 4, 1, shortcut::type::MAXIMAL_TANGENT} });
        BOOST_CHECK_EQUAL(assignments.size(), 1);
        BOOST_CHECK_EQUAL(assignments[0].first.id, 1);
        BOOST_CHECK_EQUAL(assignments[0].second, 0);
    }

    {
        auto assignments = distributor(1, { shortcut {1, 3, 1, shortcut::type::MAXIMAL_TANGENT}, shortcut {1, 4, 3, shortcut::type::MINIMAL_TANGENT}});
        BOOST_CHECK_EQUAL(assignments.size(), 1);
        BOOST_CHECK_EQUAL(assignments[0].first.id, 2);
        BOOST_CHECK_EQUAL(assignments[0].second, 1);
    }

    {
        auto assignments = distributor(3, std::vector<shortcut> {});
        BOOST_CHECK_EQUAL(assignments.size(), 0);
    }

    {
        auto assignments = distributor(4, std::vector<shortcut> {});
        BOOST_CHECK_EQUAL(assignments.size(), 0);
    }
}

BOOST_AUTO_TEST_CASE(example_second_line_test)
{
    //
    //       2--3
    //      / a  \
    //  0--1      4----5
    //                 |
    //                 |
    //                 |
    //                 |
    //                 6
    //

    std::vector<coordinate> cs = {
        coordinate {1, 2},      // 0
        coordinate {1.5, 2},    // 1
        coordinate {2, 3},      // 2
        coordinate {2.5, 3},    // 3
        coordinate {3, 2},      // 4
        coordinate {4, 2},      // 5
        coordinate {4, 0},      // 6
    };

    std::vector<point> points = {
        {point::NO_LINE_ID, 0, coordinate {2.25, 2.75}}, // a
        {point::NO_LINE_ID, 1, coordinate {2.1, 0.05}},  // b
        {point::NO_LINE_ID, 2, coordinate {0.9, 1.1}},   // c
        {point::NO_LINE_ID, 3, coordinate {1.1, 1.1}},   // d
    };

    std::vector<shortcut> tangents_from_first {
        shortcut {0, 2, 1, shortcut::type::MAXIMAL_TANGENT},
        shortcut {0, 6, 1, shortcut::type::MINIMAL_TANGENT},
    };

    poly_line line {2, cs};

    point_distributor distributor(line, std::move(points));
    auto assignments = distributor(0, { shortcut {0, 4, 1, shortcut::type::MAXIMAL_TANGENT} });
}

BOOST_AUTO_TEST_CASE(data_test1)
{
    std::vector<coordinate> coords = {
        coordinate {-5368675.69013, -7984749.59382},
        coordinate {-5367570.7913,  -7984392.03562},
        coordinate {-5366848.50016, -7983220.84326},
        coordinate {-5364796.57957, -7982343.53435},
    };

    std::vector<shortcut> tangents {
        shortcut {0, 2, 1, shortcut::type::MAXIMAL_TANGENT},
        shortcut {0, 3, 2, shortcut::type::MINIMAL_TANGENT},
    };

    std::vector<point> points = {
        {point::NO_LINE_ID, 0, coordinate {3879.013320000842, 11682.980560000055}},
        {point::NO_LINE_ID, 1, coordinate {4560.606140000746, 13655.561940000392}}
    };

    poly_line line {0, coords};
    point_distributor distributor(line, std::move(points));
    auto assignments = distributor(0, tangents);
}

BOOST_AUTO_TEST_SUITE_END()


/******************************************************************************

  Point-to-point implementation of the ICP registration algorithm for 3D point
  clouds.

  Copyright (c) 2012
  Alexander Rukletsov <rukletsov@gmail.com>
  Dzmitry Hlindzich <hlindzich@gmail.com>
  All rights reserved.

  Redistribution and use in source and binary forms, with or without
  modification, are permitted provided that the following conditions
  are met:
  1.  Redistributions of source code must retain the above copyright
      notice, this list of conditions and the following disclaimer.
  2.  Redistributions in binary form must reproduce the above copyright
      notice, this list of conditions and the following disclaimer in the
      documentation and/or other materials provided with the distribution.

  THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS "AS IS" AND
  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  SUCH DAMAGE.

*******************************************************************************/

#ifndef ICP_3D_HPP_507CF525_CC8A_4499_80D2_E8C8644603F5_
#define ICP_3D_HPP_507CF525_CC8A_4499_80D2_E8C8644603F5_

#include <vector>
#include <utility>
#include <boost/array.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include <boost/assert.hpp>

#include "bo/core/vector.hpp"
#include "bo/core/kdtree.hpp"
#include "bo/math/mean.hpp"
#include "bo/math/transformation_3d.hpp"
#include "bo/math/blas_extensions.hpp"
#include "bo/math/blas_conversions.hpp"

/* Usage example:

    std::vector<Vertex> std_source();
    ... // Initialize the source point cloud here.

    boost::shared_ptr<std::vector<Vertex> > std_target_ptr = boost::shared_ptr<std::vector<Vertex> >
    (new std::vector<Vertex>());
    ... // Initialize the target point cloud here.

    // Create an ICP registrator.
    bo::registration::ICP3D<float> icp(std_source, std_target_ptr,
    bo::distances::euclidean_distance<float, 3>);

    // Initialize auxiliary variables.
    // Iteration counter.
    std::size_t iteration = 0;
    // Norm of the transformation difference.
    float epsilon = std::numeric_limits<float>::max();
    // Distance between the source and target point clouds.
    float distance = std::numeric_limits<float>::max();
    // Transformation matrices.
    bo::math::matrix<float> m1;
    bo::math::matrix<float> m2;

    // Iteratively register two point clouds.
    while (iteration < max_allowed_iterations && epsilon >= min_transformation_epsilon)
    {
        // Cache the transformation from the previous iteration.
        m1 = m2;

        // Perform a new ICP registration step and update the current distance
        // between the source and target.
        distance = icp.next();

        // Update the current transformation.
        m2 = icp.current_transformation().matrix();

        // Update the norm of the transformation difference.
        if (iteration > 0)
        {
            bo::math::matrix<float> mdif = m2 - m1;
            epsilon = bo::math::l1_norm(mdif);
        }

        ++iteration;
    }

    // The final transformation.
    bo::Transformation3D<float> final_transformation = icp.current_transformation();

    // The corresponding final transformation matrix.
    bo::math::matrix<float> final_matrix = final_transformation.matrix();
*/

namespace bo {
namespace registration {

template <typename RealType>
class ICP3D: public boost::noncopyable
{
public:
    typedef Vector<RealType, 3> Point3D;
    typedef std::vector<Point3D> PointCloud;
    typedef boost::shared_ptr<PointCloud> PointCloudPtr;
    typedef boost::function<RealType (Point3D, Point3D)> Metric;
    typedef math::Transformation3D<RealType> Transformation;

public:
    ICP3D(const PointCloud& source, PointCloudPtr target, Metric dist_fun,
          bool is_preprocess = true);

    RealType next();
    Transformation current_transformation() const;
    const PointCloud& current_cloud() const;
    const PointCloud& current_correspondence() const;

private:

    typedef bo::KDTree<3, Point3D, std::pointer_to_binary_function
        <const Point3D&, std::size_t, RealType> > Point3DTree;
    static RealType point_bac(const Point3D& p, std::size_t k);

    void overlay_();
    Point3D centroid_(PointCloud* cloud) const;
    // Attention: the elements cloud2[i] must correspond to the elements cloud1[i].
    math::matrix<RealType> cross_covariance_(PointCloud* cloud1, PointCloud* cloud2,
        const Point3D& centroid1, const Point3D& centroid2) const;
    // Attention: the elements cloud2[i] must correspond to the elements cloud1[i].
    RealType distance_(PointCloud* cloud1, PointCloud* cloud2) const;
    void update_current_transform_and_cloud_(const Transformation& m);

private:
    PointCloudPtr target_cloud_;
    PointCloud current_cloud_;
    PointCloud corresp_cloud_;
    Transformation current_trans_;
    Point3DTree tree_;
    Metric dist_fun_;
};

// Includes computing a kd-tree for the target point cloud.
template <typename RealType>
ICP3D<RealType>::ICP3D(const PointCloud& source, PointCloudPtr target,
    Metric dist_fun, bool is_preprocess):
    current_cloud_(source), target_cloud_(target), dist_fun_(dist_fun),
    current_trans_(),
    tree_(target->begin(), target->end(), std::ptr_fun(ICP3D<RealType>::point_bac))
{
    if (is_preprocess)
        overlay_();
}

template <typename RealType>
RealType ICP3D<RealType>::next()
{
    // Calculate the mass center of the current point cloud.
    Point3D current_centroid = centroid_(&current_cloud_);

    // Update the correspondence.
    corresp_cloud_.clear();
    for (typename PointCloud::const_iterator it = current_cloud_.begin(); it != current_cloud_.end(); ++it)
    {
        std::pair<typename Point3DTree::const_iterator, RealType> closest = tree_.find_nearest(*it);
        corresp_cloud_.push_back(*closest.first);
    }

    // Calculate the mass center of the corresponding points.
    Point3D corresp_centroid = centroid_(&corresp_cloud_);

    // Calculate the cross covariance for the current points and the target ones.
    math::matrix<RealType> Spx = cross_covariance_(&current_cloud_, &corresp_cloud_, current_centroid,
        corresp_centroid);

    math::matrix<RealType> SpxT = trans(Spx);

    // Create the asymmetrical matrix.
    math::matrix<RealType> Apx = Spx - SpxT;

    // Calculate the matrix trace.
    RealType traceSpx = Spx(0, 0) + Spx(1, 1) + Spx(2, 2);

    math::matrix<RealType> Bpx = Spx + SpxT -
        traceSpx * math::identity_matrix<RealType>(3);

    // Create the 4x4 matrix.
    math::matrix<RealType> Qpx(4, 4);

    // Fill in the matrix.
    // Block 1.
    Qpx(0, 0) = traceSpx;
    // Block 2.
    Qpx(0, 1) = Apx(1, 2);
    Qpx(0, 2) = Apx(2, 0);
    Qpx(0, 3) = Apx(0, 1);
    // Block 3.
    Qpx(1, 0) = Apx(1, 2);
    Qpx(2, 0) = Apx(2, 0);
    Qpx(3, 0) = Apx(0, 1);
    // Block 4.
    math::subrange(Qpx, 1, 4, 1, 4) = math::subrange(Bpx, 0, 3, 0, 3);

    math::eigen_symmetric(Qpx);

    // Quaternion that defines the optimal rotation.
//    Vector<RealType, 4> quaternion = math::to_bo_vector(math::column(Qpx, 3));
    math::bounded_vector<RealType, 4> col(math::column(Qpx, 3));
    Vector<RealType, 4> quaternion = math::to_bo_vector(col);

    // Optimal translation.
    Point3D translation = corresp_centroid - Transformation(quaternion) * current_centroid;

    // Create the optimal transformation.
    Transformation optimal_trans(quaternion, translation);

    // Update the current point cloud and the transformation.
    update_current_transform_and_cloud_(optimal_trans);

    return distance_(&current_cloud_, &corresp_cloud_);
}

template <typename RealType>
typename ICP3D<RealType>::Transformation ICP3D<RealType>::current_transformation() const
{
    return current_trans_;
}

template <typename RealType>
const typename ICP3D<RealType>::PointCloud& ICP3D<RealType>::current_cloud() const
{
    return current_cloud_;
}

template <typename RealType>
const typename ICP3D<RealType>::PointCloud& ICP3D<RealType>::current_correspondence() const
{
    return corresp_cloud_;
}

template <typename RealType>
void ICP3D<RealType>::overlay_()
{
    // Cloud shift using centroids.

    Point3D translation = centroid_(target_cloud_.get()) - centroid_(&current_cloud_);

    Transformation shift_transform(translation);

    // Update the current point cloud and the transformation.
    update_current_transform_and_cloud_(shift_transform);
}

template <typename RealType>
typename ICP3D<RealType>::Point3D ICP3D<RealType>::centroid_(PointCloud* cloud) const
{
    BOOST_ASSERT(cloud->size() > 0);
    Point3D mass_center = math::mean(*cloud);

    return mass_center;
}

template <typename RealType>
math::matrix<RealType> ICP3D<RealType>::cross_covariance_(PointCloud* cloud1, PointCloud* cloud2,
    const Point3D& centroid1, const Point3D& centroid2) const
{
    const std::size_t n = cloud1->size();

    BOOST_ASSERT(cloud2->size() == n && n > 0);

    math::matrix<RealType> m = math::zero_matrix<RealType>(3, 3);
    math::matrix<RealType> p(3, 1);
    math::matrix<RealType> x(1, 3);

    for (std::size_t i = 0; i < n; ++i)
    {
        Point3D a = cloud1->at(i);
        Point3D b = cloud2->at(i);

        p(0, 0) = a[0] - centroid1[0];
        p(1, 0) = a[1] - centroid1[1];
        p(2, 0) = a[2] - centroid1[2];

        x(0, 0) = b[0] - centroid2[0];
        x(0, 1) = b[1] - centroid2[1];
        x(0, 2) = b[2] - centroid2[2];

        // Accumulate the elements.
        // Warning: type overflow is possible here!
        m = m + math::prod(p, x);
    }

    m = m / n;

    return m;
}

template <typename RealType>
RealType ICP3D<RealType>::distance_(PointCloud* cloud1, PointCloud* cloud2) const
{
    const std::size_t n = cloud1->size();

    BOOST_ASSERT(cloud2->size() == n);

    RealType sum(0);

    for (std::size_t i = 0; i < n; ++i)
    {
        Point3D a = cloud1->at(i);
        Point3D b = cloud2->at(i);
        sum += dist_fun_(a, b);
    }

    return sum;
}

// Point3D brackets accessor.
template <typename RealType>
inline RealType ICP3D<RealType>::point_bac(const Point3D& p, std::size_t k)
{
    return p[k];
}

template <typename RealType>
void ICP3D<RealType>::update_current_transform_and_cloud_(const Transformation& m)
{
    // Update the current transformation.
    current_trans_ = m * current_trans_;

    // Update the current point cloud.
    for (typename PointCloud::iterator it = current_cloud_.begin(); it != current_cloud_.end(); ++it)
    {
        *it = m * (*it);
    }
}

} // namespace registration
} // namespace bo

#endif // ICP_3D_HPP_507CF525_CC8A_4499_80D2_E8C8644603F5_


#ifndef CONCEPTS_DIMENSIONS_COMMON_HPP_INCLUDED
#define CONCEPTS_DIMENSIONS_COMMON_HPP_INCLUDED

#include <limits>

#include <boost/hana/core/common.hpp>
#include <boost/hana/integral_constant.hpp>
#include <boost/hana/max.hpp>
#include <boost/hana/min.hpp>
#include <boost/hana/value.hpp>

#include <gem/fwd/dimensions.hpp>

namespace gem::concepts {

template <Dimension D1, Dimension D2>
concept bool CommonDimensionPair =
    Dimension<D1> && Dimension<D2> &&
    !(boost::hana::value(D1::min > D2::max) ||
      boost::hana::value(D2::min > D1::max));

template <Dimension D1, Dimension D2>
concept bool InvalidCommonDimensionPair =
    Dimension<D1> && Dimension<D2> &&
    (boost::hana::value(D1::min > D2::max) ||
      boost::hana::value(D2::min > D1::max));

template <gem::concepts::detail::Integral T1, Dimension D2>
concept bool CommonIntegralDimensionPair =
    gem::concepts::detail::Integral<T1> && Dimension<D2> &&
    !(std::numeric_limits<T1>::min() > boost::hana::value(D2::max) ||
      boost::hana::value(D2::min) > std::numeric_limits<T1>::max());

template <Dimension D1, gem::concepts::detail::Integral T2>
concept bool CommonDimensionIntegralPair =
    Dimension<D1> && gem::concepts::detail::Integral<T2> &&
    !(boost::hana::value(D1::min) > std::numeric_limits<T2>::max() ||
      std::numeric_limits<T2>::min() > boost::hana::value(D1::max));

}  // namespace gem::concepts

namespace boost::hana {

gem::concepts::CommonDimensionPair {D1, D2}
struct common<D1, D2>
{
    using type = gem::Dimension<boost::hana::common_t<typename D1::dim_t,
                                                      typename D2::dim_t>,
                                value(D1::hint) / 2 + value(D2::hint) / 2,
                                value(max(D1::max, D2::max)),
                                value(min(D1::min, D2::min))>;
};

gem::concepts::InvalidCommonDimensionPair {D1, D2}
struct common<D1, D2>
{
    using type = gem::Dimension<boost::hana::common_t<typename D1::dim_t,
                                                      typename D2::dim_t>,
                                value(D1::hint) / 2 + value(D2::hint) / 2,
                                value(max(D1::max, D2::max)),
                                value(min(D1::min, D2::min))>;
};

gem::concepts::CommonIntegralDimensionPair {T1, D2}
struct common<T1, D2>
{
    using type = boost::hana::common_t<T1, typename D2::dim_t>;
};

gem::concepts::CommonDimensionIntegralPair {D1, T2}
struct common<D1, T2>
{
    using type = boost::hana::common_t<typename D1::dim_t, T2>;
};

gem::concepts::CommonIntegralDimensionPair {T1, D2}
struct common<boost::hana::integral_constant_tag<T1>, D2>
{
    using type = typename decltype(
        if_(D2::max == D2::min,
            type_c<boost::hana::integral_constant_tag<
                boost::hana::common_t<T1, typename D2::dim_t>>>,
            type_c<
                boost::hana::common_t<T1, typename D2::dim_t>>))::type;
};

gem::concepts::CommonDimensionIntegralPair {D1, T2}
struct common<D1, boost::hana::integral_constant_tag<T2>>
{
    using type = typename decltype(
        if_(D1::max == D1::min,
            type_c<boost::hana::integral_constant_tag<
                boost::hana::common_t<typename D1::dim_t, T2>>>,
            type_c<
                boost::hana::common_t<typename D1::dim_t, T2>>))::type;
};

}  // namespace boost::hana

#endif  // !CONCEPTS_DIMENSIONS_COMMON_HPP_INCLUDED


// smooth_feedback: Control theory on Lie groups
// https://github.com/pettni/smooth_feedback
//
// Licensed under the MIT License <http://opensource.org/licenses/MIT>.
//
// Copyright (c) 2021 Petter Nilsson
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in all
// copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
// SOFTWARE.

#ifndef SMOOTH__FEEDBACK__OCP_TO_QP_HPP_
#define SMOOTH__FEEDBACK__OCP_TO_QP_HPP_

/**
 * @file
 * @brief Formulate optimal control problem as a quadratic program
 */

#include <Eigen/Core>
#include <smooth/diff.hpp>

#include "collocation/mesh.hpp"
#include "collocation/mesh_function.hpp"
#include "ocp.hpp"
#include "qp.hpp"

namespace smooth::feedback {

// \cond
namespace detail {

/**
 * @brief Working memory for ocp_to_qp
 */
struct OcpToQpWorkmemory
{
  MeshValue<1> cr_out;   /// @brief output of mesh_eval
  MeshValue<2> int_out;  /// @brief output of mesh_integrate
};

/**
 * @brief Allocate a qp for ocp_to_qp_update()
 *
 * @param[out] qp quadratic program to allocate
 * @param[out] work working memory to allocate
 * @param[in] ocp input problem
 * @param[in] mesh time discretization
 */
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_allocate(
  QuadraticProgramSparse<double> & qp,
  OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh)
{
  using ocp_t = typename std::decay_t<decltype(ocp)>;

  using X = typename ocp_t::X;
  using U = typename ocp_t::U;

  static constexpr auto Nx = ocp_t::Nx;
  static constexpr auto Nu = ocp_t::Nu;

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  // [x0 x1 ... xN u0 u1 ... uN-1]

  const auto N = mesh.N_colloc();

  const auto xvar_L = Nx * (N + 1);
  const auto uvar_L = Nu * N;

  const auto dcon_L  = Nx * N;
  const auto crcon_L = ocp_t::Ncr * N;
  const auto cecon_L = ocp_t::Nce;

  const auto dcon_B  = 0u;
  const auto crcon_B = dcon_L;
  const auto cecon_B = crcon_B + crcon_L;

  const auto Nvar = xvar_L + uvar_L;
  const auto Ncon = dcon_L + crcon_L + cecon_L;

  // resize qp
  qp.P.resize(Nvar, Nvar);
  qp.q.setZero(Nvar);
  qp.A.resize(Ncon, Nvar);
  qp.l.setZero(Ncon);
  qp.u.setZero(Ncon);

  // sparsity pattern of A (row-major)
  Eigen::VectorXi A_pattern = Eigen::VectorXi::Zero(Ncon);
  for (auto ival = 0u, I0 = 0u; ival < mesh.N_ivals(); I0 += mesh.N_colloc_ival(ival), ++ival) {
    const auto Ki = mesh.N_colloc_ival(ival);  // number of nodes in interval
    A_pattern.segment(dcon_B + I0 * Nx, Ki * Nx) +=
      Eigen::VectorXi::Constant(Ki * Nx, Nx + Ki + Nu);
  }
  A_pattern.segment(crcon_B, crcon_L).setConstant(Nx + Nu);
  A_pattern.segment(cecon_B, cecon_L).setConstant(2 * Nx);
  qp.A.reserve(A_pattern);

  // sparsity pattern of P
  Eigen::VectorXi P_pattern = Eigen::VectorXi::Zero(Nvar);
  for (auto i = 0u; i < N; ++i) {
    for (auto j = 0u; j < Nx; ++j) { P_pattern(Nx * i + j) = j + 1; }
  }
  for (auto j = 0u; j < Nx; ++j) { P_pattern(Nx * N + j) = Nx + j + 1; }
  for (auto i = 0u; i < N; ++i) {
    for (auto j = 0u; j < Nu; ++j) { P_pattern(Nx * (N + 1) + Nu * i + j) = Nx + (j + 1); }
  }
  qp.P.reserve(P_pattern);

  // compute work stuff once to allocate pattern
  const double tf = 1.;
  auto xslin      = mesh.all_nodes() | transform([&](double) { return Identity<X>(); });
  auto uslin      = mesh.all_nodes() | transform([&](double) { return Identity<U>(); });

  work.int_out.lambda.setConstant(1, 1);
  mesh_eval<1, DT>(work.cr_out, mesh, ocp.cr, 0, tf, xslin, uslin);       // allocates work.cr_out
  mesh_integrate<2, DT>(work.int_out, mesh, ocp.g, 0, tf, xslin, uslin);  // allocates work.int_out

  work.cr_out.dF.makeCompressed();
  work.int_out.dF.makeCompressed();
  work.int_out.d2F.makeCompressed();
}

/// @brief ocp_to_qp_update: cost part
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update_cost(
  QuadraticProgramSparse<double> & qp,
  OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  auto && xl_fun,
  auto && ul_fun)
{
  using ocp_t = typename std::decay_t<decltype(ocp)>;
  using X     = typename ocp_t::X;

  static constexpr auto Nx = ocp_t::Nx;
  static constexpr auto Nu = ocp_t::Nu;
  static constexpr auto Nq = ocp_t::Nq;
  const double t0          = 0.;

  static_assert(ocp_t::Nq == 1, "exactly one integral supported in ocp_to_qp");

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  const auto N      = mesh.N_colloc();
  const auto xvar_L = Nx * (N + 1);
  const auto uvar_L = Nu * N;
  const auto xvar_B = 0u;
  const auto uvar_B = xvar_L;

  ////////////////////////
  //// ZERO VARIABLES ////
  ////////////////////////

  set_zero(qp.P);
  qp.q.setZero();

  //////////////////////////////
  //// LINEARIZATION POINTS ////
  //////////////////////////////

  const X xl0 = xl_fun(0.);
  const X xlf = xl_fun(tf);

  auto xslin = mesh.all_nodes() | transform([&](double t) { return xl_fun(t0 + (tf - t0) * t); });
  auto uslin = mesh.all_nodes() | transform([&](double t) { return ul_fun(t0 + (tf - t0) * t); });

  const Eigen::Vector<double, 1> ql{1.};

  ///////////////////////
  //// INTEGRAL COST ////
  ///////////////////////

  const auto & [th, dth, d2th] = diff::dr<2, DT>(ocp.theta, wrt(tf, xl0, xlf, ql));

  const Eigen::Vector<double, Nx> qo_x0 = dth.middleCols(1, Nx).transpose();
  const Eigen::Vector<double, Nx> qo_xf = dth.middleCols(1 + Nx, Nx).transpose();
  const Eigen::Vector<double, Nq> qo_q  = dth.middleCols(1 + 2 * Nx, Nq).transpose();

  mesh_integrate<2, DT>(work.int_out, mesh, ocp.g, 0, tf, xslin, uslin);

  // clang-format off
  block_add(qp.P, 0, 0, work.int_out.d2F.block(2, 2, xvar_L + uvar_L, xvar_L + uvar_L), qo_q.x(), true);

  qp.q.segment(xvar_B, xvar_L) = qo_q.x() * work.int_out.dF.middleCols(2, xvar_L).transpose();
  qp.q.segment(uvar_B, uvar_L) = qo_q.x() * work.int_out.dF.middleCols(2 + xvar_L, uvar_L).transpose();
  // clang-format on

  ///////////////////////
  //// ENDPOINT COST ////
  ///////////////////////

  block_add(qp.P, 0, 0, d2th.block(1, 1, Nx, Nx), 0.5, true);                      // d2q / dx0x0
  block_add(qp.P, 0, Nx * N, d2th.block(1, 1 + Nx, Nx, Nx), 0.5, true);            // d2q / dx0xf
  block_add(qp.P, Nx * N, Nx * N, d2th.block(1 + Nx, 1 + Nx, Nx, Nx), 0.5, true);  // d2q / dxfxf

  qp.q.segment(0, Nx) += qo_x0;       // dq / dx0
  qp.q.segment(Nx * N, Nx) += qo_xf;  // dq / dxf
}

/// @brief ocp_to_qp_update: dyn part
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update_dyn(
  QuadraticProgramSparse<double> & qp,
  [[maybe_unused]] OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  auto && xl_fun,
  auto && ul_fun)
{
  using utils::zip;
  using namespace std::views;
  using ocp_t = typename std::decay_t<decltype(ocp)>;
  using X     = typename ocp_t::X;

  static constexpr auto Nx = ocp_t::Nx;
  static constexpr auto Nu = ocp_t::Nu;
  const double t0          = 0.;

  static_assert(ocp_t::Nq == 1, "exactly one integral supported in ocp_to_qp");

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  const auto N      = mesh.N_colloc();
  const auto xvar_L = Nx * (N + 1);
  const auto dcon_L = Nx * N;
  const auto xvar_B = 0u;
  const auto uvar_B = xvar_L;
  const auto dcon_B = 0u;

  ////////////////////////
  //// ZERO VARIABLES ////
  ////////////////////////

  set_zero(qp.A.middleRows(dcon_B, dcon_L));

  /////////////////////////////////
  //// COLLOCATION CONSTRAINTS ////
  /////////////////////////////////

  for (auto ival = 0u, M = 0u; ival < mesh.N_ivals(); M += mesh.N_colloc_ival(ival), ++ival) {
    const auto Ki = mesh.N_colloc_ival(ival);  // number of nodes in interval

    const auto [alpha, Dus] = mesh.interval_diffmat_unscaled(ival);

    // in each interval the collocation constraint is
    // [A0 x0 ... Ak-1 xk-1 0]  + [B0 u0 ... Bk-1 uk-1] + [E0 ... Ek-1] = alpha * X Dus

    for (const auto & [i, tau_i] : zip(iota(0u, Ki), mesh.interval_nodes(ival))) {
      const auto t_i             = t0 + (tf - t0) * tau_i;             // unscaled time
      const auto & [xl_i, dxl_i] = diff::dr<1, DT>(xl_fun, wrt(t_i));  // x-lin
      const auto ul_i            = ul_fun(t_i);                        // u-lin

      // linearize dynamics and insert new constraint A xi + B ui + E = [x0 ... XNi] di

      const auto & [f_i, df_i] = diff::dr<1, DT>(ocp.f, wrt(t_i, xl_i, ul_i));

      // clang-format off
      block_add(qp.A, dcon_B + (M + i) * Nx, xvar_B + (M + i) * Nx, df_i.template middleCols<Nx>(1), tf);        // A
      block_add(qp.A, dcon_B + (M + i) * Nx, uvar_B + (M + i) * Nu, df_i.template middleCols<Nu>(1 + Nx), tf);   // B
      // clang-format on

      if constexpr (!IsCommutative<X>) {
        block_add(qp.A, dcon_B + (M + i) * Nx, xvar_B + (M + i) * Nx, ad<X>(f_i + dxl_i), -tf / 2);
      }

      for (auto j = 0u; j < Ki + 1; ++j) {
        for (auto diag = 0u; diag < Nx; ++diag) {
          qp.A.coeffRef(dcon_B + (M + i) * Nx + diag, (M + j) * Nx + diag) -= alpha * Dus(j, i);
        }
      }

      qp.l.segment(dcon_B + (M + i) * Nx, Nx) = -tf * (f_i - dxl_i);
      qp.u.segment(dcon_B + (M + i) * Nx, Nx) = qp.l.segment(dcon_B + (M + i) * Nx, Nx);
    }
  }
}

/// @brief ocp_to_qp_update: running constraints part
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update_cr(
  QuadraticProgramSparse<double> & qp,
  OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  auto && xl_fun,
  auto && ul_fun)
{
  using ocp_t = typename std::decay_t<decltype(ocp)>;

  static constexpr auto Nx = ocp_t::Nx;
  static constexpr auto Nu = ocp_t::Nu;

  const double t0 = 0.;

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  const auto N       = mesh.N_colloc();
  const auto xvar_L  = Nx * (N + 1);
  const auto uvar_L  = Nu * N;
  const auto dcon_L  = Nx * N;
  const auto crcon_L = ocp_t::Ncr * N;
  const auto crcon_B = dcon_L;

  //////////////////////////////
  //// LINEARIZATION POINTS ////
  //////////////////////////////

  auto xslin = mesh.all_nodes() | transform([&](double t) { return xl_fun(t0 + (tf - t0) * t); });
  auto uslin = mesh.all_nodes() | transform([&](double t) { return ul_fun(t0 + (tf - t0) * t); });

  /////////////////////////////
  //// RUNNING CONSTRAINTS ////
  /////////////////////////////

  mesh_eval<1, DT>(work.cr_out, mesh, ocp.cr, 0, tf, xslin, uslin);

  block_write(qp.A, crcon_B, 0, work.cr_out.dF.middleCols(2, xvar_L + uvar_L));
  qp.l.segment(crcon_B, crcon_L) = ocp.crl.replicate(N, 1) - work.cr_out.F;
  qp.u.segment(crcon_B, crcon_L) = ocp.cru.replicate(N, 1) - work.cr_out.F;
}

/// @brief ocp_to_qp_update: end constraints part
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update_ce(
  QuadraticProgramSparse<double> & qp,
  [[maybe_unused]] OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  auto && xl_fun,
  [[maybe_unused]] auto && ul_fun)
{
  using ocp_t = typename std::decay_t<decltype(ocp)>;
  using X     = typename ocp_t::X;

  static constexpr auto Nx = ocp_t::Nx;

  /////////////////////////
  //// VARIABLE LAYOUT ////
  /////////////////////////

  const auto N       = mesh.N_colloc();
  const auto xvar_L  = Nx * (N + 1);
  const auto xvar_B  = 0u;
  const auto dcon_L  = Nx * N;
  const auto crcon_L = ocp_t::Ncr * N;
  const auto cecon_L = ocp_t::Nce;
  const auto crcon_B = dcon_L;
  const auto cecon_B = crcon_B + crcon_L;

  //////////////////////////////
  //// LINEARIZATION POINTS ////
  //////////////////////////////

  const X xl0 = xl_fun(0.);
  const X xlf = xl_fun(tf);

  //////////////////////////////
  //// ENDPOINT CONSTRAINTS ////
  //////////////////////////////

  const Eigen::Vector<double, 1> ql{1.};
  const auto & [ceval, dceval] = diff::dr<1, DT>(ocp.ce, wrt(tf, xl0, xlf, ql));

  block_write(qp.A, cecon_B, xvar_B, dceval.middleCols(1, Nx));                     // dce / dx0
  block_write(qp.A, cecon_B, xvar_B + xvar_L - Nx, dceval.middleCols(1 + Nx, Nx));  // dce / dxf

  qp.l.segment(cecon_B, cecon_L) = ocp.cel - ceval;
  qp.u.segment(cecon_B, cecon_L) = ocp.ceu - ceval;
}

/**
 * @brief Update a qp for ocp_to_qp_update()
 *
 * @param[out] qp quadratic program
 * @param[in, out] work
 * @param[in] ocp input problem
 * @param[in] mesh time discretization
 * @param[in] tf time horizon
 * @param[in] xl_fun state linearization (must be differentiable w.r.t. time)
 * @param[in] ul_fun input linearization
 */
template<diff::Type DT = diff::Type::Default>
void ocp_to_qp_update(
  QuadraticProgramSparse<double> & qp,
  [[maybe_unused]] OcpToQpWorkmemory & work,
  OCPType auto & ocp,
  const MeshType auto & mesh,
  double tf,
  const auto & xl_fun,
  [[maybe_unused]] const auto & ul_fun)
{
  ocp_to_qp_update_cost<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);
  ocp_to_qp_update_dyn<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);
  ocp_to_qp_update_cr<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);
  ocp_to_qp_update_ce<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);
}

}  // namespace detail
// \endcond

/**
 * @brief Formulate an optimal control problem as a quadratic program via linearization.
 *
 * @param ocp input problem
 * @param mesh time discretization
 * @param tf time horizon
 * @param xl_fun state linearization (must be differentiable w.r.t. time)
 * @param ul_fun input linearization
 *
 * @return sparse quadratic program for a flattened formulation of ocp.
 *
 * @note allocates memory for each call. To reduce memory allocation for repeated calls, see
 * ocp_to_qp_allocate() and ocp_to_qp_update().
 *
 * @see qpsol_to_ocpsol()
 */
template<diff::Type DT = diff::Type::Default>
QuadraticProgramSparse<double> ocp_to_qp(
  const OCPType auto & ocp, const MeshType auto & mesh, double tf, auto && xl_fun, auto && ul_fun)
{
  QuadraticProgramSparse<double> qp;
  detail::OcpToQpWorkmemory work;

  detail::ocp_to_qp_allocate<DT>(qp, work, ocp, mesh);
  detail::ocp_to_qp_update<DT>(qp, work, ocp, mesh, tf, xl_fun, ul_fun);

  qp.A.makeCompressed();
  qp.P.makeCompressed();

  return qp;
}

/**
 * @brief Convert QP solution to OCP solution
 *
 * If qp_sol solves a QP obtained via ocp_to_qp(), then qpsol_to_ocpsol(qp_sol)
 * is the corrensponding OCP solution.
 *
 * @param ocp optimal control problem
 * @param mesh discretization mesh
 * @param qpsol solution to quadratic program obtained via ocp_to_qp()
 * @param tf final time used in ocp_to_qp()
 * @param xl_fun state linearization trajectory used in ocp_to_qp()
 * @param ul_fun input linearization trajectory used in ocp_to_qp()
 *
 * @see ocp_to_qp()
 */
auto qpsol_to_ocpsol(
  const OCPType auto & ocp,
  const MeshType auto & mesh,
  const QPSolution<-1, -1, double> & qpsol,
  double tf,
  auto && xl_fun,
  auto && ul_fun)
{
  using ocp_t = std::decay_t<decltype(ocp)>;

  using X = typename ocp_t::X;
  using U = typename ocp_t::U;

  static constexpr int Nx  = ocp_t::Nx;
  static constexpr int Nu  = ocp_t::Nu;
  static constexpr int Nq  = ocp_t::Nq;
  static constexpr int Ncr = ocp_t::Ncr;
  static constexpr int Nce = ocp_t::Nce;

  const auto N = mesh.N_colloc();

  const auto xvar_L = Nx * (N + 1);
  const auto uvar_L = Nu * N;

  const auto xvar_B    = 0u;
  const auto uvar_B    = xvar_L;
  Eigen::MatrixXd Xmat = qpsol.primal.segment(xvar_B, xvar_L).reshaped(Nx, N + 1);
  Eigen::MatrixXd Umat = qpsol.primal.segment(uvar_B, uvar_L).reshaped(Nu, N);

  auto xfun = [t0     = 0.,
               tf     = tf,
               mesh   = mesh,
               Xmat   = std::move(Xmat),
               xl_fun = std::forward<decltype(xl_fun)>(xl_fun)](double t) -> X {
    const auto tngnt =
      mesh.template eval<Eigen::Vector<double, Nx>>((t - t0) / (tf - t0), Xmat.colwise(), 0, true);
    return rplus(xl_fun(t), tngnt);
  };

  auto ufun = [t0     = 0.,
               tf     = tf,
               mesh   = mesh,
               Umat   = std::move(Umat),
               ul_fun = std::forward<decltype(ul_fun)>(ul_fun)](double t) -> U {
    const auto tngnt =
      mesh.template eval<Eigen::Vector<double, Nu>>((t - t0) / (tf - t0), Umat.colwise(), 0, false);
    return rplus(ul_fun(t), tngnt);
  };

  return OCPSolution<X, U, Nq, Nce, Ncr>{
    .t0 = 0.,
    .tf = tf,
    .u  = std::move(ufun),
    .x  = std::move(xfun),
  };
}

}  // namespace smooth::feedback

#endif  // SMOOTH__FEEDBACK__OCP_TO_QP_HPP_


/*************************************************************************\
License
    Copyright (c) 2017 Kavvadias Ioannis.

    This file is part of SPHSimulator.

    Licensed under the MIT License. See LICENSE file in the project root for
    full license information.

\************************************************************************/

#include "Reorderer.hpp"

#include "Kernels.hpp"
#include "Statistics/Statistics.hpp"
#include "Util/ZValue.hpp"

#include <boost/range/irange.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/adaptor/indexed.hpp>

#include <numeric>
#include <limits>

using boost::adaptors::transformed;
using boost::adaptors::indexed;

namespace
{
  template <typename RandomAccessCont, typename RandomAccessIndexes>
  void indexedReordering (RandomAccessCont& c, const RandomAccessIndexes& indexes)
  {
    auto indexedValuesR = indexes | transformed([&c]( auto i ){ return c[i]; });

    RandomAccessCont tempCont(std::size(indexes));
    boost::copy(indexedValuesR, std::begin(tempCont));
    c = std::move(tempCont);
  }
}

//********************************************************************************
void Reorderer::reorderCloud(ParticleCloud& cloud)
//********************************************************************************
{
    static auto reorTimerID = Statistics::createTimer("Reorderer::reorderTimer");
    Statistics::TimerGuard reorderTimerGuard(reorTimerID);

    //if (cloud.empty())
    //  return;

    const auto& particlePos = cloud.get<Attr::ePosition>();

    glm::dvec2 minPos = glm::dvec2(std::numeric_limits<double>::max());
    for (const auto& iPos : particlePos ) {
      if (minPos.x > iPos.x)
        minPos.x = iPos.x;
      if (minPos.y > iPos.y)
        minPos.y = iPos.y;
    }

    auto calcZIndex = [&minPos](const glm::dvec2& pos) {
      const glm::uvec2 gridIndex = floor((pos-minPos)*Kernel::SmoothingLength::dh);
      return Util::ZValue(gridIndex.x, gridIndex.y);
    };

    auto zValR = particlePos | transformed(calcZIndex);

    std::vector<unsigned> zVal(zValR.begin(), zValR.end());

    std::vector<unsigned> indexes(cloud.size());
    std::iota(indexes.begin(), indexes.end(), 0);

    std::sort(indexes.begin(),indexes.end(), [&zVal](const unsigned& i, const unsigned& j) {
      return zVal[i]<zVal[j];
    });

    auto indexedReordered = [&indexes](auto& v){ ::indexedReordering(v, indexes); };
    cloud.applyFunctor(indexedReordered);
}


//////////////////////////////////////////////////////////////////////////////
///   Copyright 2003 and onward LASMEA UMR 6602 CNRS/U.B.P Clermont-Ferrand
///   Copyright 2009 and onward LRI    UMR 8623 CNRS/Univ Paris Sud XI
///
///          Maxributed under the Boost Software License, Version 1.0
///                 See accompanying file LICENSE.txt or copy at
///                     http://www.boost.org/LICENSE_1_0.txt
//////////////////////////////////////////////////////////////////////////////
#define NT2_UNIT_MODULE "nt2 complex.operator toolbox - max/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.operator components in scalar mode
//////////////////////////////////////////////////////////////////////////////
/// created  by jt the 18/02/2011
///
#include <nt2/toolbox/arithmetic/include/functions/max.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/functions/real.hpp>
#include <boost/type_traits/is_same.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <nt2/sdk/meta/as_signed.hpp>
#include <nt2/sdk/meta/upgrade.hpp>
#include <nt2/sdk/meta/downgrade.hpp>
#include <nt2/sdk/meta/scalar_of.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/type_traits/common_type.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>

#include <nt2/toolbox/constant/constant.hpp>
#include <nt2/sdk/meta/cardinal_of.hpp>
#include <nt2/include/functions/splat.hpp>

#include <nt2/include/functions/load.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/complex/dry.hpp>
#include <nt2/sdk/complex/imaginary.hpp>
#include <nt2/sdk/complex/meta/as_complex.hpp>
#include <nt2/sdk/complex/meta/as_imaginary.hpp>
#include <nt2/sdk/complex/meta/as_dry.hpp>
#include <nt2/include/functions/average.hpp>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/include/functions/extract.hpp>

NT2_TEST_CASE_TPL ( max_real__2_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::native;
  typedef NT2_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef std::complex<T>                              cT;
  typedef native<T ,ext_t>                             vT;
  typedef native<cT ,ext_t>                           vcT;
  typedef typename nt2::meta::as_imaginary<T>::type   ciT;
  typedef native<ciT ,ext_t>                         vciT;
  typedef typename nt2::meta::as_dry<T>::type          dT;
  typedef native<dT ,ext_t>                           vdT;
   double ulpd;
   ulpd=0.0;

   // specific values tests
   {
     typedef vcT r_t;
     NT2_TEST_ULP_EQUAL(nt2::max(vcT(nt2::Inf<vT>(), nt2::Zero<vT>()), vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Inf<vT>(), nt2::Zero<vT>())[0], 0);
     NT2_TEST_EQUAL(nt2::max(vcT(nt2::One<vT>(), nt2::Zero<vT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::One<vT>(),nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vcT(nt2::Zero<vT>(),nt2::Zero<vT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vcT(nt2::Zero<vT>(),nt2::One<vT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::One<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vcT(nt2::One<vT>(), nt2::Zero<vT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::One<vT>(), nt2::Zero<vT>())[0]);

     NT2_TEST_EQUAL(nt2::max(vciT(nt2::Inf<vciT>()), vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(), nt2::Inf<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::One<vciT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::One<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::Zero<vciT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::Zero<vciT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::One<vT>(),nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::One<vciT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(), nt2::One<vT>())[0]);

     NT2_TEST_EQUAL(nt2::max(vdT(nt2::Inf<vdT>()),  vcT(nt2::Inf<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::Inf<vT>(), nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::One<vdT>()),  vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::One<vT>(),nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::Zero<vdT>()), vcT(nt2::Zero<vT>(),nt2::Zero<vT>()))[0], vcT(nt2::Zero<vT>(),nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::Zero<vdT>()), vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::One<vT>(),nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::One<vdT>()),  vcT(nt2::One<vT>(), nt2::Zero<vT>()))[0], vcT(nt2::One<vT>(), nt2::Zero<vT>())[0]);

     NT2_TEST_EQUAL(nt2::max(nt2::Inf<vdT>(),   nt2::Inf<vciT>())[0] , vcT(nt2::Zero<vT>() ,  nt2::Inf<vT>())[0] );
     NT2_TEST_EQUAL(nt2::max(nt2::One<vdT>(),   nt2::Zero<vciT>())[0], vcT(nt2::One<vT>(),  nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(nt2::Zero<vdT>(),  nt2::Zero<vciT>())[0], vcT(nt2::Zero<vT>(),  nt2::Zero<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(nt2::Zero<vdT>(),  nt2::One<vciT>())[0] , vcT(nt2::Zero<vT>(),  nt2::One<vT>())[0]);
     NT2_TEST_EQUAL(nt2::max(nt2::One<vdT>(),   nt2::One<vciT>())[0] , vcT(nt2::Zero<vT>(),  nt2::One<vT>())[0]);
   }
   {
     typedef vciT r_t;
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::Inf<vciT>()),   vciT(nt2::Inf<vciT>()))[0],  vciT(nt2::Inf<vciT>() )[0]);
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::One<vciT>()),   vciT(nt2::Zero<vciT>()))[0], vciT(nt2::One<vciT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::Zero<vciT>()),  vciT(nt2::Zero<vciT>()))[0], vciT(nt2::Zero<vciT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::Zero<vciT>()),  vciT(nt2::One<vciT>()))[0],  vciT(nt2::One<vciT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vciT(nt2::One<vciT>()),   vciT(nt2::One<vciT>()))[0],  vciT(nt2::One<vciT>() )[0]);
   }
   {
     typedef vdT r_t;
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::Inf<vdT>()),   vdT(nt2::Inf<vdT>()))[0],  vdT(nt2::Inf<vdT>() )[0]);
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::One<vdT>()),   vdT(nt2::Zero<vdT>()))[0], vdT(nt2::One<vdT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::Zero<vdT>()),  vdT(nt2::Zero<vdT>()))[0], vdT(nt2::Zero<vdT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::Zero<vdT>()),  vdT(nt2::One<vdT>()))[0],  vdT(nt2::One<vdT>())[0]);
     NT2_TEST_EQUAL(nt2::max(vdT(nt2::One<vdT>()),   vdT(nt2::One<vdT>()))[0],  vdT(nt2::One<vdT>() )[0]);
   }
} // end of test for floating_


//  (C) Copyright Jeremy Siek 2004
//  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 <string>
#include <iostream>
#include <boost/cstdlib.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/subgraph.hpp>

int main()
{
    using namespace boost;
    using std::string;

    typedef adjacency_list< vecS, vecS, directedS, no_property,
        property< edge_index_t, int >, property< graph_name_t, string > >
        graph_t;

    graph_t g;
    get_property(g, graph_name) = "graph";

    std::cout << "name: " << get_property(g, graph_name) << std::endl;

    typedef subgraph< graph_t > subgraph_t;

    subgraph_t sg;
    get_property(sg, graph_name) = "subgraph";

    std::cout << "name: " << get_property(sg, graph_name) << std::endl;

    return exit_success;
}