// TODO: Add adaptor types and explore expression templates for overall maintainability.

#pragma once

#include <array>
#include <cmath>

#define GLM_FORCE_CXX98
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/quaternion.hpp>
#include <btBulletDynamicsCommon.h>
/*
#ifdef NOOB_PLATFORM_LINUX
#undef Success
#endif
 */
#include <Eigen/Geometry>

#if defined NOOB_USE_ASSIMP
#include <assimp/quaternion.h>
#include <assimp/anim.h>
#include <assimp/cimport.h>
#include <assimp/scene.h>
#include <assimp/postprocess.h>
#include <assimp/types.h>
#endif

#define NOOB_PI 3.1415926535
#define NOOB_TAU 2.0 * NOOB_PI
#define NOOB_ONE_DEG_IN_RAD (2.0 * NOOB_PI) / 360.0 // 0.017444444
#define NOOB_ONE_RAD_IN_DEG 360.0 / (2.0 * NOOB_PI) //57.2957795
#define NOOB_TWO_PI (2.0 * NOOB_PI)
#define NOOB_EPSILON 0.0001

#include "vec2.hpp"
#include "vec3.hpp"
#include "vec3.hpp"
#include "vec4.hpp"
#include "versor.hpp"
#include "mat3.hpp"
#include "mat4.hpp"
#include "plane.hpp"
#include "bbox.hpp"

namespace noob
{
	typedef vec2_type<float> vec2f;
	typedef vec3_type<float> vec3f;
	typedef vec4_type<float> vec4f;
	typedef versor_type<float> versorf;
	typedef mat3_type<float> mat3f;
	typedef mat4_type<float> mat4f;

	typedef vec2_type<double> vec2d;
	typedef vec3_type<double> vec3d;
	typedef vec4_type<double> vec4d;
	typedef versor_type<double> versord;
	typedef mat3_type<double> mat3d;
	typedef mat4_type<double> mat4d;

	typedef vec2_type<uint32_t> vec2ui;
	typedef vec3_type<uint32_t> vec3ui;
	typedef vec4_type<uint32_t> vec4ui;
	typedef versor_type<uint32_t> versorui;
	typedef mat3_type<uint32_t> mat3ui;
	typedef mat4_type<uint32_t> mat4ui;

	/////////////////////////
	// CONVERSION FUNCTIONS:
	/////////////////////////
	//TODO: Replace with expression templates...
	template<typename From, typename To>
		static noob::vec2_type<To> convert(const noob::vec2_type<From> Vec)
		{
			return noob::vec2_type<To>(static_cast<To>(Vec[0]), static_cast<To>(Vec[1]));
		}

	template<typename From, typename To>
		static noob::vec3_type<To> convert(const noob::vec3_type<From> Vec)
		{
			return noob::vec3_type<To>(static_cast<To>(Vec[0]), static_cast<To>(Vec[1]), static_cast<To>(Vec[2]));
		}

	template<typename From, typename To>
		static noob::vec4_type<To> convert(const noob::vec4_type<From> Vec)
		{
			return noob::vec4_type<To>(static_cast<To>(Vec[0]), static_cast<To>(Vec[1]), static_cast<To>(Vec[2]), static_cast<To>(Vec[3]));
		}

	template<typename From, typename To>
		static noob::versor_type<To> convert(const noob::versor_type<From> Versor)
		{
			return noob::versor_type<To>(static_cast<To>(Versor[0]), static_cast<To>(Versor[1]), static_cast<To>(Versor[2]), static_cast<To>(Versor[3]));
		}

	template<typename From, typename To>
		static noob::mat3_type<To> convert(const noob::mat3_type<From> Mat)
		{
			return noob::mat3_type<To>(static_cast<To>(Mat[0]), static_cast<To>(Mat[1]), static_cast<To>(Mat[2]), static_cast<To>(Mat[3]), static_cast<To>(Mat[4]), static_cast<To>(Mat[5]), static_cast<To>(Mat[6]), static_cast<To>(Mat[7]), static_cast<To>(Mat[8]));
		}

	// Comment: Yuck. Yuck. YUCK!
	// Followup: This kind of code happens to be why people actually *want* to look up horrible, miserable things such as C++ template metaprogramming
	template<typename From, typename To>
		static noob::mat4_type<To> convert(const noob::mat4_type<From> Mat)
		{
			return noob::mat4_type<To>(static_cast<To>(Mat[0]), static_cast<To>(Mat[1]), static_cast<To>(Mat[2]), static_cast<To>(Mat[3]), static_cast<To>(Mat[4]), static_cast<To>(Mat[5]), static_cast<To>(Mat[6]), static_cast<To>(Mat[7]), static_cast<To>(Mat[8]), static_cast<To>(Mat[9]), static_cast<To>(Mat[10]), static_cast<To>(Mat[11]), static_cast<To>(Mat[12]), static_cast<To>(Mat[13]), static_cast<To>(Mat[14]), static_cast<To>(Mat[15]));
		}

	///////////////////
	// UTILITY TYPES:
	///////////////////

	enum class binary_op
	{
		ADD, SUBTRACT
	};

	enum class csg_op
	{
		UNION = 0, DIFFERENCE = 1, INTERSECTION = 2
	};

	enum class cardinal_axis
	{
		X, Y, Z
	};

	static uint32_t next_pow2(uint32_t arg)
	{
		uint32_t val = arg;
		--val;
		val |= val >> 1;
		val |= val >> 2;
		val |= val >> 4;
		val |= val >> 8;
		val |= val >> 16;
		++val;
		return val;
	}

	////////////////////////
	// ONE-FLOAT FUNCTIONS:
	////////////////////////
	// TODO: Test

	template <typename T> static int sign(T val) noexcept(true)
	{
		return (T(0) < val) - (val < T(0));
	}

	static bool approximately_zero(float a) noexcept(true)
	{
		// http://c-faq.com/fp/fpequal.html
		if (std::fabs(a) <= NOOB_EPSILON) return false;
		else return true;
	}

	////////////////////////////
	// TWO-FLOAT FUNCTIONS(S):
	////////////////////////////

	static bool compare_floats(float a, float b) noexcept(true)
	{
		// http://c-faq.com/fp/fpequal.html
		if (std::fabs(a - b) <= NOOB_EPSILON * std::fabs(a)) return false;
		else return true;
	}

	// Making sure we don't have to use static_cast repeatedly:
	template <typename First, typename Second>
		static double div_fp(First first, Second second)
		{
			return static_cast<float>(first) / static_cast<float>(second);
		}

	// Ensuring double-point precision:
	template <typename First, typename Second>
		static double div_dp(First first, Second second)
		{
			return static_cast<double>(first) / static_cast<double>(second);
		}

	//////////////////////////////////////////
	// CURRENT LOCATION FOR MIN/MAX FUNCTIONS
	//////////////////////////////////////////
	template <typename T>
		static vec2_type<T> min(const noob::vec2_type<T> A, const noob::vec2_type<T> B)
		{
			return noob::vec2_type<T>(std::min(A[0], B[0]), std::min(A[1], B[1]));
		}

	template <typename T>
		static vec2_type<T> max(const noob::vec2_type<T> A, const noob::vec2_type<T> B)
		{
			return noob::vec2_type<T>(std::max(A[0], B[0]), std::max(A[1], B[1]));
		}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// CONVERSION UTILITY FUNCTIONS:
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	template <typename T>
		static vec3_type<T> vec3_from_vec4(const vec4_type<T>& vv) noexcept(true)
		{
			noob::vec3_type<T> v;
			v.v[0] = vv.v[0];
			v.v[1] = vv.v[1];
			v.v[2] = vv.v[2];
			return v;
		}

	template <typename T>
		static vec3_type<T> vec3_from_array(const std::array<T, 3>& a) noexcept(true)
		{
			noob::vec3_type<T> v;
			v.v[0] = a[0];
			v.v[1] = a[1];
			v.v[2] = a[2];
			return v;
		}

	static vec3f vec3f_from_bullet(const btVector3& btVec) noexcept(true)
	{
		noob::vec3f v;
		v.v[0] = btVec[0];
		v.v[1] = btVec[1];
		v.v[2] = btVec[2];
		return v;
	}

	static vec3f vec3f_from_eigen(const Eigen::Vector3f& p) noexcept(true)
	{
		noob::vec3f v;
		v.v[0] = p[0];
		v.v[1] = p[1];
		v.v[2] = p[2];
		return v;
	}

	// Whatever the hell you gotta do to compile sometimes, man...
	static vec3f vec3f_from_eigen_block(const Eigen::Block<const Eigen::Matrix<float, 4, 1>, 3, 1, false> n) noexcept(true)
	{
		noob::vec3f v;
		v.v[0] = n[0];
		v.v[1] = n[1];
		v.v[2] = n[2];
		return v;
	}

	static btVector3 vec3f_to_bullet(const noob::vec3f& arg)
	{
		btVector3 results(arg.v[0], arg.v[1], arg.v[2]);
		return results;
	}

	static versorf versorf_from_bullet(const btQuaternion & arg) noexcept(true)
	{
		noob::versorf qq;
		qq.q[0] = arg[0];
		qq.q[1] = arg[1];
		qq.q[2] = arg[2];
		qq.q[3] = arg[3];
		return qq;
	}

	static btQuaternion versorf_to_bullet(const noob::versorf & arg) noexcept(true)
	{
		btQuaternion results(arg.q[0], arg.q[1], arg.q[2], arg[3]);
		return results;
	}

	static versorf versorf_from_eigen(const Eigen::Quaternion<float>& arg) noexcept(true)
	{
		noob::versorf qq;
		qq.q[0] = arg.x();
		qq.q[1] = arg.y();
		qq.q[2] = arg.z();
		qq.q[3] = arg.w();
		return qq;
	}

	static mat4f mat4f_from_bullet(const btTransform arg) noexcept(true)
	{
		noob::mat4f t;
		arg.getOpenGLMatrix(&t.m[0]);
		return t;
	}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// VECTOR FUNCTIONS:
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	template <typename T>
		static noob::vec3_type<T> negate(const noob::vec3_type<T>& arg) noexcept(true)
		{
			return arg * -1.0;
		}

	template <typename T>
		static bool vec3_equality(const vec3_type<T> first, const vec3_type<T> second) noexcept(true)
		{
			for (size_t i = 0; i < 3; ++i)
			{
				if (!compare_floats(first.v[i], second.v[i])) return false;
			}
			return true;
		}

	template <typename T>
		static float length_squared(const vec3_type<T> v) noexcept(true)
		{
			return v.v[0] * v.v[0] + v.v[1] * v.v[1] + v.v[2] * v.v[2];
		}

	template <typename T>
		static float length(const vec3_type<T> v) noexcept(true)
		{
			return sqrt(v.v[0] * v.v[0] + v.v[1] * v.v[1] + v.v[2] * v.v[2]);
		}

	template <typename T>
		static vec3_type<T> normalize(const vec3_type<T> v) noexcept(true)
		{
			vec3_type<T> vb;
			float len = length(v);
			if (0.0 == len)
			{
				return vec3_type<T>(0.0, 0.0, 0.0);
			}
			vb.v[0] = v.v[0] / len;
			vb.v[1] = v.v[1] / len;
			vb.v[2] = v.v[2] / len;
			return vb;
		}

	template <typename T>
		static float dot(const vec3_type<T>& a, const vec3_type<T>& b) noexcept(true)
		{
			return a.v[0] * b.v[0] + a.v[1] * b.v[1] + a.v[2] * b.v[2];
		}

	template <typename T>
		static vec3_type<T> cross(const vec3_type<T>& a, const vec3_type<T>& b) noexcept(true)
		{
			float x = a.v[1] * b.v[2] - a.v[2] * b.v[1];
			float y = a.v[2] * b.v[0] - a.v[0] * b.v[2];
			float z = a.v[0] * b.v[1] - a.v[1] * b.v[0];
			return vec3_type<T>(x, y, z);
		}

	template <typename T>
		static float get_squared_dist(const vec3_type<T> from, const vec3_type<T> to) noexcept(true)
		{
			float x = (to.v[0] - from.v[0]) * (to.v[0] - from.v[0]);
			float y = (to.v[1] - from.v[1]) * (to.v[1] - from.v[1]);
			float z = (to.v[2] - from.v[2]) * (to.v[2] - from.v[2]);
			return x + y + z;
		}

	// Converts an un-normalized direction into a heading in degrees.
	template <typename T>
		static float direction_to_heading(const vec3_type<T> d) noexcept(true)
		{
			return atan2(-d.v[0], d.v[2]) * NOOB_ONE_RAD_IN_DEG;
		}

	template <typename T>
		static vec3_type<T> heading_to_direction(float degrees) noexcept(true)
		{
			float rad = degrees * NOOB_ONE_DEG_IN_RAD;
			return vec3_type<T>(-sinf(rad), 0.0f, -cosf(rad));
		}

	template <typename T>
		static bool linearly_dependent(const noob::vec3_type<T>& a, const noob::vec3_type<T>& b, const noob::vec3_type<T>& c) noexcept(true)
		{
			// if (a cross b) dot c = 0
			if (dot(cross(a, b), c) == 0.0) return true;
			else return false;
		}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// QUATERNION FUNCTIONS:
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	template <typename T>
		static versor_type<T> normalize(const versor_type<T>& q) noexcept(true)
		{
			// norm(q) = q / magnitude (q)
			// magnitude (q) = sqrt (w*w + x*x...)
			// only compute sqrt if interior sum != 1.0
			versor_type<T> qq(q);
			float sum = qq.q[0] * qq.q[0] + qq.q[1] * qq.q[1] + qq.q[2] * qq.q[2] + qq.q[3] * qq.q[3];
			// NB: floats have min 6 digits of precision
			const float thresh = 0.0001f;
			if (fabs (1.0f - sum) < thresh)
			{
				return q;
			}
			float mag = sqrt(sum);
			return qq / mag;
		}

	template <typename T>
		static float dot(const versor_type<T>& q, const versor_type<T>& r) noexcept(true)
		{
			return q.q[0] * r.q[0] + q.q[1] * r.q[1] + q.q[2] * r.q[2] + q.q[3] * r.q[3];
		}

	template <typename T>
		static versor_type<T> slerp(const versor_type<T>& q, const versor_type<T>& r, float t) noexcept(true)
		{
			versor_type<T> temp_q(q);
			// angle between q0-q1
			float cos_half_theta = dot(temp_q, r);
			// as found here http://stackoverflow.com/questions/2886606/flipping-issue-when-interpolating-rotations-using-quaternions
			// if dot product is negative then one quaternion should be negated, to make
			// it take the short way around, rather than the long way
			// yeah! and furthermore Susan, I had to recalculate the d.p. after this

			if (cos_half_theta < 0.0f)
			{
				for (int i = 0; i < 4; ++i)
				{
					temp_q.q[i] *= -1.0f;
				}
				cos_half_theta = dot(temp_q, r);
			}
			// if qa=qb or qa=-qb then theta = 0 and we can return qa
			if (fabs(cos_half_theta) >= 1.0f)
			{
				return temp_q;
			}
			// Calculate temporary values
			float sin_half_theta = sqrt(1.0f - cos_half_theta * cos_half_theta);
			// If theta = 180 degrees then result is not fully defined we could rotate around any axis normal to qa or qb
			versor_type<T> result;
			if (fabs(sin_half_theta) < 0.001f)
			{
				for (int i = 0; i < 4; i++)
				{
					result.q[i] = (1.0f - t) * temp_q.q[i] + t * r.q[i];
				}
				return result;
			}
			float half_theta = acos(cos_half_theta);
			float a = sin((1.0f - t) * half_theta) / sin_half_theta;
			float b = sin(t * half_theta) / sin_half_theta;
			for (int i = 0; i < 4; i++)
			{
				result.q[i] = temp_q.q[i] * a + r.q[i] * b;
			}
			return result;
		}

	template <typename T>
		static versor_type<T> versor_from_mat4(const mat4_type<T>& m) noexcept(true)
		{
			// Eigen::Map<const Eigen::Matrix4f> mat_map(&m.m[0]);
			// const Eigen::Quaternion<float> vv(mat_map); //Eigen::Matrix4f(&m.m[0]));
			// return versor_from_eigen(vv);
			glm::mat4 mm = glm::make_mat4(&m.m[0]);
			glm::quat q = glm::quat_cast(mm);
			noob::versor_type<T> qq;
			qq.q[0] = q[0];
			qq.q[1] = q[1];
			qq.q[2] = q[2];
			qq.q[3] = q[3];
			return qq;
		}

	template <typename T>
		static vec3_type<T> lerp(const noob::vec3_type<T>& a, const noob::vec3_type<T>& b, float t) noexcept(true)
		{
			return a + (b - a) * t;
		}

	template <typename T>
		static versor_type<T> versor_from_axis_rad(float radians, float x, float y, float z) noexcept(true)
		{
			versor_type<T> result;
			result.q[0] = cos (radians / 2.0);
			result.q[1] = sin (radians / 2.0) * x;
			result.q[2] = sin (radians / 2.0) * y;
			result.q[3] = sin (radians / 2.0) * z;
			return result;
		}
	template <typename T>
		static versor_type<T> versor_from_axis_deg(float degrees, float x, float y, float z) noexcept(true)
		{
			return versor_from_axis_rad<T>(NOOB_ONE_DEG_IN_RAD * degrees, x, y, z);
		}

	template <typename T>
		static mat4_type<T> versor_to_mat4(const noob::versor_type<T>& q) noexcept(true)
		{
			const float w = q.q[0];
			const float x = q.q[1];
			const float y = q.q[2];
			const float z = q.q[3];
			return mat4_type<T>(1.0f - 2.0f * y * y - 2.0f * z * z,
					2.0f * x * y + 2.0f * w * z,
					2.0f * x * z - 2.0f * w * y,
					0.0f,
					2.0f * x * y - 2.0f * w * z,
					1.0f - 2.0f * x * x - 2.0f * z * z,
					2.0f * y * z + 2.0f * w * x,
					0.0f,
					2.0f * x * z + 2.0f * w * y,
					2.0f * y * z - 2.0f * w * x,
					1.0f - 2.0f * x * x - 2.0f * y * y,
					0.0f,
					0.0f,
					0.0f,
					0.0f,
					1.0f
					);
		}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// MATRIX FUNCTIONS:
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	template <typename T>
		static mat3_type<T> zero_mat3() noexcept(true)
		{
			return mat3_type<T>(	0.0f, 0.0f, 0.0f,
					0.0f, 0.0f, 0.0f,
					0.0f, 0.0f, 0.0f);
		}
	template <typename T>
		static mat3_type<T> identity_mat3() noexcept(true)
		{
			return mat3_type<T>(	1.0f, 0.0f, 0.0f,
					0.0f, 1.0f, 0.0f,
					0.0f, 0.0f, 1.0f);
		}

	template <typename T>
		static mat4_type<T> zero_mat4() noexcept(true)
		{
			return mat4_type<T>(	0.0f, 0.0f, 0.0f, 0.0f,
					0.0f, 0.0f, 0.0f, 0.0f,
					0.0f, 0.0f, 0.0f, 0.0f,
					0.0f, 0.0f, 0.0f, 0.0f);
		}

	template <typename T>
		static mat4_type<T> identity_mat4() noexcept(true)
		{
			return mat4_type<T>(	1.0f, 0.0f, 0.0f, 0.0f,
					0.0f, 1.0f, 0.0f, 0.0f,
					0.0f, 0.0f, 1.0f, 0.0f,
					0.0f, 0.0f, 0.0f, 1.0f);
		}

	// Returns a scalar value with the determinant for a 4x4 matrix
	// see http://www.euclideanspace.com/maths/algebra/matrix/functions/determinant/fourD/index.htm
	template <typename T>
		static float determinant(const mat4_type<T>& mm) noexcept(true)
		{
			return
				mm.m[12] * mm.m[9] * mm.m[6] * mm.m[3] -
				mm.m[8] * mm.m[13] * mm.m[6] * mm.m[3] -
				mm.m[12] * mm.m[5] * mm.m[10] * mm.m[3] +
				mm.m[4] * mm.m[13] * mm.m[10] * mm.m[3] +
				mm.m[8] * mm.m[5] * mm.m[14] * mm.m[3] -
				mm.m[4] * mm.m[9] * mm.m[14] * mm.m[3] -
				mm.m[12] * mm.m[9] * mm.m[2] * mm.m[7] +
				mm.m[8] * mm.m[13] * mm.m[2] * mm.m[7] +
				mm.m[12] * mm.m[1] * mm.m[10] * mm.m[7] -
				mm.m[0] * mm.m[13] * mm.m[10] * mm.m[7] -
				mm.m[8] * mm.m[1] * mm.m[14] * mm.m[7] +
				mm.m[0] * mm.m[9] * mm.m[14] * mm.m[7] +
				mm.m[12] * mm.m[5] * mm.m[2] * mm.m[11] -
				mm.m[4] * mm.m[13] * mm.m[2] * mm.m[11] -
				mm.m[12] * mm.m[1] * mm.m[6] * mm.m[11] +
				mm.m[0] * mm.m[13] * mm.m[6] * mm.m[11] +
				mm.m[4] * mm.m[1] * mm.m[14] * mm.m[11] -
				mm.m[0] * mm.m[5] * mm.m[14] * mm.m[11] -
				mm.m[8] * mm.m[5] * mm.m[2] * mm.m[15] +
				mm.m[4] * mm.m[9] * mm.m[2] * mm.m[15] +
				mm.m[8] * mm.m[1] * mm.m[6] * mm.m[15] -
				mm.m[0] * mm.m[9] * mm.m[6] * mm.m[15] -
				mm.m[4] * mm.m[1] * mm.m[10] * mm.m[15] +
				mm.m[0] * mm.m[5] * mm.m[10] * mm.m[15];
		}

	// returns a 16-element array that is the inverse of a 16-element array (4x4 matrix). see http://www.euclideanspace.com/maths/algebra/matrix/functions/inverse/fourD/index.htm
	template <typename T>
		static mat4_type<T> inverse(const mat4_type<T>& mm) noexcept(true)
		{
			float det = determinant(mm);

			// There is no inverse if determinant is zero (not likely unless scale is broken)
			if (compare_floats(0.0, det))
			{
				// logger::log("WARNING. matrix has no determinant. can not invert");
				return mm;
			}

			float inv_det = 1.0f / det;

			return mat4_type<T> (
					inv_det * (
						mm.m[9] * mm.m[14] * mm.m[7] - mm.m[13] * mm.m[10] * mm.m[7] +
						mm.m[13] * mm.m[6] * mm.m[11] - mm.m[5] * mm.m[14] * mm.m[11] -
						mm.m[9] * mm.m[6] * mm.m[15] + mm.m[5] * mm.m[10] * mm.m[15]
						),
					inv_det * (
						mm.m[13] * mm.m[10] * mm.m[3] - mm.m[9] * mm.m[14] * mm.m[3] -
						mm.m[13] * mm.m[2] * mm.m[11] + mm.m[1] * mm.m[14] * mm.m[11] +
						mm.m[9] * mm.m[2] * mm.m[15] - mm.m[1] * mm.m[10] * mm.m[15]
						),
					inv_det * (
						mm.m[5] * mm.m[14] * mm.m[3] - mm.m[13] * mm.m[6] * mm.m[3] +
						mm.m[13] * mm.m[2] * mm.m[7] - mm.m[1] * mm.m[14] * mm.m[7] -
						mm.m[5] * mm.m[2] * mm.m[15] + mm.m[1] * mm.m[6] * mm.m[15]
						),
					inv_det * (
						mm.m[9] * mm.m[6] * mm.m[3] - mm.m[5] * mm.m[10] * mm.m[3] -
						mm.m[9] * mm.m[2] * mm.m[7] + mm.m[1] * mm.m[10] * mm.m[7] +
						mm.m[5] * mm.m[2] * mm.m[11] - mm.m[1] * mm.m[6] * mm.m[11]
						),
					inv_det * (
							mm.m[12] * mm.m[10] * mm.m[7] - mm.m[8] * mm.m[14] * mm.m[7] -
							mm.m[12] * mm.m[6] * mm.m[11] + mm.m[4] * mm.m[14] * mm.m[11] +
							mm.m[8] * mm.m[6] * mm.m[15] - mm.m[4] * mm.m[10] * mm.m[15]
						  ),
					inv_det * (
							mm.m[8] * mm.m[14] * mm.m[3] - mm.m[12] * mm.m[10] * mm.m[3] +
							mm.m[12] * mm.m[2] * mm.m[11] - mm.m[0] * mm.m[14] * mm.m[11] -
							mm.m[8] * mm.m[2] * mm.m[15] + mm.m[0] * mm.m[10] * mm.m[15]
						  ),
					inv_det * (
							mm.m[12] * mm.m[6] * mm.m[3] - mm.m[4] * mm.m[14] * mm.m[3] -
							mm.m[12] * mm.m[2] * mm.m[7] + mm.m[0] * mm.m[14] * mm.m[7] +
							mm.m[4] * mm.m[2] * mm.m[15] - mm.m[0] * mm.m[6] * mm.m[15]
						  ),
					inv_det * (
							mm.m[4] * mm.m[10] * mm.m[3] - mm.m[8] * mm.m[6] * mm.m[3] +
							mm.m[8] * mm.m[2] * mm.m[7] - mm.m[0] * mm.m[10] * mm.m[7] -
							mm.m[4] * mm.m[2] * mm.m[11] + mm.m[0] * mm.m[6] * mm.m[11]
						  ),
					inv_det * (
							mm.m[8] * mm.m[13] * mm.m[7] - mm.m[12] * mm.m[9] * mm.m[7] +
							mm.m[12] * mm.m[5] * mm.m[11] - mm.m[4] * mm.m[13] * mm.m[11] -
							mm.m[8] * mm.m[5] * mm.m[15] + mm.m[4] * mm.m[9] * mm.m[15]
						  ),
					inv_det * (
							mm.m[12] * mm.m[9] * mm.m[3] - mm.m[8] * mm.m[13] * mm.m[3] -
							mm.m[12] * mm.m[1] * mm.m[11] + mm.m[0] * mm.m[13] * mm.m[11] +
							mm.m[8] * mm.m[1] * mm.m[15] - mm.m[0] * mm.m[9] * mm.m[15]
						  ),
					inv_det * (
							mm.m[4] * mm.m[13] * mm.m[3] - mm.m[12] * mm.m[5] * mm.m[3] +
							mm.m[12] * mm.m[1] * mm.m[7] - mm.m[0] * mm.m[13] * mm.m[7] -
							mm.m[4] * mm.m[1] * mm.m[15] + mm.m[0] * mm.m[5] * mm.m[15]
						  ),
					inv_det * (
							mm.m[8] * mm.m[5] * mm.m[3] - mm.m[4] * mm.m[9] * mm.m[3] -
							mm.m[8] * mm.m[1] * mm.m[7] + mm.m[0] * mm.m[9] * mm.m[7] +
							mm.m[4] * mm.m[1] * mm.m[11] - mm.m[0] * mm.m[5] * mm.m[11]
						  ),
					inv_det * (
							mm.m[12] * mm.m[9] * mm.m[6] - mm.m[8] * mm.m[13] * mm.m[6] -
							mm.m[12] * mm.m[5] * mm.m[10] + mm.m[4] * mm.m[13] * mm.m[10] +
							mm.m[8] * mm.m[5] * mm.m[14] - mm.m[4] * mm.m[9] * mm.m[14]
						  ),
					inv_det * (
							mm.m[8] * mm.m[13] * mm.m[2] - mm.m[12] * mm.m[9] * mm.m[2] +
							mm.m[12] * mm.m[1] * mm.m[10] - mm.m[0] * mm.m[13] * mm.m[10] -
							mm.m[8] * mm.m[1] * mm.m[14] + mm.m[0] * mm.m[9] * mm.m[14]
						  ),
					inv_det * (
							mm.m[12] * mm.m[5] * mm.m[2] - mm.m[4] * mm.m[13] * mm.m[2] -
							mm.m[12] * mm.m[1] * mm.m[6] + mm.m[0] * mm.m[13] * mm.m[6] +
							mm.m[4] * mm.m[1] * mm.m[14] - mm.m[0] * mm.m[5] * mm.m[14]
						  ),
					inv_det * (
							mm.m[4] * mm.m[9] * mm.m[2] - mm.m[8] * mm.m[5] * mm.m[2] +
							mm.m[8] * mm.m[1] * mm.m[6] - mm.m[0] * mm.m[9] * mm.m[6] -
							mm.m[4] * mm.m[1] * mm.m[10] + mm.m[0] * mm.m[5] * mm.m[10]
						  )
						);
		}

	// Returns a 16-element array flipped on the main diagonal
	template <typename T>
		static mat4_type<T> transpose(const mat4_type<T>& mm) noexcept(true)
		{
			return mat4_type<T>(
					mm.m[0], mm.m[4], mm.m[8], mm.m[12],
					mm.m[1], mm.m[5], mm.m[9], mm.m[13],
					mm.m[2], mm.m[6], mm.m[10], mm.m[14],
					mm.m[3], mm.m[7], mm.m[11], mm.m[15]
					);
		}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// AFFINE MATRIX FUNCTIONS:
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	template <typename T>
		static mat4_type<T> translate(const mat4_type<T>& m, const vec3_type<T> v) noexcept(true)
		{
			mat4_type<T> m_t = identity_mat4<T>();
			m_t.m[12] = v.v[0];
			m_t.m[13] = v.v[1];
			m_t.m[14] = v.v[2];
			return m_t * m;
		}

	template <typename T>
		static mat4_type<T> rotate(const mat4_type<T>& m, const versor_type<T>& v) noexcept(true)
		{
			return versor_to_mat4(v) * m;
		}

	template <typename T>
		static mat4_type<T> rotate_x_deg(const mat4_type<T>& m, float deg) noexcept(true)
		{
			// convert to radians
			float rad = deg * NOOB_ONE_DEG_IN_RAD;
			mat4_type<T> m_r = identity_mat4<T>();
			m_r.m[5] = cos(rad);
			m_r.m[9] = -sin(rad);
			m_r.m[6] = sin(rad);
			m_r.m[10] = cos(rad);
			return m_r * m;
		}

	template <typename T>
		static mat4_type<T> rotate_y_deg(const mat4_type<T>& m, float deg) noexcept(true)
		{
			// convert to radians
			const float rad = deg * NOOB_ONE_DEG_IN_RAD;
			mat4_type<T> m_r = identity_mat4<T>();
			m_r.m[0] = cos(rad);
			m_r.m[8] = sin(rad);
			m_r.m[2] = -sin(rad);
			m_r.m[10] = cos(rad);
			return m_r * m;
		}

	template <typename T>
		static mat4_type<T> rotate_z_deg(const mat4_type<T>& m, float deg) noexcept(true)
		{
			// convert to radians
			const float rad = deg * NOOB_ONE_DEG_IN_RAD;
			mat4_type<T> m_r = identity_mat4<T>();
			m_r.m[0] = cos(rad);
			m_r.m[4] = -sin(rad);
			m_r.m[1] = sin(rad);
			m_r.m[5] = cos(rad);
			return m_r * m;
		}

	template <typename T>
		static mat4_type<T> scale(const mat4_type<T>& m, const vec3_type<T> v) noexcept(true)
		{
			mat4_type<T> a = identity_mat4<T>();
			a.m[0] = v.v[0];
			a.m[5] = v.v[1];
			a.m[10] = v.v[2];
			return a * m;
		}
	template <typename T>
		static vec3_type<T> get_normal(const std::array<vec3_type<T>, 3>& vertices) noexcept(true)
		{
			// Note: We onlt need the first three points on the face to calculate its normal.
			noob::vec3_type<T> u = vertices[1] - vertices[0];
			noob::vec3_type<T> v = vertices[2] - vertices[0];
			noob::vec3_type<T> results = noob::cross(u, v);
			return normalize(results);
		}

	// TODO: Implement. PITA (?)
	// vec4 rotation_from_mat4(const mat4& m);
	template <typename T>
		static vec3_type<T> translation_from_mat4(const mat4_type<T>& m) noexcept(true)
		{
			vec3_type<T> v;
			v.v[0] = m.m[12];
			v.v[1] = m.m[13];
			v.v[2] = m.m[14];
			return v;
		}

	template <typename T>
		static vec3_type<T> scale_from_mat4(const mat4_type<T>& m) noexcept(true)
		{
			vec3_type<T> v;
			v.v[0] = m.m[0];
			v.v[1] = m.m[5];
			v.v[2] = m.m[10];
			return v;
		}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// CAMERA FUNCTIONS:
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

	// Returns a view matrix using the opengl lookAt style. COLUMN ORDER:
	template <typename T>
		static mat4_type<T> look_at(const vec3_type<T> cam_pos, const vec3_type<T> targ_pos, const vec3_type<T> up) noexcept(true)
		{
			// inverse translation
			mat4_type<T> p = identity_mat4<T>();
			p = translate(p, vec3_type<T>(-cam_pos[0], -cam_pos[1], -cam_pos[2]));
			// distance vector
			vec3_type<T> d = targ_pos - cam_pos;
			// forward vector
			vec3_type<T> f = normalize(d);
			// right vector
			vec3_type<T> r = normalize(cross(f, up));
			// real up vector
			vec3_type<T> u = normalize(cross(r, f));
			mat4_type<T> ori = identity_mat4<T>();
			ori.m[0] = r.v[0];
			ori.m[4] = r.v[1];
			ori.m[8] = r.v[2];
			ori.m[1] = u.v[0];
			ori.m[5] = u.v[1];
			ori.m[9] = u.v[2];
			ori.m[2] = -f.v[0];
			ori.m[6] = -f.v[1];
			ori.m[10] = -f.v[2];

			return ori * p;//p * ori;
		}

	// returns a perspective function mimicking the opengl projection style.
	template <typename T>
		static mat4_type<T> perspective(float fovy, float aspect, float near, float far) noexcept(true)
		{
			float fov_rad = fovy * NOOB_ONE_DEG_IN_RAD;
			float range = tan (fov_rad / 2.0f) * near;
			float sx = (2.0f * near) / (range * aspect + range * aspect);
			float sy = near / range;
			float sz = -(far + near) / (far - near);
			float pz = -(2.0f * far * near) / (far - near);
			mat4_type<T> m = zero_mat4<T>(); // make sure bottom-right corner is zero
			m.m[0] = sx;
			m.m[5] = sy;
			m.m[10] = sz;
			m.m[14] = pz;
			m.m[11] = -1.0f;
			return m;
		}

	/*
	   0 4 8  12
	   1 5 9  13
	   2 6 10 14
	   3 7 11 15
	 */
	template <typename T>
		static mat4_type<T> ortho(float left, float right, float bottom, float top, float near, float far) noexcept(true)
		{
			mat4_type<T> m = zero_mat4<T>();
			m.m[0] = 2.0/(right-left);
			m.m[5] = 2.0/(top - bottom);
			m.m[10] = -2.0/(far - near);
			m.m[12] = (right + left)/(right - left);
			m.m[13] = (top + bottom)/(top - bottom);
			m.m[14] = (far + near)/(far - near);
			m.m[15] = 1.0;
			return m;
		}

	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	// GEOMETRY UTILITY FUNCTIONS:
	//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
	template <typename T>
		static bbox_type<T> update_bbox_type(const noob::bbox_type<T>& b, const noob::vec3_type<T>& v)
		{
			bbox_type<T> results;

			results.min[0] = std::min(b.min[0], v[0]);
			results.min[1] = std::min(b.min[1], v[1]);
			results.min[2] = std::min(b.min[2], v[2]);

			results.max[0] = std::max(b.max[0], v[0]);
			results.max[1] = std::max(b.max[1], v[1]);
			results.max[2] = std::max(b.max[2], v[2]);

			return results;
		}

	template <typename T>
		static bbox_type<T> update_bbox_type(const noob::bbox_type<T>& a, const noob::bbox_type<T>& b)
		{
			noob::bbox_type<T> results;

			results = update_bbox_type(a, b.min);
			results = update_bbox_type(results, b.max);

			return results;
		}
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 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_FUNCTION_SAFE_MIN_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SAFE_MIN_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing safe_min capabilities

    Returns a safe mininimal value relative to the input, i.e. a
    value which will not underflow when divided by the input.

    @par Semantic:

    @code
    T r = safe_min(x);
    @endcode

    is similar to:

    @code
    T r =  x ? Sqrtsmallestposval<T>()*abs(x);
    @endcode

    @see Sqrtsmallestposval

  **/
  const boost::dispatch::functor<tag::safe_min_> safe_min = {};
} }
#endif

#include <boost/simd/function/scalar/safe_min.hpp>
#include <boost/simd/function/simd/safe_min.hpp>

#endif


/*
 * random.cpp
 *
 *  Created on: Jan 30, 2016
 *      Author: Jeff Idol
 */

#include "random.h"
#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp>

random::random() {
	// TODO Auto-generated constructor stub

}

random::~random() {
	// TODO Auto-generated destructor stub
}

std::vector<long> random::randomLong(int size, long start, long end)
{
	boost::random::mt19937 gen(std::time(0));
	std::vector<long> output;
	boost::random::uniform_int_distribution<> dist(start, end);

	for(int i=0; i < size; ++i)
	{
		output.push_back(dist(gen));
	}

	return output;
}


//  (C) Copyright Gennadiy Rozental 2011-2014.
//  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)

//  See http://www.boost.org/libs/test for the library home page.
//
///@file
///Defines range generator
// ***************************************************************************

#ifndef BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_XRANGE_HPP_112011GER
#define BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_XRANGE_HPP_112011GER

// Boost.Test
#include <boost/test/data/config.hpp>

#include <boost/test/data/monomorphic/generators/keywords.hpp>
#include <boost/test/data/monomorphic/generate.hpp>

// Boost
#include <boost/optional.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/type_traits/is_unsigned.hpp>

// STL
#include <limits>
#include <cmath>

#include <boost/test/detail/suppress_warnings.hpp>

//____________________________________________________________________________//

namespace boost {
namespace unit_test {
namespace data {
namespace monomorphic {

// ************************************************************************** //
// **************             monomorphic::xrange_t            ************** //
// ************************************************************************** //

/*!@brief Generator for the range sequences
 *
 * This class implements the generator concept (see @ref boost::unit_test::data::monomorphic::generated_by) for implementing
 * a range like sequence of numbers.
 */
template<typename SampleType, typename StepType=SampleType>
class xrange_t {
public:
    typedef SampleType data_type;

    xrange_t( SampleType const& begin, StepType const& step, data::size_t size )
    : m_begin( begin )
    , m_curr( begin )
    , m_step( step )
    , m_index( 0 )
    , m_size( size )
    {}

    // Generator interface
    data::size_t    capacity() const { return m_size; }
    SampleType      next()
    {
        if( m_index == m_size )
            return m_curr;

        SampleType res = m_curr;

        m_curr += m_step;
        ++m_index;

        return res;
    }
    void                reset()
    {
        m_curr  = m_begin;
        m_index = 0;
    }

private:
    // Data members
    SampleType      m_begin;
    SampleType      m_curr;
    StepType        m_step;
    data::size_t    m_index;
    data::size_t    m_size;
};

//____________________________________________________________________________//

namespace ds_detail {

template<typename SampleType, typename StepType=SampleType>
struct make_xrange {
    static StepType    abs( StepType s, boost::true_type* )   { return s; }
    static StepType    abs( StepType s, boost::false_type* )  { return std::abs(s); }

    typedef xrange_t<SampleType, StepType> range_gen;

    template<typename Params>
    static generated_by<range_gen>
    _( Params const& params )
    {
        SampleType           begin_val  = params.has( data::begin )  ? params[data::begin] : SampleType();
        optional<SampleType> end_val    = params.has( data::end )    ? params[data::end]   : optional<SampleType>();
        StepType             step_val   = params.has( data::step )   ? params[data::step]  : 1;

        BOOST_TEST_DS_ASSERT( step_val != 0, "Range step can't be zero" );

        data::size_t size;
        if( !end_val.is_initialized() )
            size = BOOST_TEST_DS_INFINITE_SIZE;
        else {
            BOOST_TEST_DS_ASSERT( (step_val < 0) ^ (begin_val < *end_val), "Invalid step direction" );

            SampleType  abs_distance    = step_val < 0 ? begin_val - *end_val : *end_val-begin_val;
            StepType    abs_step        = make_xrange::abs(step_val, (typename boost::is_unsigned<StepType>::type*)0 );
            std::size_t s = static_cast<std::size_t>(abs_distance/abs_step);

            if( static_cast<SampleType>(s*abs_step) < abs_distance )
                s++;

            size = s;
        }

        return generated_by<range_gen>( range_gen( begin_val, step_val, size ) );
    }
};

} // namespace ds_detail
} // namespace monomorphic

//____________________________________________________________________________//

//! Creates a range (sequence) dataset.
//!
//! The following overloads are available:
//! @code
//! auto d = xrange();
//! auto d = xrange(end_val);
//! auto d = xrange(end_val, param);
//! auto d = xrange(begin_val, end_val);
//! auto d = xrange(begin_val, end_val, step_val);
//! auto d = xrange(param);
//! @endcode
//!
//! - @c begin_val indicates the start of the sequence (default to 0).
//! - @c end_val is the end of the sequence. If ommited, the dataset has infinite size.\n
//! - @c step_val is the step between two consecutive elements of the sequence, and defaults to 1.\n
//! - @c param is the named parameters that describe the sequence. The following parameters are accepted:
//!   - @c begin: same meaning @c begin_val
//!   - @c end: same meaning as @c end_val
//!   - @c step: same meaning as @c step_val
//!
//!
//! The returned value is an object that implements the dataset API.
//!
//! @note the step size cannot be null, and it should be positive if @c begin_val < @c end_val, negative otherwise.
template<typename SampleType, typename Params>
inline monomorphic::generated_by<monomorphic::xrange_t<SampleType> >
xrange( Params const& params )
{
    return monomorphic::ds_detail::make_xrange<SampleType>::_( params );
}

//____________________________________________________________________________//

/// @overload boost::unit_test::data::xrange()
template<typename SampleType>
inline monomorphic::generated_by<monomorphic::xrange_t<SampleType> >
xrange( SampleType const& end_val )
{
    return monomorphic::ds_detail::make_xrange<SampleType>::_( data::end=end_val );
}

//____________________________________________________________________________//

/// @overload boost::unit_test::data::xrange()
template<typename SampleType, typename Params>
inline typename enable_if_c<nfp::is_named_params<Params>::value,monomorphic::generated_by<monomorphic::xrange_t<SampleType> > >::type
xrange( SampleType const& end_val, Params const& params )
{
    return monomorphic::ds_detail::make_xrange<SampleType>::
            _(( params, data::end=end_val ));
}

//____________________________________________________________________________//

/// @overload boost::unit_test::data::xrange()
template<typename SampleType>
inline monomorphic::generated_by<monomorphic::xrange_t<SampleType> >
xrange( SampleType const& begin_val, SampleType const& end_val )
{
    return monomorphic::ds_detail::make_xrange<SampleType>::
            _(( data::begin=begin_val, data::end=end_val ));
}

//____________________________________________________________________________//

/// @overload boost::unit_test::data::xrange()
template<typename SampleType,typename StepType>
inline monomorphic::generated_by<monomorphic::xrange_t<SampleType> >
xrange( SampleType const& begin_val, SampleType const& end_val, StepType const& step_val )
{
    return monomorphic::ds_detail::make_xrange<SampleType,StepType>::
            _(( data::begin=begin_val, data::end=end_val, data::step=step_val ));
}

//____________________________________________________________________________//

} // namespace data
} // namespace unit_test
} // namespace boost

#include <boost/test/detail/enable_warnings.hpp>

#endif // BOOST_TEST_DATA_MONOMORPHIC_GENERATORS_XRANGE_HPP_112011GER


//
// Created by Alex Beccaro on 18/01/18.
//

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>
#include "../../src/problems/1-50/48/problem48.hpp"

BOOST_AUTO_TEST_SUITE( Problem48 )

    BOOST_AUTO_TEST_CASE( Example ) {
        auto res = problems::problem48::solve(10);
        std::vector<uint32_t> sol = {0, 4, 0, 5, 0, 7, 1, 3, 1, 7};
        BOOST_CHECK_EQUAL_COLLECTIONS(res.begin(), res.end(), sol.begin(), sol.end());
    }

    BOOST_AUTO_TEST_CASE( Solution ) {
        auto res = problems::problem48::solve();
        std::vector<uint32_t> sol = {9, 1, 1, 0, 8, 4, 6, 7, 0, 0};
        BOOST_CHECK_EQUAL_COLLECTIONS(res.begin(), res.end(), sol.begin(), sol.end());
    }

BOOST_AUTO_TEST_SUITE_END()


// Copyright [2020] Yoonyoung (Jamie) Cho

#pragma once

#include <vector>

#include <Eigen/Core>
#include <Eigen/StdVector>

namespace cho {

using MatrixXu = Eigen::Matrix<std::uint8_t, Eigen::Dynamic, Eigen::Dynamic>;
using Vector2i = Eigen::Vector2i;
using VecVector2i = std::vector<Vector2i>;

namespace graph {
VecVector2i FindPath(const MatrixXu& grid, const Vector2i& iv0,
                     const Vector2i& iv1);
}

}  // namespace cho


/********************************************************************************
  Copyright 2017 Frank Imeson and Stephen L. Smith

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

#ifndef SAT_H		// guard
#define SAT_H

/********************************************************************************
 * INCLUDE
 ********************************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <time.h>
#include <ctime>
#include <string>
#include <iostream>
#include <sstream>
#include <vector>
#include <algorithm>
#include <sys/time.h>
#include <boost/format.hpp>
#include <boost/algorithm/string.hpp>

#include "tsp.hpp"
#include "theories.hpp"
#include "formula.hpp"
#include "minisat/core/Solver.h"

using namespace std;
//using namespace boost::timer;
using namespace Minisat;

/********************************************************************************
 * Defs
 ********************************************************************************/
#ifdef VER100
    #define LIB_VER   1.00
#else
    #define LIB_VER   1.10
#endif
enum SEARCH_METHOD {LINEAR, BINARY, ADAPTIVE_BINARY};

/********************************************************************************
 * Prototypes
 ********************************************************************************/

class SATTSP {
  private:
    Solver              *original_formula;
    Solver              *formula;
    TSP                 *graph;
    MST                 *mst;
    string              sat_filename, tsp_filename;
    int                 verbose_level;
    bool                edge_theory, vertex_theory, lkh_theory, mst_theory, solving;
    double              reduction_time, solver_time, theory_time;
    int                 minisat_nDecisions, minisat_nLearnts;
    int                 cb_minisat_error;
    int                 new_clauses;
    timeval             max_wall_time;
    int                 search_method, bdiv_parameter;

    vector< vector<Theory*> >   var_theories;
    vector<Theory*>             theories;
    TSP_Theory                  *tsp_theory;

    int                 soln_cost, tsp_cost_budget, subgraph_cost_budget;
    vector<int>         soln_tour;
    vector<bool>        soln_assigns;

    bool                clause_checked, vid_conflict, eid_conflict, mst_valid;
    vector<int>         soln_vids, soln_eids;
    int                 solver_time_budget, usat_time_budget;
    int64_t             conflict_budget, propagation_budget;

    static void minisat_trail_push_cb_wrapper (
        void* object_pointer,
        const VMap<lbool> &assigns,
        const vec<Lit>& trail,
        vec<Lit>& propagate_list);

    static void minisat_trail_shrink_cb_wrapper (
        void* object_pointer,
        const VMap<lbool> &assigns,
        const vec<Lit> &trail,
        int amount);

    static bool minisat_check_conflict_cb_wrapper (
        void* object_pointer,
        const VMap<lbool> &assigns,
        const vec<Lit> &trail,
        vec<Lit> &conflict_list);

    static bool minisat_within_budget_cb_wrapper (
        void* object_pointer);

    void reset_formula();
    string output_solution (const vector<int> &tour, const vector<bool> &assigns);

  public:
    SATTSP (string sat_filename, string tsp_filename, int verbose_level=-1);

    bool solve (int tsp_cost_budget, int subgraph_cost_budget);
    bool solve_optimal (int verbose_level=2);

    string output_conf();
    string output_stats();
    string output_solution();
    void print_smt2(int tsp_cost_budget);
    void print_cnf_smt2();

    void get_solution ();
    bool verify_soln (const int &cost, const vector<int> &tour, const vector<bool> &assigns, int verbose_level=-1);
    bool verify_soln (int verbose_level=-1);

    bool enable_linear_search();
    bool enable_adaptive_search();
    bool enable_edge_theory();
    bool enable_lkh_theory();
    bool enable_mst_theory();

    bool set_bdiv_parameter(int x) {bdiv_parameter = x;}

    bool setConfBudget(int64_t x)         {conflict_budget    = x;}
    bool setPropBudget(int64_t x)         {propagation_budget = x;}
    void setTimeBudget(int x)             {solver_time_budget = x;}
    void setUsatTimeBudget(int x)         {usat_time_budget   = x;}
    void assume_metric() {graph->assume_metric();};
    void assume_non_metric() {graph->assume_non_metric();};
    void assume_symmetric() {graph->assume_symmetric();};
    void assume_non_symmetric() {graph->assume_non_symmetric();};
    void assume_tsp_monotonic() {graph->assume_tsp_monotonic();};
    void assume_non_tsp_monotonic() {graph->assume_non_tsp_monotonic();};
    void set_lkh_parameters(string input_file) {graph->set_lkh_parameters(input_file);};
    void set_cb_interval(int x) {tsp_theory->cb_interval = x;};
    string get_lkh_parameters() {return graph->get_lkh_parameters();};
    bool set_search_method(string method);
};

#endif


// This file is part of Eigen, a lightweight C++ template library
// for linear algebra. Eigen itself is part of the KDE project.
//
// Copyright (C) 2008 Benoit Jacob <jacob.benoit.1@gmail.com>
//
// Eigen is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 3 of the License, or (at your option) any later version.
//
// Alternatively, 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.
//
// Eigen 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 Lesser General Public License or the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License and a copy of the GNU General Public License along with
// Eigen. If not, see <http://www.gnu.org/licenses/>.

#include "main.h"
#include <Eigen/LeastSquares>

template<typename VectorType,
         typename HyperplaneType>
void makeNoisyCohyperplanarPoints(int numPoints,
                                  VectorType **points,
                                  HyperplaneType *hyperplane,
                                  typename VectorType::Scalar noiseAmplitude)
{
  typedef typename VectorType::Scalar Scalar;
  const int size = points[0]->size();
  // pick a random hyperplane, store the coefficients of its equation
  hyperplane->coeffs().resize(size + 1);
  for(int j = 0; j < size + 1; j++)
  {
    do {
      hyperplane->coeffs().coeffRef(j) = ei_random<Scalar>();
    } while(ei_abs(hyperplane->coeffs().coeff(j)) < 0.5);
  }

  // now pick numPoints random points on this hyperplane
  for(int i = 0; i < numPoints; i++)
  {
    VectorType& cur_point = *(points[i]);
    do
    {
      cur_point = VectorType::Random(size)/*.normalized()*/;
      // project cur_point onto the hyperplane
      Scalar x = - (hyperplane->coeffs().start(size).cwise()*cur_point).sum();
      cur_point *= hyperplane->coeffs().coeff(size) / x;
    } while( cur_point.norm() < 0.5
          || cur_point.norm() > 2.0 );
  }

  // add some noise to these points
  for(int i = 0; i < numPoints; i++ )
    *(points[i]) += noiseAmplitude * VectorType::Random(size);
}

template<typename VectorType,
         typename HyperplaneType>
void check_fitHyperplane(int numPoints,
                         VectorType **points,
                         const HyperplaneType& original,
                         typename VectorType::Scalar tolerance)
{
  int size = points[0]->size();
  HyperplaneType result(size);
  fitHyperplane(numPoints, points, &result);
  result.coeffs() *= original.coeffs().coeff(size)/result.coeffs().coeff(size);
  typename VectorType::Scalar error = (result.coeffs() - original.coeffs()).norm() / original.coeffs().norm();
  VERIFY(ei_abs(error) < ei_abs(tolerance));
}

void test_regression()
{
  for(int i = 0; i < g_repeat; i++)
  {
    {
      Vector2f points2f [1000];
      Vector2f *points2f_ptrs [1000];
      for(int i = 0; i < 1000; i++) points2f_ptrs[i] = &(points2f[i]);
      Hyperplane<float,2> coeffs3f;
      makeNoisyCohyperplanarPoints(1000, points2f_ptrs, &coeffs3f, 0.01f);
      CALL_SUBTEST(check_fitHyperplane(10, points2f_ptrs, coeffs3f, 0.05f));
      CALL_SUBTEST(check_fitHyperplane(100, points2f_ptrs, coeffs3f, 0.01f));
      CALL_SUBTEST(check_fitHyperplane(1000, points2f_ptrs, coeffs3f, 0.002f));
    }

    {
      Vector4d points4d [1000];
      Vector4d *points4d_ptrs [1000];
      for(int i = 0; i < 1000; i++) points4d_ptrs[i] = &(points4d[i]);
      Hyperplane<double,4> coeffs5d;
      makeNoisyCohyperplanarPoints(1000, points4d_ptrs, &coeffs5d, 0.01);
      CALL_SUBTEST(check_fitHyperplane(10, points4d_ptrs, coeffs5d, 0.05));
      CALL_SUBTEST(check_fitHyperplane(100, points4d_ptrs, coeffs5d, 0.01));
      CALL_SUBTEST(check_fitHyperplane(1000, points4d_ptrs, coeffs5d, 0.002));
    }

    {
      VectorXcd *points11cd_ptrs[1000];
      for(int i = 0; i < 1000; i++) points11cd_ptrs[i] = new VectorXcd(11);
      Hyperplane<std::complex<double>,Dynamic> *coeffs12cd = new Hyperplane<std::complex<double>,Dynamic>(11);
      makeNoisyCohyperplanarPoints(1000, points11cd_ptrs, coeffs12cd, 0.01);
      CALL_SUBTEST(check_fitHyperplane(100, points11cd_ptrs, *coeffs12cd, 0.025));
      CALL_SUBTEST(check_fitHyperplane(1000, points11cd_ptrs, *coeffs12cd, 0.006));
      delete coeffs12cd;
      for(int i = 0; i < 1000; i++) delete points11cd_ptrs[i];
    }
  }
}


/**
 * @file test_vector_max_simd.cpp
 * @author Bensuperpc (bensuperpc@gmail.com)
 * @brief
 * @version 1.0.0
 * @date 2021-04-01
 *
 * MIT License
 *
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE vector_max_simd

#include <algorithm>
#include <boost/predef.h>
#include <random>

#if BOOST_COMP_GNUC
extern "C"
{
#    include "quadmath.h"
}
#endif

#include <boost/multiprecision/cpp_int.hpp>
#if BOOST_COMP_GNUC
#    include <boost/multiprecision/float128.hpp>
#endif
#include <boost/test/unit_test.hpp>
#include "vector/vector_avx.hpp"

BOOST_AUTO_TEST_CASE(test_vector_max_simd_1)
{
    const size_t i = 8192;
    int *n = new int[i];
    for (size_t x = 0; x < i; ++x) {
        n[x] = rand() % 2000000000;
    }
#if (__AVX2__ || __AVX__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_normal != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_sse(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_sse != find_max_avx");
//#endif
//#if (__SSE3__ || __SSE2__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_sse(n, i), "find_max_normal != find_max_sse");
#endif
}

BOOST_AUTO_TEST_CASE(test_vector_max_simd_2)
{
    const size_t i = 8192;
    int *n = new int[i];
    for (size_t x = 0; x < i; ++x) {
        n[x] = rand() % 8000;
    }
#if (__AVX2__ || __AVX__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_normal != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_sse(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_sse != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_sse(n, i), "find_max_normal != find_max_sse");
#endif
}

BOOST_AUTO_TEST_CASE(test_vector_max_simd_3)
{
    const size_t i = 1024;
    int *n = new int[i];
    for (size_t x = 0; x < i; ++x) {
        n[x] = rand() % 1000;
    }
#if (__AVX2__ || __AVX__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_normal != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_sse(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_sse != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_sse(n, i), "find_max_normal != find_max_sse");
#endif
}

BOOST_AUTO_TEST_CASE(test_vector_max_simd_4)
{
    const size_t i = 8192;
    int *n = new int[i];
    for (size_t x = 0; x < i; ++x) {
        n[x] = 1000;
    }
#if (__AVX2__ || __AVX__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_normal != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_sse(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_sse != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_sse(n, i), "find_max_normal != find_max_sse");
#endif
}

BOOST_AUTO_TEST_CASE(test_vector_max_simd_5)
{
    const size_t i = 16384;
    int *n = new int[i];
    for (size_t x = 0; x < i; ++x) {
        n[x] = 0;
    }
#if (__AVX2__ || __AVX__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_normal != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_sse(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_sse != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_sse(n, i), "find_max_normal != find_max_sse");
#endif
}

BOOST_AUTO_TEST_CASE(test_vector_max_simd_6)
{
    const size_t i = 16384;
    int *n = new int[i];
    for (size_t x = 0; x < i; ++x) {
        n[x] = 2147483647;
    }
#if (__AVX2__ || __AVX__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_normal != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_sse(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_sse != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_sse(n, i), "find_max_normal != find_max_sse");
#endif
}

BOOST_AUTO_TEST_CASE(test_vector_max_simd_7)
{
    const size_t i = 16384;
    int *n = new int[i];
    for (size_t x = 0; x < i; ++x) {
        n[x] = -2147483647;
    }
#if (__AVX2__ || __AVX__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_normal != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_sse(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_sse != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_sse(n, i), "find_max_normal != find_max_sse");
#endif
}

BOOST_AUTO_TEST_CASE(test_vector_max_simd_8)
{
    const size_t i = 16384;
    int *n = new int[i];
    for (size_t x = 0; x < i; ++x) {
        n[x] = rand() % 100000;
    }
#if (__AVX2__ || __AVX__)
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_normal != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_sse(n, i) == my::vector_avx::find_max_avx(n, i), "find_max_sse != find_max_avx");
    BOOST_REQUIRE_MESSAGE(my::vector_avx::find_max_normal(n, i) == my::vector_avx::find_max_sse(n, i), "find_max_normal != find_max_sse");
#endif
}


#pragma once

#include <metaSMT/API/Assertion.hpp>
#include <metaSMT/support/cardinality.hpp>

#include <boost/foreach.hpp>
#include <boost/timer.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/tuple/tuple_comparison.hpp>
#include <boost/tuple/tuple_io.hpp>

#include <iostream>

using namespace metaSMT;
using namespace metaSMT::logic;

template <typename Context>
struct SynthNet
{
  Context ctx;

  typedef boost::multi_array<Level*, 2> NetType;
  typedef boost::tuple<unsigned, unsigned> Tuple;
  typedef std::vector<Tuple> ExchangeGates;
  typedef std::map<Tuple, predicate> ExchangeIndex;
  typedef std::vector<ExchangeIndex> GateVariables;

  unsigned num_lines, num_levels, num_cex;
  NetType net;
  NetType::extent_gen extents;
  GateVariables gate_vars;
  ExchangeGates all_gates;

  SynthNet(unsigned lines)
  : num_lines(lines)
  , num_levels(1)
  , num_cex(0)
  {
    for (unsigned i = 0; i < lines; i++) {
      for (unsigned j = 0; j < lines; j++) {
        if (i<j) {
          all_gates.push_back(Tuple (i,j));
        }
      }
    }
    add_level_vars();
  }

  ~SynthNet() {
    // TODO: delete Levels
  }

  // add sortnet for counterexample
  //
  // 1 \/ x --\  /--y
  // 0 /\ x \/.\/...y ...
  // 1 \/ x /\./\...y
  // 0 /\ x --/  \--y
  //
  void add_counterexample( std::string const & cex) {
    //assert(num_cex < 5);

    unsigned ncx = num_cex;
    ++num_cex;
    net.resize(extents[num_cex][num_levels]);
    for (unsigned i = 0; i < num_levels; i++) {
      //std::cout << "creating CEX " << ncx << "," << i << "\n";
      net[ncx][i] = new Level(num_lines);
      if( i!=0 ) {
        connect(ncx, i-1, i);
      } else {
        comment(ctx, "equal to cex");
        metaSMT::assertion(ctx,
          net[ncx][i]->equal_to(ctx, cex)
        );
      }
    }
  }

  void add_level( ) {
    add_level_vars();
    unsigned nlv = num_levels;
    ++num_levels;
    std::cout << "adding level to " << num_cex << " " << num_levels << std::endl;
    net.resize(extents[num_cex][num_levels]);
    for (unsigned i = 0; i < num_cex; i++) {
      //std::cout << "creating Level " << i << "," << nlv << "\n";
      net[i][nlv] = new Level (num_lines);
      connect(i, nlv-1, nlv);
    }
  }

  void assumeSorted() {
    for (unsigned i = 0; i < num_cex; i++) {
      assumption(ctx,
        net[i][num_levels-1]->is_sorted(ctx)
      );
    }
  }

  void connect(unsigned ncx, unsigned aidx, unsigned bidx) {
    assert(aidx == bidx-1);
    std::vector< typename Context::result_type > ops(num_lines);
    Level & a = *net[ncx][aidx];
    Level & b = *net[ncx][bidx];
    for (unsigned i = 0; i < num_lines; i++) {
      ops[i] = evaluate(ctx, equal(a[i], b[i]));
    }

    BOOST_FOREACH (Tuple const & pos, all_gates) {
      predicate & swap = gate_vars[aidx][pos];
      unsigned p1 = pos.get<0>();
      unsigned p2 = pos.get<1>();
      comment(ctx, "exchange gate");
      assertion(ctx, implies(swap, And(
        equal(b[p1], And(a[p1],a[p2])),
        equal(b[p2],  Or(a[p1],a[p2]))
      )));
      ops[p1] = evaluate(ctx, Or(ops[p1], swap));
      ops[p2] = evaluate(ctx, Or(ops[p2], swap));
    }
    for (unsigned i = 0; i < num_lines; i++) {
      comment(ctx, "equal or exchange");
      metaSMT::assertion(ctx, ops[i]);
    }
  }

  void add_level_vars() {
    ExchangeIndex ei;
    std::vector<predicate> vars;
    BOOST_FOREACH (Tuple & tup, all_gates) {
      predicate p = new_variable();
      ei.insert(std::make_pair(tup, p));
      vars.push_back(p);
    }
    gate_vars.push_back(ei);
    comment(ctx, "swap cardinality");
    metaSMT::assertion(ctx, cardinality_leq(ctx, vars, num_lines/2u));
  }

  bool synth() {
    std::cout
      << "starting synthesis for depth " << num_levels-1
      << ", " << num_cex << " counterexamples" << std::endl;
    boost::timer timer;
    assumeSorted();
    bool b = solve(ctx);
    std::cout << "synthesis took " << timer.elapsed() << " seconds" << std::endl;
    return b;
  }

  ExchangeGates gates(unsigned level) {
    ExchangeIndex & ei = gate_vars[level];
    ExchangeGates ret;
    for( ExchangeIndex::iterator i = ei.begin();
         i != ei.end(); ++i)
    {
      bool b = read_value(ctx, i->second);
      if( b ) {
        ret.push_back( i->first );
      }
    }
    return ret;
  }

  template <typename VerifyerContext>
  bool verify_or_add_counterexample(VerifyerContext& ver) {
    boost::timer timer;
    assert(num_cex != 0);

    // build the sorting network
    for (unsigned lev = 1; lev < num_levels; lev++) {
      std::vector<bool> unused(num_lines, true);
      Level& a = *net[0][lev-1];
      Level& b = *net[0][lev];
      BOOST_FOREACH (Tuple const & exg, gates(lev-1)) {
        //std::cout << "creating gate " << lev <<" " << exg << '\n';
        unsigned p1 = exg.get<0>();
        unsigned p2 = exg.get<1>();
        //std::cout << "-- " << p1 <<" " << p2 << '\n';
        assertion(ver, And(
          equal(b[p1], And(a[p1], a[p2])),
          equal(b[p2],  Or(a[p1], a[p2]))
        ));
        unused[p1]=false;
        unused[p2]=false;
      }
      for (unsigned i = 0; i < num_lines; i++) {
        if(unused[i]) {
          //std::cout << "creating equal " << lev << " " << i << '\n';
          assertion(ver, equal(a[i],b[i]));
        }
      }
    }
    assertion(ver, Not(net[0][num_levels-1]->is_sorted(ver)));
    assertion(ver, Not(net[0][0]->is_sorted(ver)));

    bool cex_exists = solve(ver);
    std::cout << "verification took " << timer.elapsed() << " seconds" << std::endl;
    if(cex_exists) {
      Level& lev = *net[0][0];
      std::string cex = lev.assignment(ver);
      add_counterexample(cex);
      std::cout
        << "found counterexample " << num_cex << '\n'
        << cex
        << '\n'
        ;
      for (unsigned i = 1; i < num_levels; i++) {
        std::cout << net[0][num_levels-1]->assignment(ver) << '\n';
      }
    }

    return !cex_exists;
  }

  void print_all(std::ostream& out ) {
    for (unsigned i = 0; i < num_cex; i++) {
      print_one(out,i);
      out << "----\n";
    }
  }

  void print_one(std::ostream& out, unsigned cex=std::numeric_limits<unsigned>::max()) {
    if(cex >= num_cex) cex = num_cex-1;

    for (unsigned i = 0; i < num_levels; i++) {
      net[cex][i]->print_assignment(out, ctx);

      if (i == num_levels -1) {
        break; // do not print the last gates, they are not used.
      }

      ExchangeIndex & ei = gate_vars[i];

      for( ExchangeIndex::iterator i = ei.begin();
          i != ei.end(); ++i)
      {
        bool b = read_value(ctx, i->second);
        if( b ) {
          out << i->first << " ";
        }
      }
      out << '\n';
    }
  }

};


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

  @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_HMSB_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_HMSB_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>
#include <climits>
#include <cstddef>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(hmsb_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE std::size_t operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        using stype = bd::scalar_of_t<A0>;
        using itype = bd::as_integer_t<stype, unsigned>;
        std::size_t z = 0;
        const std::size_t N = A0::static_size;
        for(std::size_t i = 0; i != N; ++i)
        {
          stype t = a0[i]; // TO DO why must I use t or else bitwise_cast does not compile (clang 3.6)?
          z |= size_t((bitwise_cast<itype>(t) >> (sizeof(stype)*CHAR_BIT - 1)) << i);
        }
         return z;
      }
   };

   BOOST_DISPATCH_OVERLOAD(hmsb_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bs::logical_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE std::size_t operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return bs::hmsb(bs::genmask(a0));
      }
   };

} } }

#endif


/* compile with g++ mp_test.cpp -o mp_test -l arprec */
// g++ mp_lu_test.cpp -o mp_lu_test -I/home/pgottsch/Software/arprec-2.2.7/include -I$BOOST_ROOT -I$MTL -L/home/pgottsch/Software/arprec-2.2.7/src -l arprec -DMTL_HAS_ARPREC

#include <iostream>
#include <arprec/mp_real.h>
#include <complex>
#include <boost/numeric/mtl/mtl.hpp>

using namespace std;

int main() {

	mp::mp_init(102);

	mp_real a,b,c;

	a = "1.0e30";
	b = a+5;
	c = b-mp_real("1e30");
	c/= b;

	cout.precision(50);
	cout << "a = " << a << "\n";
	cout << "b = " << b << "\n";
	cout << "c = " << c << "\n";
	cout << "---------------\n";

	double d,e,f;
	d = 1e30;
	e = d+5;
	f = e-1e30;
	cout << "d = " << d << "\n";
	cout << "e = " << e << "\n";
	cout << "f = " << f << "\n";
	cout << "---------------\n";

	complex<double> z1;
	z1 = complex<double>(4,3);
	cout << "z1 = " << z1 << "\n";
	cout << "---------------\n";

	mtl::dense2D<mp_real> A(3,3);
	A = mp_real("2.43"), mp_real("2.33"), mp_real("7.18"),
	    mp_real("5.83"), mp_real("4.91"), mp_real("3.49"),
	    mp_real("8.12"), mp_real("0.45"), mp_real("6.33");
	cout << "A = \n" << A << "\n";
	mtl::dense_vector<int> p(3);

#if 0
	mtl::ashape::ashape<int>::type x= "";
	mtl::ashape::ashape<mp_real>::type y= "";
	mtl::traits::is_scalar<int>::type x= "";
	mtl::traits::is_scalar<mp_real>::type y= "";
#endif
	lu(A,p);
	cout << "A = \n" << A << "\n";
}


//---------------------------------------------------------------------------//
//!
//! \file   DataGen_FreeGasElasticSAlphaBetaFunction.hpp
//! \author Alex Robinson
//! \brief  Free gas elastic scattering S(alpha,beta) function definition
//!
//---------------------------------------------------------------------------//

// Std Lib Includes
#include <iostream>
#include <limits>

// Boost Includes
#include <boost/math/special_functions/bessel.hpp>
#include <boost/function.hpp>
#include <boost/bind.hpp>

// FRENSIE Includes
#include "DataGen_FreeGasElasticSAlphaBetaFunction.hpp"
#include "Utility_PhysicalConstants.hpp"
#include "Utility_KinematicHelpers.hpp"
#include "Utility_ComparePolicy.hpp"
#include "Utility_ContractException.hpp"

namespace DataGen{

// Initialize static member data
double FreeGasElasticSAlphaBetaFunction::neutron_kinetic_energy_multiplier=
  0.5*Utility::PhysicalConstants::neutron_rest_mass_energy/
  (Utility::PhysicalConstants::speed_of_light*
   Utility::PhysicalConstants::speed_of_light);

double FreeGasElasticSAlphaBetaFunction::min_exp_arg =
  log(std::numeric_limits<double>::min());

// Constructor
FreeGasElasticSAlphaBetaFunction::FreeGasElasticSAlphaBetaFunction(
	  const Teuchos::RCP<Utility::OneDDistribution>&
	  zero_temp_elastic_cross_section,
          const Teuchos::RCP<MonteCarlo::NuclearScatteringAngularDistribution>&
	  cm_scattering_distribution,
	  const double A,
	  const double kT )
  : d_gkq_set( 1e-6 ),
    d_zero_temp_elastic_cross_section( zero_temp_elastic_cross_section ),
    d_cm_scattering_distribution( cm_scattering_distribution ),
    d_A( A ),
    d_kT( kT ),
    d_average_zero_temp_elastic_cross_section( 1.0 )
{
  // Make sure the distributions are valid
  testPrecondition( !zero_temp_elastic_cross_section.is_null() );
  testPrecondition( !cm_scattering_distribution.is_null() );
  // Make sure the values are valid
  testPrecondition( A > 0.0 );
  testPrecondition( kT > 0.0 );

  // Compute the average zero temp elastic cross section
  boost::function<double (double)> integrand =
    boost::bind<double>( &Utility::OneDDistribution::evaluate,
			 boost::ref( *zero_temp_elastic_cross_section ),
			 _1 );

  double error;

  d_gkq_set.integrateAdaptively<15>(
		    integrand,
		    zero_temp_elastic_cross_section->getLowerBoundOfIndepVar(),
		    zero_temp_elastic_cross_section->getUpperBoundOfIndepVar(),
		    d_average_zero_temp_elastic_cross_section,
		    error );

  d_average_zero_temp_elastic_cross_section /=
    (zero_temp_elastic_cross_section->getUpperBoundOfIndepVar() -
     zero_temp_elastic_cross_section->getLowerBoundOfIndepVar());
}

// Return the atomic weight ratio
double FreeGasElasticSAlphaBetaFunction::getAtomicWeightRatio() const
{
  return d_A;
}

// Return the temperature
double FreeGasElasticSAlphaBetaFunction::getTemperature() const
{
  return d_kT;
}

// Evaluate the gkq_set factor integrand
double FreeGasElasticSAlphaBetaFunction::evaluateIntegrand(
						     const double alpha,
						     const double beta,
						     const double E,
						     const double mu_cm ) const
{
  // Make sure the energy is valid
  testPrecondition( E > 0.0 );
  // Make sure beta is valid
  testPrecondition( beta >= Utility::calculateBetaMin( E, d_kT ) );
  // Make sure alpha is valid
  remember( double alpha_min = Utility::calculateAlphaMin(E,beta,d_A,d_kT) );
  testPrecondition( alpha >= alpha_min );
  remember( double alpha_max = Utility::calculateAlphaMax(E,beta,d_A,d_kT) );
  testPrecondition( alpha <= alpha_max );
  // Make sure the cm angle is valid
  testPrecondition( mu_cm >= -1.0 );
  testPrecondition( mu_cm <= 1.0 );

  if( alpha > 0.0 )
  {
    if( mu_cm != 1.0 )
    {
      // Compute the relative velocity
      double relative_velocity = (d_A+1)/d_A*
	Utility::PhysicalConstants::speed_of_light*
	sqrt(d_A*d_kT*alpha/
	     (Utility::PhysicalConstants::neutron_rest_mass_energy*(1-mu_cm)));

      double relative_energy = neutron_kinetic_energy_multiplier*
	relative_velocity*relative_velocity;

      // Calculate the first term of the integrand
      double term_1 = sqrt(alpha)*
	d_zero_temp_elastic_cross_section->evaluate( relative_energy )*
	d_cm_scattering_distribution->evaluatePDF( E, mu_cm )/
	((1.0-mu_cm)*(1.0-mu_cm));

      // Compute the exponential argument
      double exp_arg = calculateExpArgConst( alpha, beta, E ) +
	calculateExpArgMult( alpha )/(1-mu_cm);

      // Compute the modified bessel function argument
      double bessel_arg_mult = calculateBesselArgMult( alpha, beta, E );

      double bessel_arg = calculateBesselArgMult( alpha, beta, E )*
	sqrt((1.0+mu_cm)/(1.0-mu_cm));

      // Calculate the second term of the integrand
      double term_2;

      try{
	if( exp_arg > min_exp_arg )
	{
	  term_2 = boost::math::cyl_bessel_i( 0, bessel_arg, Policy() )*
	    exp( exp_arg );
	}
	else // Use extended precision
	{
	  BoostLongDouble exp_arg_long = exp_arg;
	  BoostLongDouble term_2_long =
	    boost::math::cyl_bessel_i( 0, bessel_arg, Policy() )*
	    exp( exp_arg_long );

	  term_2 = term_2_long.convert_to<double>();
	}
      }
      //If the bessel argument is large use the asymptotic form of the function
      catch( std::exception& exception )
      {
	term_2 = exp( bessel_arg + exp_arg )/
	  sqrt( 2*Utility::PhysicalConstants::pi*bessel_arg );
      }

      // Make sure the return value is valid
      testPostcondition(!Teuchos::ScalarTraits<double>::isnaninf(term_1*term_2));

      return term_1*term_2;
    }
    // The integrand goes to zero at mu_cm = 1.0 (when alpha != 0.0)
    else
      return 0.0;
  }
  // There is a pole at (alpha=0.0,mu_cm=1.0)
  else // alpha == 0.0
  {
    if( mu_cm == 1.0 )
      return std::numeric_limits<double>::max();
    else
      return 0.0;
  }
}

// Evaluate the function at a desired alpha, beta and E
double FreeGasElasticSAlphaBetaFunction::operator()( const double alpha,
						     const double beta,
						     const double E ) const
{
  // Make sure the values are valid
  testPrecondition( E > 0.0 );
  testPrecondition( beta >= Utility::calculateBetaMin( E, d_kT ) );

  double alpha_min = Utility::calculateAlphaMin(E,beta,d_A,d_kT);
  double alpha_max = Utility::calculateAlphaMax(E,beta,d_A,d_kT);

  double value;

  // Test for special condition (alpha = 0.0)
  // Note: alpha = 0.0 can only occur when beta = 0.0. The S(alpha,beta)
  // function has an integrable singularity at alpha = 0.0. As alpha
  // aproaches 0.0, the integration gkq_set will likely fail - use the
  // approximate form for S(alpha,beta) when this occurs
  if( alpha >= alpha_min && alpha <= alpha_max )
  {
    if( alpha > 0.0 )
    {
      boost::function<double (double mu_cm)> integrand =
	boost::bind<double>(
			  &FreeGasElasticSAlphaBetaFunction::evaluateIntegrand,
			  boost::ref( *this ),
			  alpha,
			  beta,
			  E,
			  _1 );

      double value_error, lower_limit, upper_limit;

      this->findLimits( alpha, beta, E, lower_limit, upper_limit );

      try{
	d_gkq_set.integrateAdaptively<15>( integrand,
					  lower_limit,
					  upper_limit,
					  value,
					  value_error );
      }
      catch( Utility::IntegratorException& integration_exception )
      {
	std::cerr << "Warning: difficulty computing S("
		  << alpha << "," << beta << "," << E
		  << ") using approximate form." << std::endl;

	// Approximate S(alpha,beta) function
	value = d_average_zero_temp_elastic_cross_section/
	  ((d_A+1)*(d_A+1)*sqrt(alpha))*
	  exp( -(alpha + beta)*(alpha + beta)/(4*alpha) );

	if( value > std::numeric_limits<double>::max() )
	  value = std::numeric_limits<double>::max();
      }
    }
    else // alpha == 0.0
      value = std::numeric_limits<double>::infinity();
  }
  else
    value = 0.0;

  // Make sure the value is valid
  testPostcondition( value == value );

  return value;
}

// Calculate the exponential argument constant
double FreeGasElasticSAlphaBetaFunction::calculateExpArgConst(
							 const double alpha,
							 const double beta,
							 const double E ) const
{
  return -d_A*E/d_kT + -(d_A+1.0)/2.0*(beta - d_A*alpha);
}

// Calculate the exponential argument multiplier
double FreeGasElasticSAlphaBetaFunction::calculateExpArgMult(
						     const double alpha ) const
{
  return -(d_A+1)*(d_A+1)*alpha/2.0;
}

// Calculate the bessel argument multiplier
double FreeGasElasticSAlphaBetaFunction::calculateBesselArgMult(
							 const double alpha,
							 const double beta,
							 const double E ) const
{
  double bessel_arg_mult_arg =
    4.0*d_A*alpha*E/d_kT - (beta - d_A*alpha)*(beta - d_A*alpha);

  // When alpha ~ alpha_min, alpha ~ alpha_max or beta ~ beta_min,
  // a very small negative argument is possible due to roundoff-set it to 0
  if( bessel_arg_mult_arg < 0.0 )
    bessel_arg_mult_arg = 0.0;

  return (d_A+1)/2.0*sqrt(bessel_arg_mult_arg);
}

// Find limits to integrate over
void FreeGasElasticSAlphaBetaFunction::findLimits(
						    const double alpha,
						    const double beta,
						    const double E,
						    double& lower_limit,
						    double& upper_limit ) const
{
  // find an independent value where the function is non-zero
  std::list<double> search_grid;

  double arg1 = 4*d_A*alpha*E/d_kT -
    (beta - d_A*alpha)*(beta - d_A*alpha);
  double arg2 = (d_A + 1)*(d_A + 1)*alpha*alpha;

  double estimated_peak_mu_cm = (arg1 - arg2)/(arg1 + arg2);

  double estimated_peak_exp_arg = calculateExpArgConst( alpha, beta, E ) +
    calculateExpArgMult( alpha )/(1.0 - estimated_peak_mu_cm) +
    calculateBesselArgMult( alpha, beta, E )*
    sqrt((1.0+estimated_peak_mu_cm)/(1.0-estimated_peak_mu_cm));

  // Check if the integrand can be expected to return non-zero values
  if( estimated_peak_exp_arg > min_exp_arg )
  {
    search_grid.push_back( std::max( estimated_peak_mu_cm - 1e-6,
				     -1.0 ) );
    search_grid.push_back( std::min( estimated_peak_mu_cm + 1e-6,
				     1.0 ) );

    double center_value =
      this->findCMScatteringAngleCosineWithNonZeroIntegrandValue(alpha,
								 beta,
								 E,
								 search_grid );

    // binary search to find closer lower and upper limits
    double tol = 1e-15;

    double lower_bound = -1.0;
    double upper_bound = center_value;
    double new_bound;

    while( Utility::Policy::relError(upper_bound, lower_bound ) > tol  )
    {
      new_bound = (upper_bound + lower_bound)/2;

      if( this->evaluateIntegrand( alpha, beta, E, new_bound ) == 0.0 )
	lower_bound = new_bound;
      else
	upper_bound = new_bound;
    }

    // set the lower integration limit
    lower_limit = lower_bound;

    lower_bound = center_value;
    upper_bound = 1.0;

    while( Utility::Policy::relError(upper_bound, lower_bound) > tol )
    {
      new_bound = (upper_bound + lower_bound)/2;

      if( this->evaluateIntegrand( alpha, beta, E, new_bound ) == 0.0 )
	upper_bound = new_bound;
      else
	lower_bound = new_bound;
    }

    // set the upper integration limit
    upper_limit = upper_bound;
  }
  else // integrand will always return zero
  {
    upper_limit = 0.0;
    lower_limit = 0.0;
  }
}

// Find a CM scattering angle cosine where the function is non-zero
double FreeGasElasticSAlphaBetaFunction::findCMScatteringAngleCosineWithNonZeroIntegrandValue(
					 const double alpha,
					 const double beta,
					 const double E,
					 std::list<double>& grid_points ) const
{
  std::list<double>::iterator first_grid_point = grid_points.begin();
  std::list<double>::iterator second_grid_point = first_grid_point;
  ++second_grid_point;

  double cm_center_value;

  // Find if any center points have non-zero function values
  while( second_grid_point != grid_points.end() )
  {
    cm_center_value = (*first_grid_point + *second_grid_point)/2;

    if( this->evaluateIntegrand( alpha, beta, E, cm_center_value ) > 0.0 )
      return cm_center_value;

    first_grid_point = grid_points.insert( second_grid_point,
					   cm_center_value );
    ++first_grid_point;
    ++second_grid_point;
  }

  return findCMScatteringAngleCosineWithNonZeroIntegrandValue( alpha,
							       beta,
							       E,
							       grid_points );
}

} // end DataGen namespace

//---------------------------------------------------------------------------//
// end DataGen_FreeGasElasticSAlphaBetaFunction.cpp
//---------------------------------------------------------------------------//


//
// panoramic image align tool
// ulrich.krispel@fraunhofer.at
//

#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include <algorithm>
#include <map>
#include <iomanip>
#include <chrono>

#include <boost/program_options.hpp>

// images/jpg
#include "../src/image.h"
#include "../src/meanshift.h"

// use eigen matrices
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace po = boost::program_options;

//#define PI 3.14159265358979323846
#define PI2 (PI / 2.0)

using namespace OrthoGen;

int main(int ac, char *av[]) {
  Image imsrc, imdst;
  double selmin = -PI2, selmax = PI2, delmin = -PI2, delmax = PI2;
  int optimum = 0;
  int y_offset = 0;
  std::string srcname = "src.jpg";
  std::string dstname = "dst.jpg";
  std::string outname = "dst_aligned.jpg";

  po::options_description desc("commandline options");

  // read input panoramas and opening angle, convert to grayscale
  desc.add_options()("help", "show this help message")
      ("imsrc,S", po::value<std::string>(), "source panoramic image [.JPG]")
      ("imdst,D", po::value<std::string>(), "destination panoramic image [.JPG]")
      ("writesad", po::value<int>(),"export column SAD as csv")
      ("readsad", po::value<int>(),"import column SAD as csv, do not calculate SAD optimum")
      ("shift", po::value<int>(), "just perform shift")
      ("yoff", po::value<int>(),"Y-Offset [0]")
      ("writenormalized", po::value<int>(), "also write normalized images")
      ("selrange", po::value<std::vector<double>>()->multitoken(),"source elevation angle bounds [min..max], default [-PI/2..PI/2]")
      ("outname", po::value<std::string>(), "output file name [.JPG]")
      ;

  po::variables_map vm;
  po::store(po::parse_command_line(ac, av, desc,
                                   po::command_line_style::unix_style ^
                                   po::command_line_style::allow_short),
            vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("imsrc") || !vm.count("imdst")) {
    std::cout << desc << "\n";
    return 0;
  }
  if (vm.count("selrange")) {
    std::vector<double> el = vm["selrange"].as<std::vector<double>>();
    selmin = el[0];
    selmax = el[1];
  }
  if (vm.count("delrange")) {
    std::vector<double> el = vm["delrange"].as<std::vector<double>>();
    delmin = el[0];
    delmax = el[1];
  }
  if (vm.count("imsrc")) {
    srcname = vm["imsrc"].as<std::string>();
  }
  if (vm.count("imdst")) {
    dstname = vm["imdst"].as<std::string>();
  }
  if (vm.count("outname")) {
      outname = vm["outname"].as<std::string>();
  }
  if (vm.count("optimum")) {
    optimum = vm["optimum"].as<int>();
  }
  if (vm.count("yoff")) {
    y_offset = vm["yoff"].as<int>();
  }
  std::cout << "y_offset: " << y_offset << std::endl;
  // load images
  std::cout << "loading " << dstname << std::endl;
  loadJPEG(dstname.c_str(), imdst);
  if (!imdst.isValid()) {
    return -1;
  }

  if (vm.count("shift")) {
    const int shift = vm["shift"].as<int>();
    Image FinalOutput;
    FinalOutput.initialize(imdst.width(), imdst.height(), imdst.bpp());
    auto ShiftCB = [&imdst, &FinalOutput, shift, y_offset](int x, int y) {
      int x_ = (x + shift) % imdst.width();
      int y_ = (y + y_offset);
      if (y_ < 0) {
        y_ += imdst.height();
      } else {
        y_ %= imdst.height();
      }
      for (int ch = 0, che = imdst.channels(); ch < che; ++ch)
        FinalOutput(x, y, ch) = imdst(x_, y_, ch);
    };
    FinalOutput.applyPixelPosCB(ShiftCB, FinalOutput.whole());
    saveJPEG(outname.c_str(), FinalOutput);
    return 0;
  }

  std::cout << "loading " << srcname << std::endl;
  loadJPEG(srcname.c_str(), imsrc);
  if (!imsrc.isValid()) {
    return -1;
  }

  std::vector<double> SAD;
  if (!vm.count("readsad")) {
    // resample and normalize
    int R1 = imsrc.height(), C1 = imsrc.width();
    int R2 = imdst.height(), C2 = imdst.width();

    // calculate height of R1
    int R1target = (R1 * C2) / C1;
    int padlow = (int)round((R2 / 2.0) * (PI2 + selmin) / PI2);
    int padhigh = (int)round((R2 / 2.0) * (PI2 - selmax) / PI2);

    std::cout << "source elevation: " << selmin << " <-> " << selmax
              << std::endl;
    std::cout << "alignment size: " << C2 << "x" << R2 << " pixels, "
              << " padding: " << padlow << " low " << padhigh << " high"
              << std::endl;
    Image imsrcResized;
    imsrcResized.initialize(C2, R2, imsrc.bpp());
    std::cout << "resizing source image.." << std::endl;
    resizeBlit(imsrc, imsrc.whole(), imsrcResized,
               Image::AABB2D(0, padhigh+y_offset, C2, R1target+y_offset));
    //saveJPEG("srcresized.jpg", imsrcResized);

    auto NormalizeImage = [](const Image &img) -> ImageD {
      ImageD result = convertToGrayScale<Image, ImageD>(img);
      const double mean = result.mean<double>();
      const double std = result.std<double>(mean);
      {
        auto normalizeCB = [&result, &mean, &std](double pval) {
          return (pval - mean) / std;
        };
        result.transformPixelCB(normalizeCB, result.whole());
      }
      return result;
    };
    std::cout << "normalizing source image.." << std::endl;
    const ImageD src_n = NormalizeImage(imsrcResized);
    std::cout << "normalizing destination image.." << std::endl;
    const ImageD dst_n = NormalizeImage(imdst);

    if (vm.count("writenormalized")) {
      auto writeNormalized = [](const ImageD &img, const std::string &fn) {
        Image out;
        out.resize(img.width(), img.height(), 3);
        const double dmin = img.min(), dmax = img.max();
        std::cout << "min:" << dmin << " max: " << dmax << std::endl;
        auto CB = [&img, &out, dmin, dmax](int x, int y) {
          out(x, y, 0) = (unsigned char)((img(x, y, 0) - dmin) * 255.0 / (dmax - dmin));
          out(x, y, 1) = (unsigned char)((img(x, y, 0) - dmin) * 255.0 / (dmax - dmin));
          out(x, y, 2) = (unsigned char)((img(x, y, 0) - dmin) * 255.0 / (dmax - dmin));
        };
        out.applyPixelPosCB(CB, out.whole());
        saveJPEG(fn.c_str(), out);
      };
      writeNormalized(src_n, "src_normalized.jpg");
      writeNormalized(dst_n, "dst_normalized.jpg");
    }
    // perform SAD for horizontally shifted image, 1 pixel steps

    assert(src_n.width() == dst_n.width());
    SAD.resize(src_n.width(), 0.0);

    std::clock_t startcputime = std::clock();

    std::cout << "calculate SAD for horizontal shifts.." << std::endl;

    {
        const int progress_value = dst_n.width() / 10;
        int pgc = 0;
#pragma omp parallel for
        for (int shift = 0; shift < dst_n.width(); ++shift) {
            auto SAD_CB = [&SAD, &src_n, &dst_n, shift, &imsrcResized, &imdst](int x, int y) {
                const int dx = (x + shift) % dst_n.width();
                // calculate only with valid pixels
                if (!imsrcResized.isValue(x, y, 0, 0, 0) && !imdst.isValue(dx, y, 0, 0, 0))
                {
                    SAD[shift] += std::abs(src_n(x, y) - dst_n(dx, y));
                }
            };
            imsrc.applyPixelPosCBS(SAD_CB, imsrc.whole());
            if ((++pgc % progress_value) == 0) {
                std::cout << "*";
            }
        }
    }
    double cpu_duration = (std::clock() - startcputime) / (double)CLOCKS_PER_SEC;
    std::cout << "building SAD map took " << cpu_duration << "s." << std::endl;
  } else {
    std::ifstream in("sad.csv");

    while (!in.eof()) {
      std::string line;
      std::getline(in, line);
      if (line.length() > 0) {
        std::istringstream ss(line);
        int id;
        char delim;
        double value;
        ss >> id >> delim >> value;
        assert(SAD.size() == id);
        SAD.push_back(value);
      }
    }
  }

  /*
  // create output image using best guess
  const std::vector<double>::iterator it = std::min_element(SAD.begin(),
  SAD.end());
  const int shift = std::distance(SAD.begin(), it);
  */

  if (vm.count("writesad")) {
    std::ofstream of("sad.csv");
    for (size_t i = 0, ie = SAD.size(); i < ie; ++i) {
      of << i << "; " << std::fixed << std::setprecision(12) << SAD[i]
         << std::endl;
    }
  }

  //// find local minima
  // const int windowsize = SAD.size() / 10;
  // for (int i = 0, ie = SAD.size(); i < ie; ++i)
  //{

  //}
  /*{
      std::clock_t startcputime = std::clock();
      std::cout << "find meanshift SAD modes.." << std::endl;
      MEANSHIFT::Meanshift<Vec1d, 1> ms_sad;
      double sadmax = DBL_MIN, sadmin = DBL_MAX;
      for (auto const &s : SAD) {
          Vec1d d;
          d[0] = s;
          if (s > sadmax)
              sadmax = s;
          if (s < sadmin)
              sadmin = s;
          ms_sad.points.push_back(d);
      }
      ms_sad.calculate((sadmax - sadmin) / 10.0);
      int i = 0;
      for (auto cluster = ms_sad.cluster.begin(); i < 5; ++cluster, ++i) {
          std::cout << " optimum " << i << " : " << cluster->first << std::endl;
          double localmin = DBL_MAX;
          size_t localid = -1;
          for (auto const &p : cluster->second) {
              if (SAD[p] < localmin) {
                  localmin = SAD[p];
                  localid = p;
              }
          }
          std::cout << " minimum: " << localmin << " [" << localid << "]"
              << std::endl;
          std::cout << std::endl;
      }
      double cpu_duration = (std::clock() - startcputime) / (double)CLOCKS_PER_SEC;
      std::cout << "building SAD map took " << cpu_duration << "s." << std::endl;
  }
*/
  std::cout << " find minimum SAD: " << std::endl;
  // put into map (sort)
  std::map<double, int> sapcost;
  for (int i = 0, ie = (int) SAD.size(); i < ie; ++i) {
    sapcost[SAD[i]] = i;
  }

  auto it = sapcost.begin();
  for (int i = 0; i < optimum; ++i) {
    ++it;
  }
  const int shift = it->second;
  std::cout << "found optimum " << optimum << " at shift " << shift
            << std::endl;
  std::cout << "creating and writing output image..." << shift << std::endl;

  Image FinalOutput;
  FinalOutput.initialize(imdst.width(), imdst.height(), imdst.bpp());
  auto ShiftCB = [&imdst, &FinalOutput, shift](int x, int y) {
    int x_ = (x + shift) % imdst.width();
    for (int ch = 0, che = imdst.channels(); ch < che; ++ch)
      FinalOutput(x, y, ch) = imdst(x_, y, ch);
  };
  FinalOutput.applyPixelPosCB(ShiftCB, FinalOutput.whole());
  saveJPEG(outname.c_str(), FinalOutput);

  it = sapcost.begin();
  for (int i = 0; i < 5; ++i) {
    std::cout << "optimum " << i << ": at " << it->second << std::endl;
    ++it;
  }
}


//------------------------------------------//
//  This file is a modified version of      //
//  basics.cpp, which was distributed as    //
//  part of MuJoCo,  Written by Emo Todorov //
//  Copyright (C) 2017 Roboti LLC           //
//  Modifications by Atabak Dehban          //
//------------------------------------------//

#include <iostream>

#include "mujoco.h"
#include "cstdio"
#include "cstdlib"
#include "cstring"
#include "glfw3.h"

// Eigen, used by drake
#include <Eigen/Core>
#include <unsupported/Eigen/MatrixFunctions>

// for sleep timers
#include <chrono>
#include <thread>

// local variables include
#include "LQR-Acrobot.h"

// MuJoCo data structures
mjModel* m = NULL;                  // MuJoCo model
mjData* d = NULL;                   // MuJoCo data
mjvCamera cam;                      // abstract camera
mjvOption opt;                      // visualization options
mjvScene scn;                       // abstract scene
mjrContext con;                     // custom GPU context

// mouse interaction
bool button_left = false;
bool button_middle = false;
bool button_right =  false;
double lastx = 0;
double lasty = 0;

// keyboard callback
void keyboard(GLFWwindow* window, int key, int scancode, int act, int mods)
{
    // backspace: reset simulation
    if( act==GLFW_PRESS && key==GLFW_KEY_BACKSPACE )
    {
        mj_resetData(m, d);
        mj_forward(m, d);
    }
}

// mouse button callback
void mouse_button(GLFWwindow* window, int button, int act, int mods)
{
    // update button state
    button_left =   (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT)==GLFW_PRESS);
    button_middle = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE)==GLFW_PRESS);
    button_right =  (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT)==GLFW_PRESS);

    // update mouse position
    glfwGetCursorPos(window, &lastx, &lasty);
}

// mouse move callback
void mouse_move(GLFWwindow* window, double xpos, double ypos)
{
    // no buttons down: nothing to do
    if( !button_left && !button_middle && !button_right )
        return;

    // compute mouse displacement, save
    double dx = xpos - lastx;
    double dy = ypos - lasty;
    lastx = xpos;
    lasty = ypos;

    // get current window size
    int width, height;
    glfwGetWindowSize(window, &width, &height);

    // get shift key state
    bool mod_shift = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT)==GLFW_PRESS ||
                      glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT)==GLFW_PRESS);

    // determine action based on mouse button
    mjtMouse action;
    if( button_right )
        action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V;
    else if( button_left )
        action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V;
    else
        action = mjMOUSE_ZOOM;

    // move camera
    mjv_moveCamera(m, action, dx/height, dy/height, &scn, &cam);
}

// scroll callback
void scroll(GLFWwindow* window, double xoffset, double yoffset)
{
    // emulate vertical mouse motion = 5% of window height
    mjv_moveCamera(m, mjMOUSE_ZOOM, 0, -0.05*yoffset, &scn, &cam);
}

// control loop callback
void mycontroller(const mjModel* m, mjData* d)
{

    std::cout << "num deg free: " << m->nv<< std::endl;
    std::cout << "num gen coor: " << m->nq << std::endl;
    std::cout << "num act: " << m->nu << std::endl;

    std::cout << "q0: " << d->qpos[0] << std::endl;
    std::cout << "q1: " << d->qpos[1] << std::endl;
    std::cout << "v0: " << d->qvel[0] << std::endl;
    std::cout << "v1: " << d->qvel[1] << std::endl;

    mjtNum state[2*m->nq];
    mju_copy(state, d->qpos, 2*m->nq);
    state[0] -= M_PI_2; // stand-up position
    mjtNum ctrl = mju_dot(lqr_result.K.data(), state, 2*m->nq);

    std::cout << lqr_result.K << std::endl;

    d->ctrl[0] = -ctrl;

    std::cout << "actuator force: " << d->actuator_force[0] << std::endl;
}

drake::systems::controllers::LinearQuadraticRegulatorResult getLQRControl()
{
    Eigen::Matrix<mjtNum , 4, 4> A_ = Eigen::Matrix<mjtNum , 4, 4>::Zero();
    Eigen::Matrix<mjtNum , 4, 1> B_ = Eigen::Matrix<mjtNum , 4, 1>::Zero();
    Eigen::Matrix<mjtNum , 4, 4> Q_ = Eigen::Matrix<mjtNum , 4, 4>::Identity();
    Eigen::Matrix<mjtNum , 1, 1> R_ = Eigen::Matrix<mjtNum , 1, 1>::Identity();
    Eigen::Matrix<mjtNum , 4, 1> N = Eigen::Matrix<mjtNum , 4, 1>::Zero();

// A
    A_.topRightCorner(2,2) = Eigen::Matrix<mjtNum , 2, 2>::Identity();

// Based on Spong
    A_(2,0) = 12.49;
    A_(2,1) = -12.54;
    A_(3,0) = -14.49;
    A_(3,1) = 29.36;

// B
    B_(2,0) = -2.98;
    B_(3,0) = 5.98;

// Q
//    Q_.topLefCorner(2,2) *= 10.0;  // optional

    return drake::systems::controllers::LinearQuadraticRegulator(A_, B_, Q_, R_, N);
}

// main function
int main(int argc, const char** argv)
{

    // activate software
    mj_activate("../../../mjkey.txt");

    // load and compile model
    char error[1000] = "Could not load binary model";

    // check command-line arguments
    if( argc<2 )
        m = mj_loadXML("../../../models/Acrobot.xml", 0, error, 1000);

    else
    if( strlen(argv[1])>4 && !strcmp(argv[1]+strlen(argv[1])-4, ".mjb") )
        m = mj_loadModel(argv[1], 0);
    else
        m = mj_loadXML(argv[1], 0, error, 1000);
    if( !m )
        mju_error_s("Load model error: %s", error);

    // make data
    d = mj_makeData(m);

    // init GLFW
    if( !glfwInit() )
        mju_error("Could not initialize GLFW");

    // create window, make OpenGL context current, request v-sync
    GLFWwindow* window = glfwCreateWindow(1200, 900, "Demo", NULL, NULL);
    glfwMakeContextCurrent(window);
    glfwSwapInterval(1);

    // initialize visualization data structures
    mjv_defaultCamera(&cam);
    mjv_defaultOption(&opt);
    mjv_defaultScene(&scn);
    mjr_defaultContext(&con);
    mjv_makeScene(m, &scn, 2000);                // space for 2000 objects
    mjr_makeContext(m, &con, mjFONTSCALE_150);   // model-specific context

    // install GLFW mouse and keyboard callbacks
    glfwSetKeyCallback(window, keyboard);
    glfwSetCursorPosCallback(window, mouse_move);
    glfwSetMouseButtonCallback(window, mouse_button);
    glfwSetScrollCallback(window, scroll);

    lqr_result = getLQRControl();

    // install control callback
    mjcb_control = mycontroller;

    // initial position
//    d->qpos[0] = M_PI/10.0;
    d->qpos[0] = M_PI_2 + 0.05;
    d->qpos[1] = 0.0;
    d->qvel[0] = 0.0;
    d->qvel[1] = 0.0;

    // use the first while condition if you want to simulate for a period.
    while( !glfwWindowShouldClose(window))
    {
        //  advance interactive simulation for 1/60 sec
        //  Assuming MuJoCo can simulate faster than real-time, which it usually can,
        //  this loop will finish on time for the next frame to be rendered at 60 fps.
        //  Otherwise add a cpu timer and exit this loop when it is time to render.
        mjtNum simstart = d->time;
        while( d->time - simstart < 1.0/60.0 )
            mj_step(m, d);

        std::this_thread::sleep_for(std::chrono::milliseconds(15));

        // get framebuffer viewport
        mjrRect viewport = {0, 0, 0, 0};
        glfwGetFramebufferSize(window, &viewport.width, &viewport.height);

        // update scene and render
        mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);
        mjr_render(viewport, &scn, &con);

        // swap OpenGL buffers (blocking call due to v-sync)
        glfwSwapBuffers(window);

        // process pending GUI events, call GLFW callbacks
        glfwPollEvents();

    }

    // free visualization storage
    mjv_freeScene(&scn);
    mjr_freeContext(&con);

    // free MuJoCo model and data, deactivate
    mj_deleteData(d);
    mj_deleteModel(m);
    mj_deactivate();

    // terminate GLFW (crashes with Linux NVidia drivers)
    #if defined(__APPLE__) || defined(_WIN32)
        glfwTerminate();
    #endif
    return 1;
}


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

#include <nt2/hyperbolic/include/functions/sinhcosh.hpp>
#include <nt2/include/functions/sinh.hpp>
#include <nt2/include/functions/cosh.hpp>
#include <nt2/include/functions/tie.hpp>
#include <nt2/include/functions/splat.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <boost/fusion/include/vector_tie.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>

NT2_TEST_CASE_TPL(sinhcosh_table, NT2_SIMD_REAL_TYPES)
{
  using nt2::sinhcosh;
  using nt2::tag::sinhcosh_;
  using boost::simd::native;
  typedef native<T,BOOST_SIMD_DEFAULT_EXTENSION>            vT;
  vT a[] = {nt2::Zero<vT>(), nt2::One<vT>(), nt2::splat<vT>(-5), nt2::splat<vT>(5),
            nt2::splat<vT>(10), nt2::Inf<vT>(), nt2::Minf<vT>(), nt2::Nan<vT>()};
  size_t N =  sizeof(a)/sizeof(vT);
  NT2_TEST_TYPE_IS( (typename boost::dispatch::meta::call<sinhcosh_(vT)>::type)
                  , (std::pair<vT,vT>)
                  );

  {
    vT s, c;
    for(size_t i=0; i < N; ++i)
    {
      sinhcosh(a[i], s, c);
      NT2_TEST_ULP_EQUAL(s, nt2::sinh(a[i]), 1);
      NT2_TEST_ULP_EQUAL(c, nt2::cosh(a[i]), 1);
    }
  }

  {
    vT s, c;
    for(size_t i=0; i < N; ++i)
    {
      s = sinhcosh(a[i], c);
      NT2_TEST_ULP_EQUAL(s, nt2::sinh(a[i]), 1);
      NT2_TEST_ULP_EQUAL(c, nt2::cosh(a[i]), 1);
    }
  }

  {
    vT s, c;
    for(size_t i=0; i < N; ++i)
    {
      boost::fusion::vector_tie(s, c) = sinhcosh(a[i]);
      NT2_TEST_ULP_EQUAL(s, nt2::sinh(a[i]), 1);
      NT2_TEST_ULP_EQUAL(c, nt2::cosh(a[i]), 1);
    }
  }

  {
    for(size_t i=0; i < N; ++i)
    {
      std::pair<vT,vT> p = sinhcosh(a[i]);
      NT2_TEST_ULP_EQUAL(p.first,  nt2::sinh(a[i]), 1);
      NT2_TEST_ULP_EQUAL(p.second, nt2::cosh(a[i]), 1);
    }
  }

}