#include "stdafx.h"
#include "qmath.h"
#include "config.h"

#include <vector>
#include <ctime>
#include <sstream>
#include <iostream>
#include <boost/test/unit_test.hpp>

namespace boost
{
	void throw_exception(class std::exception const &e)
	{
		throw e;
	}
}

using namespace math;

#ifdef NDEBUG
static const int TIMES = 1000;
#else
static const int TIMES = 10;
#endif

static const int COUNT = 1024;

// #define BOOST_AUTO_TEST_CASE(X) void X()
// #define BOOST_FAIL(X)
// #define BOOST_CHECK(X)

	std::vector<vec4f> vec4_src(COUNT);
	std::vector<vec4f> vec4_ref(COUNT);
	std::vector<vec4f> vec4_dst1(COUNT);
	std::vector<vec4f> vec4_dst2(COUNT);

	std::vector<vec3f> vec3_src(COUNT);
	std::vector<vec3f> vec3_ref(COUNT);
	std::vector<vec3f> vec3_dst1(COUNT);
	std::vector<vec3f> vec3_dst2(COUNT);

	std::vector<vec3f> mat4_vec3_src(COUNT);
	std::vector<vec3f> mat4_vec3_ref(COUNT);
	std::vector<vec3f> mat4_vec3_dst1(COUNT);
	std::vector<vec3f> mat4_vec3_dst2(COUNT);

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_MAT4_VEC4_1)
	{
#ifdef MATH_USE_SIMD
		std::cout << "SIMD" << std::endl;
#else
		std::cout << "SCALAR" << std::endl;
#endif
		std::cout << COUNT << "x" << TIMES << std::endl;

// 		size_t al = std::alignment_of<vec4<float>>();
// 		al = std::alignment_of<vec4f>();
// 		al = __alignof(vec4<float>);
// 		al = __alignof(vec4f);
// 		al = __alignof(std::max_align_t);

		for (uint32_t i = 0; i < COUNT; i++)
		{
			vec4_src[i] = vec4f(vec4s32(i % 7, i % 23 - 100, i / 5, i / 32));
		}
		for (uint32_t i = 0; i < COUNT; i++)
		{
			vec3_src[i] = vec3f(vec3s32(i % 7, i % 23 - 100, i / 5));
		}

		mat4f mat(quat_axis_z(1.27f).get_as_mat3());

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					auto const& vect = vec4_src[i];
					vec4_ref[i].set(
						vect.x*mat.m[0] + vect.y*mat.m[4] + vect.z*mat.m[ 8] + vect.w*mat.m[12],
						vect.x*mat.m[1] + vect.y*mat.m[5] + vect.z*mat.m[ 9] + vect.w*mat.m[13],
						vect.x*mat.m[2] + vect.y*mat.m[6] + vect.z*mat.m[10] + vect.w*mat.m[14],
						vect.x*mat.m[3] + vect.y*mat.m[7] + vect.z*mat.m[11] + vect.w*mat.m[15]
					);
				}
			}
			std::cout << "scalar mat4*vec4 " << clock() - start << std::endl;
		}

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4_dst2[i] = math::transform(mat, vec4_src[i]);
				}
			}
			std::cout << "math::transform mat4*vec4 " << clock() - start << std::endl;
		}

		BOOST_CHECK(vec4_ref == vec4_dst2);
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_MAT4_VEC4_2)
	{
		mat4f mat;
		mat = quat_axis_z(1.27f).get_as_mat3();

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				math::batch::transform(mat, vec4_dst2.data(), sizeof(vec4f), vec4_src.data(), sizeof(vec4f), COUNT);
			}
			std::cout << "math::batch::transform vec4 " << clock() - start << std::endl;
		}

		BOOST_CHECK(vec4_ref == vec4_dst2);
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_MAT4_VEC3)
	{
		for (uint32_t i = 0; i < COUNT; i++)
		{
			mat4_vec3_src[i] = vec3f(vec3s32(i % 7, i % 23 - 100, i / 5));
		}

		mat4f mat;
		mat = quat_axis_z(1.27f).get_as_mat3();

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					mat4_vec3_dst1[i] = math::transform(mat, mat4_vec3_src[i]);
				}
			}
			std::cout << "math::transform vec3 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				math::batch::transform(mat, mat4_vec3_dst2.data(), sizeof(vec3f), mat4_vec3_src.data(), sizeof(vec3f), COUNT);
			}
			std::cout << "math::batch::transform vec3 " << clock() - start << std::endl;
		}

		for (uint32_t i = 0; i < mat4_vec3_dst1.size(); i++)
		{
			if (mat4_vec3_dst1[i] != mat4_vec3_dst2[i])
			{
				BOOST_FAIL("Different transform product");
				break;
			}
		}
		BOOST_CHECK(mat4_vec3_dst1 == mat4_vec3_dst2);
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_MAT4_MAT4)
	{
		std::vector<mat4f> src(COUNT);
		std::vector<mat4f> dst1(COUNT);
		std::vector<mat4f> dst2(COUNT);

		for (uint32_t i = 0; i < COUNT; i++)
		{
			auto mat = quat_axis_x(1.27f).get_as_mat3();
			src[i] = mat;
		}

		mat4f mat;
		mat = quat_axis_z(1.27f).get_as_mat3();

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES/10; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					const float* m1 = mat.m;
					const float* m2 = src[i].m;
					float* m3 = dst2[i].m;

					// using this code only on release and when on the iphone
					m3[0] = m1[0]*m2[0] + m1[4]*m2[1] + m1[8]*m2[2] + m1[12]*m2[3];
					m3[1] = m1[1]*m2[0] + m1[5]*m2[1] + m1[9]*m2[2] + m1[13]*m2[3];
					m3[2] = m1[2]*m2[0] + m1[6]*m2[1] + m1[10]*m2[2] + m1[14]*m2[3];
					m3[3] = m1[3]*m2[0] + m1[7]*m2[1] + m1[11]*m2[2] + m1[15]*m2[3];

					m3[4] = m1[0]*m2[4] + m1[4]*m2[5] + m1[8]*m2[6] + m1[12]*m2[7];
					m3[5] = m1[1]*m2[4] + m1[5]*m2[5] + m1[9]*m2[6] + m1[13]*m2[7];
					m3[6] = m1[2]*m2[4] + m1[6]*m2[5] + m1[10]*m2[6] + m1[14]*m2[7];
					m3[7] = m1[3]*m2[4] + m1[7]*m2[5] + m1[11]*m2[6] + m1[15]*m2[7];

					m3[8] = m1[0]*m2[8] + m1[4]*m2[9] + m1[8]*m2[10] + m1[12]*m2[11];
					m3[9] = m1[1]*m2[8] + m1[5]*m2[9] + m1[9]*m2[10] + m1[13]*m2[11];
					m3[10] = m1[2]*m2[8] + m1[6]*m2[9] + m1[10]*m2[10] + m1[14]*m2[11];
					m3[11] = m1[3]*m2[8] + m1[7]*m2[9] + m1[11]*m2[10] + m1[15]*m2[11];

					m3[12] = m1[0]*m2[12] + m1[4]*m2[13] + m1[8]*m2[14] + m1[12]*m2[15];
					m3[13] = m1[1]*m2[12] + m1[5]*m2[13] + m1[9]*m2[14] + m1[13]*m2[15];
					m3[14] = m1[2]*m2[12] + m1[6]*m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
					m3[15] = m1[3]*m2[12] + m1[7]*m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
				}
			}
			std::cout << "scalar mat4f * mat4f " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES/10; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					multiply(dst1[i], mat, src[i]);
				}
			}
			std::cout << "math::multiply mat4f " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES/10; x++)
			{
				math::batch::multiply(mat, dst1.data(), sizeof(math::mat4f), src.data(), sizeof(math::mat4f), COUNT);
			}
			std::cout << "math::batch::multiply mat4f " << clock() - start << std::endl;
		}
		BOOST_CHECK(dst1 == dst2);
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_VEC4_VEC4)
	{
		std::vector<vec4f> src(COUNT);
		std::vector<vec4f> dst1(COUNT);
		std::vector<vec4f> dst2(COUNT);

		for (uint32_t i = 0; i < COUNT; i++)
		{
			src[i] = vec4f(vec4s32(i % 7, i % 23 - 100, i / 5, i / 32));
		}

		vec4f xxx(12.f, 4.f, 7.f, 1.3f);

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4f const& v = src[i];
					float x = v.x;
					float y = v.y;
					float z = v.z;
					float w = v.w;
					dst2[i] = vec4f(x * xxx.x, y * xxx.y, z * xxx.z, w * xxx.w);
				}
			}
			std::cout << "scalar vec4*vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					dst1[i] = src[i] * xxx;
				}
			}
			std::cout << "math vec4*vec4 " << clock() - start << std::endl;
		}

		BOOST_CHECK(dst1 == dst2);
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_VEC4_LERP_VEC4)
	{
		vec4f xxx(12.f, 4.f, 7.f, 1.3f);

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					float t = 0.72f;
					vec4f const& v = vec4_src[i];
					float x = v.x;
					float y = v.y;
					float z = v.z;
					float w = v.w;
					vec4_dst2[i] = vec4f(x + (xxx.x - x)*t, y + (xxx.y - y)*t, z + (xxx.z - z)*t, w + (xxx.w - w)*t);
				}
			}
			std::cout << "scalar vec4 lerp vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4_dst1[i] = math::lerp(vec4_src[i], xxx, 0.72f);
				}
			}
			std::cout << "math vec4 lerp vec4 " << clock() - start << std::endl;
		}

		for (uint32_t i = 0; i < COUNT; i++)
		{
			BOOST_CHECK(equals(vec4_dst1[i], vec4_dst2[i], 0.001f));
		}
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_CLAMP_VEC4)
	{
		vec4f xxx(12.f, 4.f, 7.f, 1.3f);

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4f const& v = vec4_src[i];
					float x = v.x;
					float y = v.y;
					float z = v.z;
					float w = v.w;
					vec4_dst2[i] = vec4f(math::clamp(x, -3.f, 2.23f), math::clamp(y, -3.f, 2.23f), math::clamp(z, -3.f, 2.23f), math::clamp(w, -3.f, 2.23f));
				}
			}
			std::cout << "scalar clamp vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				math::vec4f min(-3.f);
				math::vec4f max(2.23f);
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4_dst1[i] = math::clamp(vec4_src[i], min, max);
				}
			}
			std::cout << "math clamp vec4 " << clock() - start << std::endl;
		}

		for (uint32_t i = 0; i < COUNT; i++)
		{
			if (vec4_dst1[i] != vec4_dst2[i])
			{
				int a = 0;
			}
			BOOST_CHECK(vec4_dst1[i] == vec4_dst2[i]);
		}
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_ABS_VEC4)
	{
		vec4f xxx(12.f, 4.f, 7.f, 1.3f);

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4f const& v = vec4_src[i];
					float x = v.x;
					float y = v.y;
					float z = v.z;
					float w = v.w;
					vec4_dst2[i] = vec4f(std::abs(x), std::abs(y), std::abs(z), std::abs(w));
				}
			}
			std::cout << "scalar abs vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4_dst1[i] = math::abs(vec4_src[i]);
				}
			}
			std::cout << "math abs vec4 " << clock() - start << std::endl;
		}

		BOOST_CHECK(vec4_dst1 == vec4_dst2);
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_VEC4_DOT)
	{
		std::vector<vec4f> src(COUNT);
		std::vector<float> dst1(COUNT);
		std::vector<float> dst2(COUNT);

		for (uint32_t i = 0; i < COUNT; i++)
		{
			src[i] = vec4f(vec4s32(i % 7, i % 23 - 100, i / 5, i / 32));
		}

		vec4f xxx(12.f, 4.f, 7.f, 1.3f);

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4f const& v = src[i];
					float x = v.x;
					float y = v.y;
					float z = v.z;
					float w = v.w;
					dst2[i] = xxx.x*x + xxx.y*y + xxx.z*z + xxx.w*w;
				}
			}
			std::cout << "scalar dot vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					dst1[i] = math::dot(src[i], xxx);
				}
			}
			std::cout << "math dot vec4 " << clock() - start << std::endl;
		}

		for (uint32_t i = 0; i < dst1.size(); i++)
		{
			if (!math::equals(dst1[i], dst2[i], 0.0001f))
			{
				BOOST_FAIL("Different dot product");
				break;
			}
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				math::batch::dot(dst1.data(), sizeof(float), src.data(), sizeof(math::vec4f), &xxx, 0, COUNT);
			}
			std::cout << "math::batch dot vec4 " << clock() - start << std::endl;
		}

		for (uint32_t i = 0; i < dst1.size(); i++)
		{
			if (!math::equals(dst1[i], dst2[i], 0.0001f))
			{
				BOOST_FAIL("Different dot product");
				break;
			}
		}
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_VEC4_DISTANCE)
	{
		std::vector<vec4f> src(COUNT);
		std::vector<float> dst1(COUNT);
		std::vector<float> dst2(COUNT);

		for (uint32_t i = 0; i < COUNT; i++)
		{
			src[i] = vec4f(vec4s32(i % 7, i % 23 - 100, i / 5, i / 32));
		}

		vec4f xxx(12.f, 4.f, 7.f, 1.3f);

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4f const& v = src[i];
					float x = v.x-xxx.x;
					float y = v.y-xxx.y;
					float z = v.z-xxx.z;
					float w = v.w-xxx.w;
					dst2[i] = math::sqrt(x*x + y*y + z*z + w*w);
				}
			}
			std::cout << "scalar distance vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					dst1[i] = math::distance(src[i], xxx);
				}
			}
			std::cout << "math distance vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					dst1[i] = math::distance<float, math::fast>(src[i], xxx);
				}
			}
			std::cout << "math fast distance vec4 " << clock() - start << std::endl;
		}
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_VEC4_LENGTH)
	{
		std::vector<vec4f> src(COUNT);
		std::vector<float> dst1(COUNT);
		std::vector<float> dst2(COUNT);

		for (uint32_t i = 0; i < COUNT; i++)
		{
			src[i] = vec4f(vec4s32(i % 7, i % 23 - 100, i / 5, i / 32));
		}

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4f const& v = src[i];
					float x = v.x;
					float y = v.y;
					float z = v.z;
					float w = v.w;
					dst2[i] = math::sqrt(x*x + y*y + z*z + w*w);
				}
			}
			std::cout << "scalar length vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					dst1[i] = math::length(src[i]);
				}
			}
			std::cout << "math length vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					dst1[i] = math::length<float, math::fast>(src[i]);
				}
			}
			std::cout << "math fast length vec4 " << clock() - start << std::endl;
		}
	}

	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_VEC4_NORMALIZE)
	{
		vec4f xxx(12.f, 4.f, 7.f, 1.3f);

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4f const& v = vec4_src[i];
					float x = v.x;
					float y = v.y;
					float z = v.z;
					float w = v.w;
					float d = 1.f / math::sqrt(x*x + y*y + z*z + w*w);
					vec4_dst2[i].set(x*d, y*d, z*d, w*d);
				}
			}
			std::cout << "scalar normalize vec4 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4_dst1[i] = math::normalized(vec4_src[i]);
				}
			}
			std::cout << "math normalize vec4 " << clock() - start << std::endl;
		}
		for (uint32_t i = 0; i < vec4_dst1.size(); i++)
		{
			if (!math::equals(vec4_dst1[i], vec4_dst2[i], 0.0001f))
			{
				BOOST_FAIL("Different normalize");
				break;
			}
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec4_dst1[i] = math::normalized<float, math::fast>(vec4_src[i]);
				}
			}
			std::cout << "math fast normalize vec4 " << clock() - start << std::endl;
		}
	}
	//////////////////////////////////////////////////////////////////////////

	BOOST_AUTO_TEST_CASE(BENCHMARK_VEC3_NORMALIZE)
	{
		vec3f xxx(12.f, 4.f, 7.f);

		std::cout << std::endl;

		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec3f const& v = vec3_src[i];
					float x = v.x;
					float y = v.y;
					float z = v.z;
					float d = 1.f / math::sqrt(x*x + y*y + z*z);
					vec3_dst2[i].set(x*d, y*d, z*d);
				}
			}
			std::cout << "scalar normalize vec3 " << clock() - start << std::endl;
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec3_dst1[i] = math::normalized(vec3_src[i]);
				}
			}
			std::cout << "math normalize vec3 " << clock() - start << std::endl;
		}
		for (uint32_t i = 0; i < vec3_dst1.size(); i++)
		{
			if (!math::equals(vec3_dst1[i], vec3_dst2[i], 0.0001f))
			{
				BOOST_FAIL("Different normalize");
				break;
			}
		}
		{
			std::clock_t start = std::clock();
			for (uint32_t x = 0; x < TIMES; x++)
			{
				for (uint32_t i = 0; i < COUNT; i++)
				{
					vec3_dst1[i] = math::normalized<float, math::fast>(vec3_src[i]);
				}
			}
			std::cout << "math fast normalize vec3 " << clock() - start << std::endl;
		}
	}


#ifndef _PCL_CYTHON_EIGEN_HPP
#define _PCL_CYTHON_EIGEN_HPP

#include <Eigen/Core>
#include <Eigen/Geometry>

template <typename T, typename U> T _toEigen(U *input)
{
    return Eigen::Map<T>(input);
}

Eigen::Affine3f _toEigenAffine(float *input)
{
    Eigen::Affine3f ret;
    ret.matrix() = Eigen::Map<Eigen::Matrix4f>(input);
    return ret;
}

#endif // _PCL_CYTHON_EIGEN_HPP


#include "simple_cfl.hpp"
#include "hydrodynamics_2d.hpp"
#include <boost/foreach.hpp>
#ifdef RICH_MPI
#include <mpi.h>
#endif

SimpleCFL::SimpleCFL(const double cfl): cfl_(cfl) {}

namespace {
  class TimeStepCalculator: public LazyList<double>
  {
  public:

    TimeStepCalculator(const Tessellation& tess,
		       const vector<ComputationalCell>& cells,
		       const EquationOfState& eos,
		       const vector<Vector2D>& edge_velocities,
		TracerStickerNames const& tracerstickernames):
      tess_(tess), cells_(cells),
      edge_velocities_(edge_velocities), eos_(eos), tracerstickernames_(tracerstickernames){}

    size_t size(void) const
    {
		return static_cast<size_t>(tess_.GetPointNo());
    }

    double operator[](size_t i) const
    {
      double res = 0;
      const double radius = tess_.GetWidth(static_cast<int>(i));
      const double c = eos_.dp2c
	(cells_[i].density,
	 cells_[i].pressure,
	 cells_[i].tracers,
		tracerstickernames_.tracer_names);
      const Vector2D v = cells_.at(i).velocity;
      BOOST_FOREACH
	(int index,
	 tess_.GetCellEdges(static_cast<int>(i))){
	const Vector2D ve = edge_velocities_.at(static_cast<size_t>(index));
	res = fmax
	  (res,
	   (c+abs(v-ve))/radius);
      }
      return 1.0/res;
    }

  private:
    const Tessellation& tess_;
    const vector<ComputationalCell>& cells_;
    const vector<Vector2D>& edge_velocities_;
    const EquationOfState& eos_;
	TracerStickerNames const& tracerstickernames_;
  };
}

double SimpleCFL::operator()(const Tessellation& tess,
			     const vector<ComputationalCell>& cells,
			     const EquationOfState& eos,
			     const vector<Vector2D>& point_velocities,
			     const double /*time*/,TracerStickerNames const& tracerstickernames) const
{
  double res =  cfl_*lazy_min(TimeStepCalculator(tess,cells,eos,point_velocities,tracerstickernames));
#ifdef RICH_MPI
  MPI_Allreduce(&res, &res, 1, MPI_DOUBLE, MPI_MIN, MPI_COMM_WORLD);
#endif
  return res;
}


//
// Created by xetql on 09.03.18.
//

#ifndef LIBLJ_FITTEDMLPAPPROXIMATOR_HPP
#define LIBLJ_FITTEDMLPAPPROXIMATOR_HPP

#include <mlpack/core.hpp>

#include <mlpack/core/optimizers/rmsprop/rmsprop.hpp>
#include <mlpack/core/optimizers/sgd/update_policies/vanilla_update.hpp>
#include <mlpack/methods/ann/layer/layer.hpp>
#include <mlpack/methods/ann/ffn.hpp>

#include <armadillo>

#include <type_traits>
#include <vector>

#include "FunctionApproximator.hpp"

namespace librl {
namespace approximator {
namespace action_value {
/**
 * Default implementation of Q Function Approximation through NeuralNet. Actions are modeled as list with indexes
 * @tparam OptimizerType Type of the optimizer used for training the NeuralNet
 * @tparam ParametersType MLPACK Inner Neural Net types, see mlpack.
 */
template<typename OptimizerType, class... ParametersType>
class MLP : public ActionValueApproximator<arma::mat, int> {
protected:
    std::unique_ptr<mlpack::ann::FFN<ParametersType... >> model;
    OptimizerType opt;
public:
    MLP(OptimizerType opt, ParametersType... params) : opt(opt), ActionValueApproximator<arma::mat, int>(opt.Alpha()) {
        model = std::move(std::make_unique<mlpack::ann::FFN<ParametersType...>>(params...));
    }
    MLP(OptimizerType opt) : opt(opt), ActionValueApproximator<arma::mat, int>(opt.Alpha()) {
        model = std::move(std::make_unique<mlpack::ann::FFN<ParametersType...>>());
    }

    template<class LayerType, class... Args>
    void add_layer(Args... args) {
        model->template Add<LayerType>(args...);
    }

    virtual void reset() { this->reset_model(); }

    virtual void reset_model(ParametersType... params) noexcept {
        this->model.reset(new mlpack::ann::FFN<ParametersType...>(params...));
    }
    virtual void reset_model() noexcept {
        this->model.reset(new mlpack::ann::FFN<ParametersType...>());
    }
    virtual void set_model(mlpack::ann::FFN<ParametersType...> *model) noexcept {
        this->model.reset(model);
    }

    /**
     * Return the output neuron that returns the highest response value
     * @param state The entry state (input)
     * @param available_actions The available actions, "output neurons"
     * @return action that returns the highest response value
     */
    virtual int argmax(const arma::mat& state, const std::vector<int>& available_actions = std::vector<int>() ) const {
        arma::mat responses;
        const_cast<mlpack::ann::FFN<ParametersType...>*>(model.get())->Predict(state, responses);//I thank a lot mlpack devs. for the non cost function.
        return responses.index_max();
    }

    /**
     * Train the neural net to match the output response (value) for a given action (output neuron)
     * @param state The entry state (input)
     * @param action The id of the output neuron
     * @param value The response value
     */
    virtual void Q(const arma::mat& state, const int& action, double value) {
        arma::mat responses;
        model->Predict(state, responses);
        responses(action) = value;
        model->Train(state, responses, opt);
    }

    /**
     * Return the highest response value.
     * @param state The entry state (input)
     * @return The highest output response
     */
    virtual double max(const arma::mat& state) const {
        arma::mat responses;
        const_cast<mlpack::ann::FFN<ParametersType...>*>(model.get())->Predict(state, responses);//I thank a lot mlpack devs. for the non const function.
        return arma::max(responses).max();
    }

    /**
     * Get the response value of a given action.
     * @param state The entry state (input)
     * @param action The output neuron of interest
     * @return The output response
     */
    virtual double Q(const arma::mat& state, const int& action) const {
        arma::mat responses;
        const_cast<mlpack::ann::FFN<ParametersType...>*>(model.get())->Predict(state, responses);//I thank a lot mlpack devs. for the non const function.
        return responses(action);
    }
};

}}}

#endif //LIBLJ_FITTEDMLPAPPROXIMATOR_HPP


/*
 *  Copyright 2015 Aldebaran. All rights reserved.
 *
 */

#include <almath/scenegraph/urdfeigen.h>
#include <gtest/gtest.h>
#include <boost/math/constants/constants.hpp>
#include <boost/property_tree/ptree.hpp>

using namespace AL;
typedef AL::urdf::ptree ptree;
const double pi = boost::math::constants::pi<double>();

#define EXPECT_ARRAY3D_EQ(expected, actual)   \
  if (1) {                                    \
    const double eps = 1e-7;                  \
    EXPECT_NEAR(expected[0], actual[0], eps); \
    EXPECT_NEAR(expected[1], actual[1], eps); \
    EXPECT_NEAR(expected[2], actual[2], eps); \
  }

#define EXPECT_IS_APPROX(expected, actual)                           \
  if (1) {                                                           \
    EXPECT_TRUE((expected.matrix() - actual.matrix()).isZero(1e-10)) \
        << "expected:\n" << (expected).matrix() << "\nactual\n"      \
        << (actual).matrix() << "\ndiff\n"                           \
        << (expected.matrix() - actual.matrix()) << "\n";            \
  }

class RotationRPY : public ::testing::TestWithParam<
                        std::tr1::tuple<double, double, double> > {
 public:
  double r, p, y;
  virtual void SetUp() {
    r = std::tr1::get<0>(GetParam());
    p = std::tr1::get<1>(GetParam());
    y = std::tr1::get<2>(GetParam());
  }
};

// check that fromUrdfRpy(toUrdfRpy(x)) is idempotent
TEST_P(RotationRPY, rotation) {
  urdf::Array3d rpy = {{r, p, y}};
  typedef double Scalar;
  {
    typedef Eigen::Quaternion<Scalar, Eigen::DontAlign> T;
    T t = Math::eigenQuaternionFromUrdfRpy(rpy);
    urdf::Array3d back = Math::urdfRpyFromEigenMatrix3(t.matrix());
    EXPECT_IS_APPROX(t, Math::eigenQuaternionFromUrdfRpy(back))
  }
  {
    typedef Eigen::Matrix<Scalar, 3, 3, Eigen::DontAlign> T;
    T t(Math::eigenQuaternionFromUrdfRpy(rpy));
    urdf::Array3d back = Math::urdfRpyFromEigenMatrix3(t);
    EXPECT_IS_APPROX(t, T(Math::eigenQuaternionFromUrdfRpy(back)))
  }
  {
    typedef Eigen::Transform<Scalar, 3, Eigen::AffineCompact, Eigen::DontAlign>
        T;
    T t(Math::eigenQuaternionFromUrdfRpy(rpy));
    urdf::Array3d back = Math::urdfRpyFromEigenMatrix3(t.linear());
    EXPECT_IS_APPROX(t, T(Math::eigenQuaternionFromUrdfRpy(back)))
  }
}

// INSTANTIATE_TEST_CASE_P(Many, RotationRPY, ::testing::Combine(
//    testing::Range(-pi, pi, pi/6),
//    testing::Range(-pi, pi, pi/6),
//    testing::Range(-pi, pi, pi/6)));

INSTANTIATE_TEST_CASE_P(
    Some, RotationRPY,
    testing::Values(std::tr1::make_tuple(0., -pi / 2, 0.),
                    std::tr1::make_tuple(0., -pi / 2, pi / 3),
                    std::tr1::make_tuple(pi * 30 / 180, -pi / 2, pi * 75 / 180),
                    std::tr1::make_tuple(-pi / 3, pi / 2, pi / 3),
                    std::tr1::make_tuple(-3 * pi / 12, pi / 2, pi / 3),
                    std::tr1::make_tuple(pi / 3, pi / 2, pi / 2)));

TEST(UrdfEigen, pose) {
  typedef double Scalar;
  typedef Eigen::Transform<Scalar, 3, Eigen::AffineCompact, Eigen::DontAlign> T;
  urdf::Pose p{{{1, 2.2, 3.3}}, {{0, -pi / 2, 0}}};

  T t = Math::toEigenTransform(p);
  EXPECT_EQ(1, t(0, 3));
  EXPECT_EQ(2.2, t(1, 3));
  EXPECT_EQ(3.3, t(2, 3));

  EXPECT_IS_APPROX(Eigen::AngleAxisd(-pi / 2, Eigen::Vector3d::UnitY()),
                   t.linear());
}


/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2012 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#ifndef NUM_SUM_HPP
#define NUM_SUM_HPP

#include <boost/shared_ptr.hpp>
#include "states_patterns.hpp"

namespace phycas
{

/*----------------------------------------------------------------------------------------------------------------------
|   Implements numerically-stable summation in which largest element of the sum is factored out and the sum returned
|   on the log scale. Used extensively in the stepping-stone and inflated density ratio methods for estimating the
|   marginal likelihood.
*/
class NumSum
	{
	public:
                    NumSum();
                    ~NumSum();

        void        clear();
        void        addValueToSum(double log_value);
        unsigned    getNSummed() const;
        double      getLogSum() const;
        double      getLogSumOfSquares() const;
        double      getMaxLogValue() const;

	private:

        double      _max_log_value;         /**< the log of the largest value in the sum */
        double      _curr_sum;              /**< the current sum of exp(log_value - `max_log_value') */
        double      _curr_sum_of_squares;   /**< the current sum of exp(log_value - `max_log_value') */
        unsigned    _num_summed;            /**< the number of elements added to `_curr_sum' */
    };

typedef boost::shared_ptr<NumSum> NumSumShPtr;

}	// namespace phycas

#endif


#ifndef ASLAM_BACKEND_DV_ROTATION_HPP
#define ASLAM_BACKEND_DV_ROTATION_HPP

#include <aslam/backend/JacobianContainer.hpp>
#include <sm/kinematics/quaternion_algebra.hpp>
#include <boost/shared_ptr.hpp>
#include <set>
#include <aslam/backend/TransformationExpressionNode.hpp>

namespace aslam {
  namespace backend {
    class ExpressionNodeVisitor;

    /**
     * \class RotationExpressionNode
     * \brief The superclass of all classes representing rotations.
     */
    class RotationExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      RotationExpressionNode();
      virtual ~RotationExpressionNode();

      /// \brief Evaluate the rotation matrix.
      EIGEN_ALWAYS_INLINE Eigen::Matrix3d evaluate() const { return toRotationMatrixImplementation(); }
      EIGEN_ALWAYS_INLINE Eigen::Matrix3d toRotationMatrix() const { return toRotationMatrixImplementation(); }

      /// \brief Evaluate the Jacobians
      EIGEN_ALWAYS_INLINE void evaluateJacobians(JacobianContainer & outJacobians) const { evaluateJacobiansImplementation(outJacobians); }

      /// \brief Evaluate the Jacobians and apply the chain rule.
      /** The chain rule matrix is assumed to be calculated in the left exponential chart centered at the current value (Phi(w)=Phi_R(w):= exp(w) R)),
       * where exp(w) applied to 3x1 vectors is the matrix exponential of the skew symmetric matrix w^x,
       * such that, assuming that this RotataionExpression, R, is the argument of a function g:
       * J g\circ R = J (g \circ Phi \circ Phi^{-1} \circ R) = J( g \circ Phi) J( Phi^{-1} \circ R)
       * applyChainRule := J(g \circ Phi);
       *
       * This implies e.g. for the product A * B of two rotation expression that:
       * evaluateJacobiansImplementation assumes that
       * applyChainRule = J ( g * Phi_{A * B} } and does return
       * J_v,w ( Phi_{A*B}^{-1} \circ (Phi_A(v) * Phi_B(w))) = J_v,w ( log( (exp(v) A * exp(w)B B^{-1}A^{-1}) ))
       * such that
       * J_v = id and
       * J_w = J_w (log (A * exp(w) * A^{-1}) = log (exp(A * w)) = A
       */
      template <typename DERIVED>
      EIGEN_ALWAYS_INLINE void evaluateJacobians(JacobianContainer & outJacobians, const Eigen::MatrixBase<DERIVED> & applyChainRule) const {
        evaluateJacobians(outJacobians.apply(applyChainRule));
      }

      void getDesignVariables(DesignVariable::set_t & designVariables) const;

      virtual void accept(ExpressionNodeVisitor& visitor);  //TODO make pure and complete nodes
    protected:
      // These functions must be implemented by child classes.
      virtual Eigen::Matrix3d toRotationMatrixImplementation() const = 0;
      virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const = 0;
      virtual void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const = 0;
    };

    /**
     * \class ConstantRotationExpression
     *
     * \brief A class representing a constant rotation matrix.
     *
     */
    class ConstantRotationExpressionNode : public RotationExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      ConstantRotationExpressionNode(const Eigen::Matrix3d & C);
      ~ConstantRotationExpressionNode() override;

    private:
      Eigen::Matrix3d toRotationMatrixImplementation() const override;
      void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
      void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

      const Eigen::Matrix3d _C;
    };

    /**
     * \class RotationExpressionNodeMultiply
     *
     * \brief A class representing the multiplication of two rotation matrices.
     *
     */
    class RotationExpressionNodeMultiply : public RotationExpressionNode
    {
    public:
      EIGEN_MAKE_ALIGNED_OPERATOR_NEW

      RotationExpressionNodeMultiply(boost::shared_ptr<RotationExpressionNode> lhs,
				     boost::shared_ptr<RotationExpressionNode> rhs);
      ~RotationExpressionNodeMultiply() override;

    private:
      Eigen::Matrix3d toRotationMatrixImplementation() const override;
      void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
      void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

      boost::shared_ptr<RotationExpressionNode> _lhs;
      mutable Eigen::Matrix3d _C_lhs;
      boost::shared_ptr<RotationExpressionNode> _rhs;
      mutable Eigen::Matrix3d _C_rhs;
    };

    /**
     * \class RotationExpressionNodeInverse
     *
     * \brief A class representing the inverse of a rotation matrix.
     *
     */
    class RotationExpressionNodeInverse : public RotationExpressionNode
    {
    public:
      RotationExpressionNodeInverse(boost::shared_ptr<RotationExpressionNode> dvRotation);

      ~RotationExpressionNodeInverse() override;

    private:
      Eigen::Matrix3d toRotationMatrixImplementation() const override;
      void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
      void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

      boost::shared_ptr<RotationExpressionNode> _dvRotation;
      mutable Eigen::Matrix3d _C;
    };

    class RotationExpressionNodeTransformation : public RotationExpressionNode
    {
    public:
      RotationExpressionNodeTransformation(boost::shared_ptr<TransformationExpressionNode> dvRotation);

      ~RotationExpressionNodeTransformation() override;

    private:
      Eigen::Matrix3d toRotationMatrixImplementation() const override;
      void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const override;
      void getDesignVariablesImplementation(DesignVariable::set_t & designVariables) const override;

      boost::shared_ptr<TransformationExpressionNode> _transformation;
    };

  } // namespace backend
} // namespace aslam

#endif /* ASLAM_BACKEND_DV_ROTATION_HPP */


#pragma once

#include <deal.II/base/quadrature_lib.h>
#include <deal.II/base/utilities.h>
#include <deal.II/fe/fe_values.h>
#include <deal.II/numerics/vector_tools.h>
#include <iostream>

namespace boltzmann {

template <int dim, typename InVector>
double
compute_mean_value(const dealii::DoFHandler<dim>& dh, const InVector& v)
{
  dealii::QGauss<dim> quad(2);
  return dealii::VectorTools::compute_mean_value(dh, quad, v, 0);
}

namespace mpi {

template <int dim>
std::vector<double>
compute_mean_value(const dealii::DoFHandler<dim>& dh, const Epetra_MultiVector& src)
{
  unsigned int pid = dealii::Utilities::MPI::this_mpi_process(MPI_COMM_WORLD);
  dealii::QGauss<dim> quad(2);
  dealii::UpdateFlags flags =
      dealii::update_values | dealii::update_JxW_values | dealii::update_quadrature_points;
  dealii::FEValues<dim> fe_values(dh.get_fe(), quad, flags);

  int nvec = src.NumVectors();

  std::vector<double> lsum(nvec, 0.0);

  unsigned int dofs_per_cell = fe_values.dofs_per_cell;
  std::vector<unsigned int> local_dof_indices(dofs_per_cell);
  for (auto cell = dh.begin_active(); cell != dh.end(); ++cell) {
    if (cell->subdomain_id() == pid) {
      fe_values.reinit(cell);
      cell->get_dof_indices(local_dof_indices);
      for (int k = 0; k < nvec; ++k) {
        for (unsigned int ix = 0; ix < dofs_per_cell; ++ix) {
          int lid = src.Map().LID(int(local_dof_indices[ix]));
          BOOST_ASSERT(lid >= 0);
          // if (lid < 0) std::cout << "lid " << lid << " gid " << local_dof_indices[ix] << "\n";
          for (unsigned int q = 0; q < quad.size(); ++q) {
            lsum[k] += src[k][lid] * fe_values.shape_value(ix, q) * fe_values.JxW(q);
          }
        }
      }
    }
  }

  std::vector<double> sum(nvec, 0.0);
  MPI_Allreduce(lsum.data(), sum.data(), nvec, MPI_DOUBLE, MPI_SUM, MPI_COMM_WORLD);
  return sum;
}
}  // namespace mpi

}  // namespace boltzmann


/*!
  @file main.cpp
  @author Klaus K. Holst
  @copyright 2018-2021, Klaus Kähler Holst

  @brief Example file

*/

#include <armadillo>
#include <cstdio>  // remove
#include <algorithm>  // max
#include <target/target.hpp>
#include <target/utils.hpp>

using namespace arma;

int main(int argc, char **argv) {
  vec y, w, p;
  mat a, x1, x2;
  if (argc == 1) {
    std::cout << "* Simulating data...\n\n";
    arma_rng::set_seed(1);
    unsigned n = 5;
    y = vec(n, 1);
    y.randn();
    w = mat(n, 1);
    w.fill(1);
    a = randi<mat>(n, 1, distr_param(0, 1));
    x2 = mat(n, 2);
    x2.randn();
    x1 = mat(n, 1);
    x1.fill(1);
    p = vec(4);
    p.fill(0.5);
  } else {
    const char *infile = argv[1];
    std::cout << "* Reading from '" << infile << "'\n\n";
  }

  unsigned n = std::min(3, (int)y.n_elem);
  std::cout << " Response:\n"
            << target::BLUE << y.rows(0, n - 1)
            << "\t..." << target::COL_RESET
            << std::endl;
  std::cout << " Exposure:\n"
            << target::CYAN << a.rows(0, n - 1)
            << "\t..." << target::COL_RESET
            << std::endl;
  std::cout << " X1:\n"
            << target::YELLOW << x1.rows(0, n - 1) << "\t..."
            << target::COL_RESET << std::endl;
  std::cout << " X2:\n"
            << target::YELLOW << x2.rows(0, n - 1) << "\t..."
            << target::COL_RESET << std::endl;
  std::cout << "\n parameter: " << target::GREEN << p.t()
            << target::COL_RESET
            << std::endl;

  target::RD<double> model(y, a, x1, x2, x2, p, w);
  vec res = model.loglik();
  std::cout << " loglik=\n"
            << target::RED << res << std::endl
            << target::COL_RESET;

  mat U = model.score(true);
  std::cout << " score=\n"
            << target::RED << U << std::endl
            << target::COL_RESET;

  arma::vec alpha2(1); alpha2.fill(1);
  U = model.est(alpha2);
  std::cout << " U=\n" << target::RED << U <<
    std::endl << target::COL_RESET;

  //arma::mat pp = model.target::TargetBinary<double>::pa();
  //std::cout << "pp=\n" << pp << std::endl;
  // const char *filen = "tmp/a.h5";
  // std::remove(filen);
  // y.save(hdf5_name(filen, "y", hdf5_opts::append+hdf5_opts::trans));
  // a.save(hdf5_name(filen, "a", hdf5_opts::append+hdf5_opts::trans));
  // w.save(hdf5_name(filen, "w", hdf5_opts::append+hdf5_opts::trans));
  // x1.save(hdf5_name(filen, "x1", hdf5_opts::append+hdf5_opts::trans));
  // x2.save(hdf5_name(filen, "x2", hdf5_opts::append+hdf5_opts::trans));
  // p.save(hdf5_name(filen, "p", hdf5_opts::append+hdf5_opts::trans));

  return 0;
}


/**
 * PYTHON Wrapper for Clothoids
 *
 * License MIT - See LICENSE file
 * 2019 Matteo Ragni, Claudio Kerov Ghiglianovich,
 *      Enrico Bertolazzi, Marco Frego
 */

#pragma once

#ifdef LMSOLVE_CLOTHOID_SPLINE

#include "python-ClothoidSpline-Solver.hh"

#include <vector>
#include <Eigen/Eigen>
#include <unsupported/Eigen/LevenbergMarquardt>

namespace G2lib {
  namespace Interpolation {

    using G2lib::ClothoidSplineG2;
    using G2lib::int_type;
    using G2lib::real_type;
    typedef Eigen::SparseFunctor<real_type, int_type> SparseFunctor;

    /* Levemberg Marquardt Solver */
    class LMSolver : public Solver {
      struct ClothoidSplineProblem : SparseFunctor {
        LMSolver &             m_solver;
        std::vector<int_type>  m_jacobian_rows;
        std::vector<int_type>  m_jacobian_cols;
        std::vector<real_type> m_jacobian_result;

        ClothoidSplineProblem(LMSolver & solver);
        int operator()(const SparseFunctor::InputType & theta, SparseFunctor::ValueType & constraints_value) const;
        int df(const SparseFunctor::InputType & theta, SparseFunctor::JacobianType & jacobian_value);
      };

     public:
      LMSolver(const ClothoidSplineG2 & spline) : Solver(spline) {};
      virtual bool solve() override;
    };
  } /* namespace Interpolation */
} /* namespace G2lib */

#endif


#include <stdio.h>
#include <stdlib.h>
#include <cstdlib>
#include <time.h>
#include <math.h>
#include <dirent.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <iomanip>
#include <limits>
#include <string>
#include <algorithm>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <boost/algorithm/string/replace.hpp>
#include <boost/math/distributions/normal.hpp>

#include "opencv2/opencv.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/nonfree/features2d.hpp"
#include "opencv2/flann/flann.hpp"

#include "FastEMD/emd_hat.hpp"
#include "FastEMD/emd_hat_signatures_interface.hpp"
#include "RubnerEMD/emd.hpp"
#include "FastEMD/tictoc.hpp"

using namespace cv;
using namespace std;

namespace fs = boost::filesystem;

#define IMAGE_WIDTH 1000

// TODO: config
/* CONFIGURATIONS */
int keypoint_filter_threshold = 1;
bool filter_kp = 0;
int sort_type = -1;
bool show_images = 0;
/* END CONFIGURATIONS */

void help();
void find_images(const char *path);
void print_progress(time_t start, int total, int& completed, int per_cout);
void print_keypoints(const vector<KeyPoint>& kp);

bool extract_descriptors(int index, int fc, vector<KeyPoint>& kps, Mat& descs);
void select_keypoints(vector<KeyPoint>& k, vector<KeyPoint>& s, int fc);
void filter_keypoints(vector<KeyPoint>& k, vector<KeyPoint>& f);
void calc_response_density_img(const Mat& img, vector<KeyPoint> kp, Mat& ds);
void calc_reponse_response_density_space(const vector<KeyPoint>& kp);
void calc_scale_density_img(const Mat& img, vector<KeyPoint> kp, Mat& ds);
void calc_scale_response_density_space(const vector<KeyPoint>& kp);
float keypoint_dist(const KeyPoint& k1, const KeyPoint& k2);
float point_dist(const Point2f& p1, const Point2f& p2);
bool response_comparator(const KeyPoint& p1, const KeyPoint& p2);
float get_density(const KeyPoint& k);
bool density_comparator(const KeyPoint& p1, const KeyPoint& p2);
float get_scale_density(const KeyPoint& k);
bool scale_density_comparator(const KeyPoint& p1, const KeyPoint& p2);
bool scale_comparator(const KeyPoint& p1, const KeyPoint& p2);

vector<fs::path> img_paths;

float response_density_space[IMAGE_WIDTH][IMAGE_WIDTH];
float scale_response_density_space[IMAGE_WIDTH][IMAGE_WIDTH];
Mat current_img;

vector<KeyPoint> filtered_keypoints;
struct response_density_classcomp {
	bool operator()(const int& lhs, const int& rhs) const {
		KeyPoint kp1, kp2;
		kp1 = filtered_keypoints[lhs];
		kp2 = filtered_keypoints[rhs];
		return density_comparator(kp1, kp2);
	}
};
struct scale_response_density_classcomp {
	bool operator()(const int& lhs, const int& rhs) const {
		KeyPoint kp1, kp2;
		kp1 = filtered_keypoints[lhs];
		kp2 = filtered_keypoints[rhs];
		return scale_density_comparator(kp1, kp2);
	}
};

int main(int argc, char *argv[]) {
	if (argc < 4) {
		help();
		return -1;
	}

	int fc = atoi(argv[1]);
	sort_type = atoi(argv[2]);
	const char *imgs_path = argv[3];
	const char *index_path = argv[4];

	cout << "Reading image paths from " << imgs_path << endl;
	string img_path;
	int img_count;
	ifstream imgs_file(imgs_path);
	imgs_file >> img_count;
	for (int i = 0; i < img_count; ++i) {
		imgs_file >> img_path;
		img_paths.push_back(img_path);
	}
	cout << img_count << " images found" << endl;

	int total_process = img_count;
	int completed_process = 0;
	time_t start = time(0);

	cout << "Building index..." << endl;

	tictoc timer;
	timer.tic();

	vector<KeyPoint> kps;
	Mat descs;
	FileStorage fs(index_path, FileStorage::WRITE);
	fs << "index" << "[";
	for (int i = 0; i < img_count; ++i) {
		if (extract_descriptors(i, fc, kps, descs)) {
			fs << "{";
			fs << "path" << img_paths[i].string();
			fs << "keypoints" << kps;
			fs << "descriptors" << descs;
			fs << "}";
		}
	}
	fs << "]";
	fs.release();

	timer.toc();
	cout << "Time in seconds: " << timer.totalTimeSec() << endl;

	return 0;
}

void help() {
	cout << "Usage: ./build_index <fc> <sort_type> <imgs_path> <index_path>" << endl;
}

void print_progress(time_t start, int total, int& completed, int per_count) {
	++completed;
	if (completed % per_count != 0)
		return;
	int remaining_process = total - completed;
	time_t current = time(0);
	float elapsed_time = difftime(current, start);
	float remaining_time = elapsed_time * remaining_process / completed;
	cout << fixed << setprecision(3);
	cout << "\rRemaining time: ";
	cout << remaining_time / 60 << "min";
}

void print_keypoints(const vector<KeyPoint>& kp) {
	KeyPoint k;
	cout << setprecision(3) << fixed;
	for (int i = 0; i < (int) kp.size(); ++i) {
		k = kp[i];
		cout << "(" << k.pt.x << "\t" << k.pt.y << "):\t" << k.response << "\t" << k.size << "\t" << get_scale_density(k) << endl;
	}
}

void find_images(const char *path) {
	fs::path dir_path(path);
	fs::recursive_directory_iterator end_iter;

	for (fs::recursive_directory_iterator iter(dir_path); iter != end_iter; ++iter) {
		if (fs::is_regular_file(iter->status())) {
			if (!boost::algorithm::ends_with(iter->path().c_str(), ".png"))
				continue;
			img_paths.push_back(iter->path());
		}
	}

	sort(img_paths.begin(), img_paths.end());
}

bool extract_descriptors(int index, int fc, vector<KeyPoint>& kps, Mat& descs) {
	string img_path = img_paths[index].string();
	fs::path yaml = img_paths[index];
	string yaml_path = yaml.replace_extension(".yml").string();

	// cout << "Calc img matrix: " << img_path << endl;
	current_img = imread(img_path.c_str(), CV_LOAD_IMAGE_GRAYSCALE);

	if (!current_img.data) {
		cout << "Error reading image: " << img_path << endl;
		return 0;
	}

	vector<KeyPoint> keypoints;
	if (fs::exists(yaml_path)) {
		FileStorage fs(yaml_path.c_str(), FileStorage::READ);
		read(fs["keypoints"], keypoints);
	} else {
		SiftFeatureDetector detector;
		detector.detect(current_img, keypoints);
		FileStorage fs(yaml_path.c_str(), FileStorage::WRITE);
		fs << "keypoints" << keypoints;
	}
	// print_keypoints(keypoints);

	// int fc = keypoints.size() * fc / 100;
	// fc = fc > 10 ? fc : 10;

	// not enough feature, dont use them
	if (keypoints.size() == 0) {
		cout << "Image " << img_path << " has no feature" << endl;
		return 0;
	}

	vector<KeyPoint> selected_keypoints;
	if (fc == 0) {
		selected_keypoints = keypoints;
	} else {
		select_keypoints(keypoints, selected_keypoints, fc);
	}

	cout << setw(50) << left << img_path
			<< setw(10) << right << keypoints.size()
			<< setw(10) << selected_keypoints.size() << endl;

	// extract descriptors
	Mat descriptors;
	SiftDescriptorExtractor extractor;
	extractor.compute(current_img, selected_keypoints, descriptors);

	// cout << img_path << ":" << endl;
	// print_keypoints(selected_keypoints);
	// cout << descriptors << endl;

	if (show_images) {
		Mat white = Mat::ones(current_img.rows, current_img.cols, CV_8UC1) * 255;
		Mat black = Mat::zeros(current_img.rows, current_img.cols, CV_8UC1);
		Mat img_density = Mat::zeros(current_img.rows, current_img.cols, CV_8UC1);
		Mat img(current_img);

		drawKeypoints(current_img, keypoints, img, Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
		imshow("kp_on_img", img);

		drawKeypoints(black, keypoints, img, Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
		imshow("kp_on_black", img);

		drawKeypoints(black, selected_keypoints, img, Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
		imshow("selected_kp_on_white", img);

		calc_scale_density_img(current_img, keypoints, img_density);
		imshow("density", img_density);

		drawKeypoints(img_density, selected_keypoints, img, Scalar::all(-1), DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
		imshow("selected_kp_on_density", img);

		imshow("img", current_img);
		waitKey(0);
	}

	descs = descriptors;
	kps = selected_keypoints;
	return 1;
}

void select_keypoints(vector<KeyPoint>& k, vector<KeyPoint>& s, int fc) {
	// filter keypoints
	filtered_keypoints.clear();
	if (filter_kp) {
		filter_keypoints(k, filtered_keypoints);
	} else {
		filtered_keypoints = k;
	}
	int filtered_kp_size = filtered_keypoints.size();

	if (sort_type == -5) { // scale + scale density
		sort(filtered_keypoints.begin(), filtered_keypoints.end(), scale_comparator);
		vector<KeyPoint> scale_selected;
		for (int i = 0; i < 2 * fc && i < filtered_kp_size; ++i) {
			scale_selected.push_back(filtered_keypoints[i]);
		}
		calc_scale_response_density_space(scale_selected);
		sort(scale_selected.begin(), scale_selected.end(), scale_density_comparator);
		for (int i = 0; i < fc && i < filtered_kp_size; ++i) {
			s.push_back(scale_selected[i]);
		}
	} else if (sort_type == -4) { // scale density with substrating
		calc_scale_response_density_space(filtered_keypoints);
		sort(filtered_keypoints.begin(), filtered_keypoints.end(), scale_density_comparator);

		for (int i = 0; i < fc && i < filtered_kp_size; ++i) {
			KeyPoint keypoint = filtered_keypoints[0];
			s.push_back(keypoint);

			// subtract the contribution of this keypoint
			for (int j = 0; j < filtered_kp_size; j++) {
				KeyPoint kp = filtered_keypoints[i];
				float dist = keypoint_dist(kp, keypoint);
				boost::math::normal_distribution<> norm_dist(0.0, 1 / keypoint.size);
				float density = boost::math::pdf(norm_dist, dist);
				int x = kp.pt.x;
				int y = kp.pt.y;
				scale_response_density_space[x][y] -= density;
			}

			// delete the selected keypoint
			filtered_keypoints.erase(filtered_keypoints.begin());
			// resort
			sort(filtered_keypoints.begin(), filtered_keypoints.end(), scale_density_comparator);
		}
	} else if (sort_type == -3) { // scale density with kdtree
		calc_scale_response_density_space(filtered_keypoints);

		// create keypoint map for easy deletion
		map<int, KeyPoint, scale_response_density_classcomp> mk;
		map<int, KeyPoint>::iterator it;

		// create kd-tree index
		Mat coord = Mat::zeros(filtered_kp_size, 2, CV_32F);
		for (int i = 0; i < filtered_kp_size; ++i) {
			mk[i] = filtered_keypoints[i];
			coord.ptr<float>(i)[0] = filtered_keypoints[i].pt.x;
			coord.ptr<float>(i)[1] = filtered_keypoints[i].pt.y;
		}

		flann::Index kdtree_index(coord, flann::KDTreeIndexParams(4));

		int max_count = 100;
		Mat indices = Mat::zeros(1, max_count, CV_32F);
		Mat dists = Mat::zeros(1, max_count, CV_32F);

		for (int i = 0; i < fc && i < filtered_kp_size; ++i) {
			if (mk.size() == 0)
				break;

			KeyPoint query_keypoint = mk.begin()->second;

			s.push_back(query_keypoint);

			Mat query = Mat::zeros(1, 2, CV_32F);
			query.ptr<float>(0)[0] = query_keypoint.pt.x;
			query.ptr<float>(0)[1] = query_keypoint.pt.y;
			float radius = query_keypoint.size * query_keypoint.size / 4;
			int found_count = kdtree_index.radiusSearch(query, indices, dists, radius, max_count, flann::SearchParams());
			for (int j = 0; j < found_count; ++j) {
				mk.erase(indices.at<int>(j));
			}
		}
	} else if (sort_type == -2) { // scale density
		calc_scale_response_density_space(filtered_keypoints);
		sort(filtered_keypoints.begin(), filtered_keypoints.end(), scale_density_comparator);
		for (int i = 0; i < fc && i < filtered_kp_size; ++i) {
			s.push_back(filtered_keypoints[i]);
		}
	} else if (sort_type == -1) { // scale
		sort(filtered_keypoints.begin(), filtered_keypoints.end(), scale_comparator);
		for (int i = 0; i < fc && i < filtered_kp_size; ++i) {
			s.push_back(filtered_keypoints[i]);
		}
	} else if (sort_type == 1) { // response
		sort(filtered_keypoints.begin(), filtered_keypoints.end(), response_comparator);
		for (int i = 0; i < fc && i < filtered_kp_size; ++i) {
			s.push_back(filtered_keypoints[i]);
		}
	} else if (sort_type == 2) { // response density
		calc_reponse_response_density_space(filtered_keypoints);
		sort(filtered_keypoints.begin(), filtered_keypoints.end(), density_comparator);
		for (int i = 0; i < fc && i < filtered_kp_size; ++i) {
			s.push_back(filtered_keypoints[i]);
		}
	} else if (sort_type == 3) { // response density with kdtree
		calc_reponse_response_density_space(filtered_keypoints);

		// create keypoint map for easy deletion
		map<int, KeyPoint, response_density_classcomp> mk;
		map<int, KeyPoint>::iterator it;

		// create kd-tree index
		Mat coord = Mat::zeros(filtered_kp_size, 2, CV_32F);
		for (int i = 0; i < filtered_kp_size; ++i) {
			mk[i] = filtered_keypoints[i];
			coord.ptr<float>(i)[0] = filtered_keypoints[i].pt.x;
			coord.ptr<float>(i)[1] = filtered_keypoints[i].pt.y;
		}

		flann::Index kdtree_index(coord, flann::KDTreeIndexParams(4));

		int max_count = 100;
		Mat indices = Mat::zeros(1, max_count, CV_32F);
		Mat dists = Mat::zeros(1, max_count, CV_32F);

		for (int i = 0; i < fc && i < filtered_kp_size; ++i) {
			if (mk.size() == 0)
				break;

			KeyPoint query_keypoint = mk.begin()->second;

			s.push_back(query_keypoint);

			Mat query = Mat::zeros(1, 2, CV_32F);
			query.ptr<float>(0)[0] = query_keypoint.pt.x;
			query.ptr<float>(0)[1] = query_keypoint.pt.y;
			float radius = query_keypoint.size * query_keypoint.size / 4;
			int found_count = kdtree_index.radiusSearch(query, indices, dists, radius, max_count, flann::SearchParams());
			for (int j = 0; j < found_count; ++j) {
				mk.erase(indices.at<int>(j));
			}
		}
	}
}

void filter_keypoints(vector<KeyPoint>& k, vector<KeyPoint>& f) {
	if (sort_type < 0)
		sort(k.begin(), k.end(), scale_comparator);
	else
		sort(k.begin(), k.end(), response_comparator);

	for (int i = k.size() - 1; i >= 0; --i) {
		KeyPoint k1 = k[i];
		bool add = 1;
		for (int j = 0; j < f.size(); ++j) {
			KeyPoint k2 = f[j];
			float dist = keypoint_dist(k1, k2);
			if (dist < keypoint_filter_threshold) {
				add = 0;
				break;
			}
		}
		if (add)
			f.push_back(k1);
	}
}

void calc_response_density_img(const Mat& img, vector<KeyPoint> kp, Mat& ds) {
	int rows = img.rows;
	int cols = img.cols;
	int size = kp.size();
	Mat tmp = Mat::zeros(rows, cols, CV_32F);

	float max_value = 0, min_value = 9999999;
	for (int i = 0; i < cols; i++) {
		for (int j = 0; j < rows; j++) {
			float sum = 0;
			for (int k = 0; k < size; k++) {
				KeyPoint p = kp[k];
				float dist = sqrt(pow(i - p.pt.x, 2) + pow(j - p.pt.y, 2));
				boost::math::normal_distribution<> norm_dist(0.0, 1 / p.response * 100);
				sum += boost::math::pdf(norm_dist, dist);
			}
			tmp.ptr<float>(j)[i] = sum;
			if (sum > max_value)
				max_value = sum;
			if (sum < min_value)
				min_value = sum;
		}
	}

	ds = Mat(rows, cols, CV_8UC1);
	for (int i = 0; i < cols; i++) {
		for (int j = 0; j < rows; j++) {
			ds.at<uchar>(j, i) = (tmp.at<float>(j, i) - min_value) / (max_value - min_value) * 255;
		}
	}

	equalizeHist(ds, ds);
}

void calc_reponse_response_density_space(const vector<KeyPoint>& kp) {
	int kp_size = kp.size();
	for (int i = 0; i < kp_size; i++) {
		KeyPoint ki = kp[i];
		float density = 0;
		for (int j = 0; j < kp_size; j++) {
			KeyPoint kj = kp[j];
			float dist = keypoint_dist(ki, kj);
			boost::math::normal_distribution<> norm_dist(0.0, 1 / kj.response * 100);
			density += boost::math::pdf(norm_dist, dist);
		}
		int x = ki.pt.x;
		int y = ki.pt.y;
		response_density_space[x][y] = density;
	}
}

void calc_scale_density_img(const Mat& img, vector<KeyPoint> kp, Mat& ds) {
	int rows = img.rows;
	int cols = img.cols;
	int size = kp.size();
	Mat tmp = Mat::zeros(rows, cols, CV_32F);

	float max_value = 0, min_value = 9999999;
	for (int i = 0; i < cols; i++) {
		for (int j = 0; j < rows; j++) {
			float sum = 0;
			for (int k = 0; k < size; k++) {
				KeyPoint p = kp[k];
				float dist = sqrt(pow(i - p.pt.x, 2) + pow(j - p.pt.y, 2));
				boost::math::normal_distribution<> norm_dist(0.0, 20 / p.size);
				sum += boost::math::pdf(norm_dist, dist);
			}
			tmp.ptr<float>(j)[i] = sum;
			if (sum > max_value)
				max_value = sum;
			if (sum < min_value)
				min_value = sum;
		}
	}

	ds = Mat(rows, cols, CV_8UC1);
	for (int i = 0; i < cols; i++) {
		for (int j = 0; j < rows; j++) {
			ds.at<uchar>(j, i) = (tmp.at<float>(j, i) - min_value) / (max_value - min_value) * 255;
		}
	}

	equalizeHist(ds, ds);
}

void calc_scale_response_density_space(const vector<KeyPoint>& kp) {
	int kp_size = kp.size();
	for (int i = 0; i < kp_size; i++) {
		KeyPoint ki = kp[i];
		float density = 0;
		for (int j = 0; j < kp_size; j++) {
			KeyPoint kj = kp[j];
			float dist = keypoint_dist(ki, kj);
			boost::math::normal_distribution<> norm_dist(0.0, 20 / kj.size);
			density += boost::math::pdf(norm_dist, dist);
		}
		int x = ki.pt.x;
		int y = ki.pt.y;
		scale_response_density_space[x][y] = density;
	}
}

float keypoint_dist(const KeyPoint& k1, const KeyPoint& k2) {
	float dist = point_dist(k1.pt, k2.pt);
	return dist;
}

float point_dist(const Point2f& p1, const Point2f& p2) {
	float x_dist = p1.x - p2.x;
	float y_dist = p1.y - p2.y;
	float dist = sqrt(x_dist*x_dist + y_dist*y_dist);
	return dist;
}

bool response_comparator(const KeyPoint& p1, const KeyPoint& p2) {
	return p1.response > p2.response;
}

float get_density(const KeyPoint& k) {
	int x = k.pt.x;
	int y = k.pt.y;
	return response_density_space[x][y];
}

bool density_comparator(const KeyPoint& p1, const KeyPoint& p2) {
	return get_density(p1) > get_density(p2);
}

float get_scale_density(const KeyPoint& k) {
	int x = k.pt.x;
	int y = k.pt.y;
	return scale_response_density_space[x][y];
}

bool scale_density_comparator(const KeyPoint& p1, const KeyPoint& p2) {
	return get_scale_density(p1) > get_scale_density(p2);
}

bool scale_comparator(const KeyPoint& p1, const KeyPoint& p2) {
	return p1.size > p2.size;
}


#include <iostream>
#include <fstream>
#include <Eigen/Geometry>

#include "srrg_types/types.hpp"

using namespace srrg_core;

struct PositionMeasurement {
    PositionMeasurement(const double& timestamp_seconds_,
                        const Eigen::Vector3d& position_): timestamp_seconds(timestamp_seconds_),
                                                           position(position_) {}
    PositionMeasurement(): timestamp_seconds(0), position(Eigen::Vector3d::Zero()) {}
    double timestamp_seconds;
    Eigen::Vector3d position;
};

const double getAbsoluteTranslationRootMeanSquaredError(const std::vector<std::pair<PositionMeasurement, PositionMeasurement>> position_correspondences_);

const Eigen::Vector3d getInterpolatedPositionLinear(const PositionMeasurement& ground_truth_previous_,
                                                    const PositionMeasurement& ground_truth_next_,
                                                    const PositionMeasurement& measurement_);

int32_t main (int32_t argc_, char** argv_) {
  if (argc_ < 5) {
    std::cerr << "usage: ./trajectory_analyzer -tum <trajectory.txt> -asl <ground_truth.txt> [-skip <integer>]" << std::endl;
    return 0;
  }

  //ds determine and log configuration
  std::string file_name_trajectory_slam = argv_[1];
  std::string file_name_trajectory_ground_truth = argv_[2];
  uint32_t number_of_poses_to_skip = 0;

  //ds parse configuration
  int32_t number_of_checked_parameters = 1;
  while (number_of_checked_parameters < argc_) {
    if (!std::strcmp(argv_[number_of_checked_parameters], "-tum")) {
      ++number_of_checked_parameters;
      if (number_of_checked_parameters == argc_) {break;}
      file_name_trajectory_slam = argv_[number_of_checked_parameters];
    } else if (!std::strcmp(argv_[number_of_checked_parameters], "-asl")) {
      ++number_of_checked_parameters;
      if (number_of_checked_parameters == argc_) {break;}
      file_name_trajectory_ground_truth = argv_[number_of_checked_parameters];
    } else if (!std::strcmp(argv_[number_of_checked_parameters], "-skip")) {
      ++number_of_checked_parameters;
      if (number_of_checked_parameters == argc_) {break;}
      number_of_poses_to_skip = std::stoi(argv_[number_of_checked_parameters]);
    }
    ++number_of_checked_parameters;
  }

  //ds log configuration
  std::cerr << "file_name_trajectory_slam: " << file_name_trajectory_slam << std::endl;
  std::cerr << "file_name_trajectory_ground_truth: " << file_name_trajectory_ground_truth << std::endl;
  std::cerr << "number_of_poses_to_skip: " << number_of_poses_to_skip << std::endl;

  //ds parse SLAM trajectory
  std::ifstream input_stream_trajectory_slam(file_name_trajectory_slam);
  if (!input_stream_trajectory_slam.good() || !input_stream_trajectory_slam.is_open()) {
    std::cerr << "ERROR: unable to open: '" << file_name_trajectory_slam << "'" << std::endl;
    return 0;
  }

  //ds load SLAM trajectory poses
  std::vector<PositionMeasurement> positions_slam;
  std::string buffer_line;
  uint32_t skipped_poses = 0;
  while (std::getline(input_stream_trajectory_slam, buffer_line)) {

    //ds get line to a string stream object
    std::istringstream stringstream(buffer_line);

    //ds possible values
    double timestamp_seconds = 0;
    double translation_x = 0;
    double translation_y = 0;
    double translation_z = 0;
    double quaternion_w  = 0;
    double quaternion_x  = 0;
    double quaternion_y  = 0;
    double quaternion_z  = 0;

    //ds parse the full line and check for failure
    if (!(stringstream >> timestamp_seconds >> translation_x >> translation_y >> translation_z
                                            >> quaternion_x >> quaternion_y >> quaternion_z >> quaternion_w)) {
      std::cerr << "ERROR: unable to parse pose lines" << std::endl;
      input_stream_trajectory_slam.close();
      return 0;
    }

    //ds set pose value
    Eigen::Isometry3d parsed_pose(Eigen::Isometry3d::Identity());
    parsed_pose.translation() = Eigen::Vector3d(translation_x, translation_y, translation_z);
    parsed_pose.linear()      = Eigen::Quaterniond(quaternion_w, quaternion_x, quaternion_y, quaternion_z).toRotationMatrix();

    if (skipped_poses >= number_of_poses_to_skip) {
      positions_slam.push_back(PositionMeasurement(timestamp_seconds, parsed_pose.translation()));
    } else {
      ++skipped_poses;
    }
  }
  input_stream_trajectory_slam.close();

  //ds check skip
  if (number_of_poses_to_skip >= positions_slam.size()) {
    std::cerr << "ERROR: insufficient number of measurements for number_of_poses_to_skip: " << number_of_poses_to_skip << std::endl;
    return 0;
  }

  //ds also cut skipped poses from the end
  positions_slam.resize(positions_slam.size()-number_of_poses_to_skip);
  std::cerr << "loaded trajectory SLAM positions: " << positions_slam.size() << " for: " << file_name_trajectory_slam << std::endl;

  //ds parse ground truth trajectory
  std::ifstream input_stream_trajectory_ground_truth(file_name_trajectory_ground_truth);
  if (!input_stream_trajectory_ground_truth.good() || !input_stream_trajectory_ground_truth.is_open()) {
    std::cerr << "ERROR: unable to open: '" << file_name_trajectory_ground_truth << "'" << std::endl;
    return 0;
  }

  //ds load ground truth poses
  std::vector<PositionMeasurement> positions_ground_truth;
  while (std::getline(input_stream_trajectory_ground_truth, buffer_line)) {

    //ds skip comment lines
    if (buffer_line[0] == '#') {
      continue;
    }

    //ds parse control
    std::string::size_type index_begin_item = 0;
    std::string::size_type index_end_item   = 0;

    //ds parse timestamp
    index_end_item = buffer_line.find(",", index_begin_item);
    const uint64_t timestamp_nanoseconds = std::atol(buffer_line.substr(index_begin_item, index_end_item).c_str());
    const double timestamp_seconds       = timestamp_nanoseconds/1e9;
    index_begin_item = index_end_item+1;

    //ds position buffer
    Eigen::Isometry3d leica_to_world(Eigen::Isometry3d::Identity());
    for (uint32_t row = 0; row < 3; ++row) {
      index_end_item = buffer_line.find(",", index_begin_item);
      leica_to_world.translation()(row) = std::strtod(buffer_line.substr(index_begin_item, index_end_item-index_begin_item).c_str(), 0);
      index_begin_item = index_end_item+1;
    }

    //ds add to buffer
    positions_ground_truth.push_back(PositionMeasurement(timestamp_seconds, leica_to_world.translation()));
  }
  input_stream_trajectory_ground_truth.close();
  std::cerr << "loaded trajectory ground truth positions: " << positions_ground_truth.size() << " for: " << file_name_trajectory_ground_truth << std::endl;

  //ds corresponding measurements
  std::vector<std::pair<PositionMeasurement, PositionMeasurement>> position_correspondences;
  Eigen::Vector3d position_shift(Eigen::Vector3d::Zero());

  //ds for each measurement
  for (uint64_t index_slam = 0; index_slam < positions_slam.size(); ++index_slam) {
    PositionMeasurement& measurement = positions_slam[index_slam];

    //ds find closest ground truth point - bruteforce
    double timestamp_difference_seconds_best = 1;
    uint32_t index_best                      = 0;
    for (uint64_t index_ground_truth = 0; index_ground_truth < positions_ground_truth.size(); ++index_ground_truth) {
      const double timestamp_difference_seconds = std::fabs(measurement.timestamp_seconds-positions_ground_truth[index_ground_truth].timestamp_seconds);
      if (timestamp_difference_seconds < timestamp_difference_seconds_best) {
        timestamp_difference_seconds_best = timestamp_difference_seconds;
        index_best = index_ground_truth;
      }
    }

    //ds skip until we arrive at the ground truth timestamp
    if (index_best == 0) {
      continue;
    }

    //ds solution
    PositionMeasurement ground_truth_interpolated(measurement.timestamp_seconds, measurement.position);

    //ds interpolation: check if before the system
    if (positions_ground_truth[index_best].timestamp_seconds < measurement.timestamp_seconds) {

      //ds interpolate to next
      ground_truth_interpolated.position = getInterpolatedPositionLinear(positions_ground_truth[index_best], positions_ground_truth[index_best+1], measurement);
    } else {

      //ds interpolate from previous
      ground_truth_interpolated.position = getInterpolatedPositionLinear(positions_ground_truth[index_best-1], positions_ground_truth[index_best], measurement);
    }

    //ds for the first measurement - compute starting point offset
    if (index_slam == 0) {
      position_shift = ground_truth_interpolated.position;
    }

    //ds adjust position
    measurement.position += position_shift;

    //ds save the first correspondence
    position_correspondences.push_back(std::make_pair(measurement, ground_truth_interpolated));
  }
  std::cerr << "\ninterpolated positions: " << position_correspondences.size() << " for: " << file_name_trajectory_ground_truth << std::endl;

  std::cerr << "\nraw RMSE: " << getAbsoluteTranslationRootMeanSquaredError(position_correspondences) << "\n" << std::endl;

  //ds objective
  Eigen::Isometry3d transform_slam_to_ground_truth(Eigen::Isometry3d::Identity());

  //ds ICP configuration
  const uint32_t number_of_iterations = 100;
  const double maximum_error_kernel   = 1; //ds (m^2)

  //ds ICP running variables
  Matrix6d H(Matrix6d::Zero());
  Vector6d b(Vector6d::Zero());

  //ds perform least squares optimization
  std::cerr << "optimizing transform .." << std::endl;
  for (uint32_t iteration = 0; iteration < number_of_iterations; ++iteration) {

    //ds initialize setup
    H.setZero();
    b.setZero();
    uint32_t number_of_inliers = 0;
    double total_error_squared = 0;

    //ds for all SLAM trajectory poses
    for (const std::pair<PositionMeasurement, PositionMeasurement>& position_correspondence: position_correspondences) {

      //ds compute current error
      const Eigen::Vector3d& measured_point_in_reference = position_correspondence.second.position;
      const Eigen::Vector3d sampled_point_in_reference   = transform_slam_to_ground_truth*position_correspondence.first.position;
      const Eigen::Vector3d error                        = sampled_point_in_reference-measured_point_in_reference;

      //ds update chi
      const double error_squared = error.transpose()*error;

      //ds check if outlier
      double weight = 1.0;
      if (error_squared > maximum_error_kernel) {
        weight = maximum_error_kernel/error_squared;
      } else {
        ++number_of_inliers;
      }
      total_error_squared += error_squared;

      //ds get the jacobian of the transform part = [I -2*skew(T*modelPoint)]
      Matrix3_6d jacobian;
      jacobian.block<3,3>(0,0).setIdentity();
      jacobian.block<3,3>(0,3) = -2*skew(sampled_point_in_reference);

      //ds precompute transposed
      const Matrix6_3d jacobian_transposed(jacobian.transpose());

      //ds accumulate
      H += weight*jacobian_transposed*jacobian;
      b += weight*jacobian_transposed*error;
    }

    //ds solve the system and update the estimate
    transform_slam_to_ground_truth = srrg_core::v2t(static_cast<const Vector6d&>(H.ldlt().solve(-b)))*transform_slam_to_ground_truth;

    //ds enforce rotation symmetry
    const Eigen::Matrix3d rotation   = transform_slam_to_ground_truth.linear();
    Eigen::Matrix3d rotation_squared = rotation.transpose( )*rotation;
    rotation_squared.diagonal().array()     -= 1;
    transform_slam_to_ground_truth.linear() -= 0.5*rotation*rotation_squared;

    //ds status
    std::printf("iteration: %03u total error (m^2): %12.3f (inliers: %4u/%4lu=%4.2f)\n",
                iteration, total_error_squared, number_of_inliers, position_correspondences.size(), static_cast<double>(number_of_inliers)/position_correspondences.size());
  }

  //ds compute optimal poses
  for (std::pair<PositionMeasurement, PositionMeasurement>& position_correspondence: position_correspondences) {
    position_correspondence.first.position = transform_slam_to_ground_truth*position_correspondence.first.position;
  }
  std::cerr << "done" << std::endl;

  //ds done
  std::cerr << "\noptimal RMSE: " << getAbsoluteTranslationRootMeanSquaredError(position_correspondences) << std::endl;
  return 0;
}

const double getAbsoluteTranslationRootMeanSquaredError(const std::vector<std::pair<PositionMeasurement, PositionMeasurement>> position_correspondences_) {

  //ds compute absolute squared errors
  std::vector<double> squared_errors_translation_absolute(0);
  for (const std::pair<PositionMeasurement, PositionMeasurement>& position_correspondence: position_correspondences_) {

    //ds compute squared errors between frames
    squared_errors_translation_absolute.push_back((position_correspondence.first.position-position_correspondence.second.position).squaredNorm());
  }

  //ds compute RMSE
  double root_mean_squared_error_translation_absolute = 0;
  for (const double& squared_error: squared_errors_translation_absolute) {
    root_mean_squared_error_translation_absolute += squared_error;
  }
  root_mean_squared_error_translation_absolute /= squared_errors_translation_absolute.size();
  root_mean_squared_error_translation_absolute = std::sqrt(root_mean_squared_error_translation_absolute);
  squared_errors_translation_absolute.clear();

  //ds done
  return root_mean_squared_error_translation_absolute;
}

const Eigen::Vector3d getInterpolatedPositionLinear(const PositionMeasurement& ground_truth_previous_,
                                              const PositionMeasurement& ground_truth_next_,
                                              const PositionMeasurement& measurement_) {

  //ds interpolate to next
  const double timestamp_difference_seconds_ground_truth = ground_truth_next_.timestamp_seconds-ground_truth_previous_.timestamp_seconds;
  const double timestamp_difference_seconds = measurement_.timestamp_seconds-ground_truth_previous_.timestamp_seconds;
//  std::printf("interpolating timestamps: [%f, %f, %f\n", ground_truth_previous_.timestamp_seconds,
//                                                         measurement_.timestamp_seconds,
//                                                         ground_truth_next_.timestamp_seconds);

  //ds compute interpolated reference measurement
  const Eigen::Vector3d position_interpolated = ground_truth_previous_.position +
                                                timestamp_difference_seconds/timestamp_difference_seconds_ground_truth*
                                                (ground_truth_next_.position-ground_truth_previous_.position);

//  std::cerr << ground_truth_previous_.position.transpose() << std::endl;
//  std::cerr << position_interpolated.transpose() << std::endl;
//  std::cerr << ground_truth_next_.position.transpose() << std::endl;

  return position_interpolated;
}


//==================================================================================================
/*!
  @file

  @copyright 2015 NumScale SAS
  @copyright 2015 J.T. Lapreste

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_COTH_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_FUNCTION_SCALAR_COTH_HPP_INCLUDED

#include <boost/simd/arch/common/detail/generic/tanh_kernel.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/ratio.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/function/scalar/abs.hpp>
#include <boost/simd/function/scalar/bitofsign.hpp>
#include <boost/simd/function/scalar/bitwise_xor.hpp>
#include <boost/simd/function/scalar/coth.hpp>
#include <boost/simd/function/scalar/exp.hpp>
#include <boost/simd/function/scalar/fma.hpp>
#include <boost/simd/function/scalar/minusone.hpp>
#include <boost/simd/function/scalar/rec.hpp>
#include <boost/simd/function/scalar/sqr.hpp>
#include <boost/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD ( coth_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 a0) const BOOST_NOEXCEPT
    {
      //////////////////////////////////////////////////////////////////////////////
      // if x = abs(a0) is less than 5/8 sinh is computed using a polynomial(float)
      // (respectively rational(double)) approx inspirated from cephes.
      // else
      // coth(a0) is  sign(a0)*(1 + 2/(exp(2*x)-1))
      //////////////////////////////////////////////////////////////////////////////
      A0 x = bs::abs(a0);
      A0 r;
      if( x < Ratio<A0, 5, 8>())
      {
        r = detail::tanh_kernel<A0>::coth(x, sqr(x));
      }
      else
      {
        r = fma(Two<A0>(), rec(minusone(exp(x+x))), One<A0>());
      }
      return bitwise_xor(r, bitofsign(a0));
    }
  };
} } }

#endif


#include "scale.h"

#include "dynamic_value/boolean_value.h"
#include "dynamic_value/float_value.h"
#include "dynamic_value/get_value_as.h"
#include "geometry_component.h"
#include "geometry_system.h"
#include "hierarchical_component.h"
#include "hierarchical_system.h"
#include "math_lib/matrix_base.h"
#include "procedural_object_system.h"

#include "geometry_operations/matrix_transform.h"

#include <boost/qvm/map_vec_mat.hpp>
#include <boost/qvm/mat_operations.hpp>

namespace pagoda
{
const std::string Scale::s_inputGeometry("in");
const std::string Scale::s_outputGeometry("out");

Scale::Scale(ProceduralObjectSystemPtr objectSystem) : ProceduralOperation(objectSystem)
{
	CreateInputInterface(s_inputGeometry);
	CreateOutputInterface(s_outputGeometry);

	RegisterValues({{"x", std::make_shared<FloatValue>(0.0f)},
	                {"y", std::make_shared<FloatValue>(0.0f)},
	                {"z", std::make_shared<FloatValue>(0.0f)},
	                {"pivotal_point", std::make_shared<String>("scope_center")}});
}
Scale::~Scale() {}

void Scale::DoWork()
{
	START_PROFILE;

	auto geometrySystem = m_proceduralObjectSystem->GetComponentSystem<GeometrySystem>();
	auto hierarchicalSystem = m_proceduralObjectSystem->GetComponentSystem<HierarchicalSystem>();

	while (HasInput(s_inputGeometry))
	{
		ProceduralObjectPtr inObject = GetInputProceduralObject(s_inputGeometry);
		ProceduralObjectPtr outObject = CreateOutputProceduralObject(s_outputGeometry);

		auto inGeometryComponent = geometrySystem->GetComponentAs<GeometryComponent>(inObject);
		GeometryPtr inGeometry = inGeometryComponent->GetGeometry();
		auto outGeometryComponent = geometrySystem->CreateComponentAs<GeometryComponent>(outObject);
		auto outGeometry = std::make_shared<Geometry>();
		outGeometryComponent->SetGeometry(outGeometry);

		auto inScope = inGeometryComponent->GetScope();
		UpdateValue("x");
		UpdateValue("y");
		UpdateValue("z");
		UpdateValue("pivotal_point");

		auto x = get_value_as<float>(*GetValue("x"));
		auto y = get_value_as<float>(*GetValue("y"));
		auto z = get_value_as<float>(*GetValue("z"));
		auto pivotalPointName = get_value_as<std::string>(*GetValue("pivotal_point"));

		Mat4x4F matrix;
		if (pivotalPointName == "scope_center")
		{
			Vec3F pivotalPoint = inScope.GetCenterPointInWorld();
			Mat4x4F translation = boost::qvm::translation_mat(XYZ(pivotalPoint));
			Mat4x4F scale = boost::qvm::diag_mat(XYZ1(Vec3F{x, y, z}));
			Mat4x4F invTranslation = boost::qvm::translation_mat(XYZ(-pivotalPoint));
			matrix = translation * scale * invTranslation;
		}
		else if (pivotalPointName == "scope_origin")
		{
			Vec3F pivotalPoint = inScope.GetWorldPoint(Scope::BoxPoints::LowerBottomLeft);
			matrix = boost::qvm::translation_mat(pivotalPoint) * boost::qvm::diag_mat(XYZ1(Vec3F{x, y, z})) *
			         boost::qvm::translation_mat(-pivotalPoint);
		}
		else if (pivotalPointName == "world_origin")
		{
			matrix = boost::qvm::diag_mat(XYZ1(Vec3F{x, y, z}));
		}

		MatrixTransform<Geometry> transform(matrix);

		transform.Execute(inGeometry, outGeometry);

		outGeometryComponent->SetScope(Scope::FromGeometryAndConstrainedRotation(outGeometry, inScope.GetRotation()));

		auto inHierarchicalComponent = hierarchicalSystem->GetComponentAs<HierarchicalComponent>(inObject);
		auto outHierarchicalComponent = hierarchicalSystem->CreateComponentAs<HierarchicalComponent>(outObject);
		hierarchicalSystem->SetParent(outHierarchicalComponent, inHierarchicalComponent);
	}
}
}  // namespace pagoda


/*
    Copyright 2005-2007 Adobe Systems Incorporated

    Use, modification and distribution are subject to the Boost Software License,
    Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
    http://www.boost.org/LICENSE_1_0.txt).
*/

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

#ifndef BOOST_GIL_EXTENSION_NUMERIC_PIXEL_NUMERIC_OPERATIONS_HPP
#define BOOST_GIL_EXTENSION_NUMERIC_PIXEL_NUMERIC_OPERATIONS_HPP

/*!
/// \file
/// \brief Structures for pixel-wise numeric operations
/// \author Lubomir Bourdev and Hailin Jin \n
///         Adobe Systems Incorporated
/// \date   2005-2007 \n
/// Currently defined structures:
///     pixel_plus_t (+), pixel_minus_t (-)
///     pixel_multiplies_scalar_t (*), pixel_divides_scalar_t (/)
///     pixel_halves_t (/=2), pixel_zeros_t (=0)
///     pixel_assigns_t (=)
*/

#include <functional>

#include <boost/gil/gil_config.hpp>
#include <boost/gil/pixel.hpp>
#include <boost/gil/color_base_algorithm.hpp>

#include <boost/gil/extension/numeric/channel_numeric_operations.hpp>

namespace boost { namespace gil {

/// \ingroup PixelNumericOperations
/// \brief construct for adding two pixels
template <typename PixelRef1, // models pixel concept
          typename PixelRef2, // models pixel concept
          typename PixelR>    // models pixel value concept
struct pixel_plus_t {
    PixelR operator() (const PixelRef1& p1,
                       const PixelRef2& p2) const {
        PixelR result;
        static_transform(p1,p2,result,
                           channel_plus_t<typename channel_type<PixelRef1>::type,
                                          typename channel_type<PixelRef2>::type,
                                          typename channel_type<PixelR>::type>());
        return result;
    }
};

/// \ingroup PixelNumericOperations
/// \brief construct for subtracting two pixels
template <typename PixelRef1, // models pixel concept
          typename PixelRef2, // models pixel concept
          typename PixelR>    // models pixel value concept
struct pixel_minus_t {
    PixelR operator() (const PixelRef1& p1,
                       const PixelRef2& p2) const {
        PixelR result;
        static_transform(p1,p2,result,
                           channel_minus_t<typename channel_type<PixelRef1>::type,
                                           typename channel_type<PixelRef2>::type,
                                           typename channel_type<PixelR>::type>());
        return result;
    }
};

/// \ingroup PixelNumericOperations
/// \brief construct for multiplying scalar to a pixel
template <typename PixelRef, // models pixel concept
          typename Scalar,   // models a scalar type
          typename PixelR>   // models pixel value concept
struct pixel_multiplies_scalar_t {
    PixelR operator () (const PixelRef& p,
                        const Scalar& s) const {
        PixelR result;
        static_transform(p,result,
                           std::bind2nd(channel_multiplies_scalar_t<typename channel_type<PixelRef>::type,
                                                                    Scalar,
                                                                    typename channel_type<PixelR>::type>(),s));
        return result;
    }
};

/// \ingroup PixelNumericOperations
/// \brief construct for dividing two pixels
template <typename PixelRef1, // models pixel concept
          typename PixelRef2, // models pixel concept
          typename PixelR>    // models pixel value concept
struct pixel_multiply_t {
    PixelR operator() (const PixelRef1& p1,
                       const PixelRef2& p2) const {
        PixelR result;
        static_transform(p1,p2,result,
                           channel_multiplies_t<typename channel_type<PixelRef1>::type,
                                                typename channel_type<PixelRef2>::type,
                                                typename channel_type<PixelR>::type>());
        return result;
    }
};

/// \ingroup PixelNumericOperations
/// \brief construct for dividing a pixel by a scalar
template <typename PixelRef, // models pixel concept
          typename Scalar,   // models a scalar type
          typename PixelR>   // models pixel value concept
struct pixel_divides_scalar_t {
    PixelR operator () (const PixelRef& p,
                        const Scalar& s) const {
        PixelR result;
        static_transform(p,result,
                           std::bind2nd(channel_divides_scalar_t<typename channel_type<PixelRef>::type,
                                                                 Scalar,
                                                                 typename channel_type<PixelR>::type>(),s));
        return result;
    }
};

/// \ingroup PixelNumericOperations
/// \brief construct for dividing two pixels
template <typename PixelRef1, // models pixel concept
          typename PixelRef2, // models pixel concept
          typename PixelR>    // models pixel value concept
struct pixel_divide_t {
    PixelR operator() (const PixelRef1& p1,
                       const PixelRef2& p2) const {
        PixelR result;
        static_transform(p1,p2,result,
                           channel_divides_t<typename channel_type<PixelRef1>::type,
                                             typename channel_type<PixelRef2>::type,
                                             typename channel_type<PixelR>::type>());
        return result;
    }
};

/// \ingroup PixelNumericOperations
/// \brief construct for dividing a pixel by 2
template <typename PixelRef> // models pixel concept
struct pixel_halves_t {
    PixelRef& operator () (PixelRef& p) const {
        static_for_each(p,channel_halves_t<typename channel_type<PixelRef>::type>());
        return p;
    }
};

/// \ingroup PixelNumericOperations
/// \brief construct for setting a pixel to zero (for whatever zero means)
template <typename PixelRef> // models pixel concept
struct pixel_zeros_t {
    PixelRef& operator () (PixelRef& p) const {
        static_for_each(p,channel_zeros_t<typename channel_type<PixelRef>::type>());
        return p;
    }
};

// Hailin: This is how you can do it:
template <typename Pixel>
void zero_channels(Pixel& p) {
    static_for_each(p,channel_zeros_t<typename channel_type<Pixel>::type>());
}

/// \ingroup PixelNumericOperations
///definition and a generic implementation for casting and assigning a pixel to another
///user should specialize it for better performance
template <typename PixelRef,  // models pixel concept
          typename PixelRefR> // models pixel concept
struct pixel_assigns_t {
    PixelRefR operator () (const PixelRef& src,
                           PixelRefR& dst) const {
        static_for_each(src,dst,channel_assigns_t<typename channel_type<PixelRef>::type,
                                                  typename channel_type<PixelRefR>::type>());
        return dst;
    }
};

} }  // namespace boost::gil

#endif // BOOST_GIL_EXTENSION_NUMERIC_PIXEL_NUMERIC_OPERATIONS_HPP