#include <boost/test/unit_test.hpp>

#define VMATH_CORE_SWIZZLE_ENABLE_ELEMENT_ACCESSORS
#include <vmath/core/vector.hpp>
#include <vmath/core/swizzle/swizzle3.hpp>

BOOST_AUTO_TEST_SUITE(Swizzle3)

BOOST_AUTO_TEST_CASE(negate_op) {
	vmath::core::Vector<float, 3> V;
	V.x = 20.12f;
	V.y = 100.89f;
	V.z = -18.2f;
	vmath::core::Vector<float, 3> V_neg;
	V_neg = -V.yzx;
	BOOST_CHECK_CLOSE(V_neg.x, -100.89f, 1e-4f);
	BOOST_CHECK_CLOSE(V_neg.y, 18.2f, 1e-4f);
	BOOST_CHECK_CLOSE(V_neg.z, -20.12f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(add_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = -18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = -15.5f;
	V2.z = 20.2f;
	vmath::core::Vector<float, 3> V_add;
	V_add = V1.yxy + V2.xxx;
	BOOST_CHECK_CLOSE(V_add.x, 111.23f, 1e-4f);
	BOOST_CHECK_CLOSE(V_add.y, 30.46f, 1e-4f);
	BOOST_CHECK_CLOSE(V_add.z, 111.23f, 1e-4f);
	V_add = V2.xxx + V1.yxy;
	BOOST_CHECK_CLOSE(V_add.x, 111.23f, 1e-4f);
	BOOST_CHECK_CLOSE(V_add.y, 30.46f, 1e-4f);
	BOOST_CHECK_CLOSE(V_add.x, 111.23f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(add_eq_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = -18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = -15.5f;
	V2.z = 20.2f;
	vmath::core::Vector<float, 3> V_add = V1;
	V_add.yxz += V2.xxx;
	BOOST_CHECK_CLOSE(V_add.x, 30.46f, 1e-4f);
	BOOST_CHECK_CLOSE(V_add.y, 111.23f, 1e-4f);
	BOOST_CHECK_CLOSE(V_add.z, -7.859999999f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(sub_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = -18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = -15.5f;
	V2.z = 20.12f;
	vmath::core::Vector<float, 3> V_sub;
	V_sub = V1.yxx - V2.xxz;
	BOOST_CHECK_CLOSE(V_sub.x, 90.55f, 1e-4f);
	BOOST_CHECK_CLOSE(V_sub.y, 9.78f, 1e-4f);
	BOOST_CHECK_SMALL(V_sub.z, 1e-7f);
	V_sub = V2.xxz - V1.yxx;
	BOOST_CHECK_CLOSE(V_sub.x, -90.55f, 1e-4f);
	BOOST_CHECK_CLOSE(V_sub.y, -9.78f, 1e-4f);
	BOOST_CHECK_SMALL(V_sub.z, 1e-7f);
}

BOOST_AUTO_TEST_CASE(sub_eq_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = -18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = -15.5f;
	V2.z = 20.2f;
	vmath::core::Vector<float, 3> V_sub = V1;
	V_sub.yxz -= V2.xxx;
	BOOST_CHECK_CLOSE(V_sub.x, 9.78f, 1e-4f);
	BOOST_CHECK_CLOSE(V_sub.y, 90.55f, 1e-4f);
	BOOST_CHECK_CLOSE(V_sub.z, -28.54f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(mult_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = -18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = -15.5f;
	V2.z = 20.2f;
	vmath::core::Vector<float, 3> V_mult;
	V_mult = V1.yxx * V2.xxx;
	BOOST_CHECK_CLOSE(V_mult.x, 1043.2026f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.y, 208.0408f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.z, 208.0408f, 1e-4f);
	V_mult = V2.xxx * V1.yxx;
	BOOST_CHECK_CLOSE(V_mult.x, 1043.2026f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.y, 208.0408f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.z, 208.0408f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(mult_eq_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = -18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = -15.5f;
	V2.z = 20.2f;
	vmath::core::Vector<float, 3> V_mult = V1;
	V_mult.yxz *= V2.xyx;
	BOOST_CHECK_CLOSE(V_mult.x, -311.86f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.y, 1043.2026f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.z, -188.188f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(div_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.0f;
	V1.y = 40.0f;
	V1.z = 60.0f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 2.0f;
	V2.y = 4.0f;
	V2.z = 6.0f;
	vmath::core::Vector<float, 3> V_div;
	V_div = V1.yxx / V2.xxx;
	BOOST_CHECK_CLOSE(V_div.x, 20.0f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.y, 10.0f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.z, 10.0f, 1e-4f);
	V_div = V1.xxx / V2.yxx;
	BOOST_CHECK_CLOSE(V_div.x, 5.0f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.y, 10.0f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.z, 10.0f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(div_eq_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.0f;
	V1.y = 40.0f;
	V1.z = 60.0f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 2.0f;
	V2.y = 4.0f;
	V2.z = 6.0f;
	vmath::core::Vector<float, 3> V_div = V1;
	V_div.yxz /= V2.yxx;
	BOOST_CHECK_CLOSE(V_div.x, 10.0f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.y, 10.0f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.z, 30.0f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(scalar_mult_op) {
	vmath::core::Vector<float, 3> V;
	V.x = 20.12f;
	V.y = 100.89f;
	V.z = -18.2f;
	float s = -34.45f;
	vmath::core::Vector<float, 3> V_mult;
	V_mult = V.xyz * s;
	BOOST_CHECK_CLOSE(V_mult.x, -693.134f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.y, -3475.6605f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.z, 626.99f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(scalar_mult_eq_op) {
	vmath::core::Vector<float, 3> V;
	V.x = 20.12f;
	V.y = 100.89f;
	V.z = -18.2f;
	float s = -34.45f;
	vmath::core::Vector<float, 3> V_mult = V;
	V_mult.zyx *= s;
	BOOST_CHECK_CLOSE(V_mult.x, -693.134f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.y, -3475.6605f, 1e-4f);
	BOOST_CHECK_CLOSE(V_mult.z, 626.99f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(scalar_div_op) {
	vmath::core::Vector<float, 3> V;
	V.x = 20.12f;
	V.y = 100.89f;
	V.z = -18.2f;
	float s = -34.45f;
	vmath::core::Vector<float, 3> V_div;
	V_div.xyz = V / s;
	BOOST_CHECK_CLOSE(V_div.x, -0.5840348330914369f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.y, -2.9285921625544264f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.z, 0.5283018867924527f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(scalar_div_eq_op) {
	vmath::core::Vector<float, 3> V;
	V.x = 20.12f;
	V.y = 100.89f;
	V.z = -18.2f;
	float s = -34.45f;
	vmath::core::Vector<float, 3> V_div = V;
	V_div.zxy /= s;
	BOOST_CHECK_CLOSE(V_div.x, -0.5840348330914369f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.y, -2.9285921625544264f, 1e-4f);
	BOOST_CHECK_CLOSE(V_div.z, 0.5283018867924527f, 1e-4f);
}

BOOST_AUTO_TEST_CASE(swizzles) {
	vmath::core::Vector<float, 3> V;
	V.x = 20.12f;
	V.y = 100.89f;
	V.z = -18.2f;
	// 2d swizzles <x, y, z>
	auto xx = V.xx;
	BOOST_CHECK_CLOSE(xx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xx.getE2(), V.x, 1e-4f);
	auto xy = V.xy;
	BOOST_CHECK_CLOSE(xy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xy.getE2(), V.y, 1e-4f);
	auto xz = V.xz;
	BOOST_CHECK_CLOSE(xz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xz.getE2(), V.z, 1e-4f);
	auto yx = V.yx;
	BOOST_CHECK_CLOSE(yx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yx.getE2(), V.x, 1e-4f);
	auto yy = V.yy;
	BOOST_CHECK_CLOSE(yy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yy.getE2(), V.y, 1e-4f);
	auto yz = V.yz;
	BOOST_CHECK_CLOSE(yz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yz.getE2(), V.z, 1e-4f);
	auto zx = V.zx;
	BOOST_CHECK_CLOSE(zx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zx.getE2(), V.x, 1e-4f);
	auto zy = V.zy;
	BOOST_CHECK_CLOSE(zy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zy.getE2(), V.y, 1e-4f);
	auto zz = V.zz;
	BOOST_CHECK_CLOSE(zz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zz.getE2(), V.z, 1e-4f);
	// 3d swizzles <x, y, z>
	auto xxx = V.xxx;
	BOOST_CHECK_CLOSE(xxx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxx.getE3(), V.x, 1e-4f);
	auto xxy = V.xxy;
	BOOST_CHECK_CLOSE(xxy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxy.getE3(), V.y, 1e-4f);
	auto xxz = V.xxz;
	BOOST_CHECK_CLOSE(xxz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxz.getE3(), V.z, 1e-4f);
	auto xyx = V.xyx;
	BOOST_CHECK_CLOSE(xyx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyx.getE3(), V.x, 1e-4f);
	auto xyy = V.xyy;
	BOOST_CHECK_CLOSE(xyy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyy.getE3(), V.y, 1e-4f);
	auto xyz = V.xyz;
	BOOST_CHECK_CLOSE(xyz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyz.getE3(), V.z, 1e-4f);
	auto xzx = V.xzx;
	BOOST_CHECK_CLOSE(xzx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzx.getE3(), V.x, 1e-4f);
	auto xzy = V.xzy;
	BOOST_CHECK_CLOSE(xzy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzy.getE3(), V.y, 1e-4f);
	auto xzz = V.xzz;
	BOOST_CHECK_CLOSE(xzz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzz.getE3(), V.z, 1e-4f);
	auto yxx = V.yxx;
	BOOST_CHECK_CLOSE(yxx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxx.getE3(), V.x, 1e-4f);
	auto yxy = V.yxy;
	BOOST_CHECK_CLOSE(yxy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxy.getE3(), V.y, 1e-4f);
	auto yxz = V.yxz;
	BOOST_CHECK_CLOSE(yxz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxz.getE3(), V.z, 1e-4f);
	auto yyx = V.yyx;
	BOOST_CHECK_CLOSE(yyx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyx.getE3(), V.x, 1e-4f);
	auto yyy = V.yyy;
	BOOST_CHECK_CLOSE(yyy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyy.getE3(), V.y, 1e-4f);
	auto yyz = V.yyz;
	BOOST_CHECK_CLOSE(yyz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyz.getE3(), V.z, 1e-4f);
	auto yzx = V.yzx;
	BOOST_CHECK_CLOSE(yzx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzx.getE3(), V.x, 1e-4f);
	auto yzy = V.yzy;
	BOOST_CHECK_CLOSE(yzy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzy.getE3(), V.y, 1e-4f);
	auto yzz = V.yzz;
	BOOST_CHECK_CLOSE(yzz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzz.getE3(), V.z, 1e-4f);
	auto zxx = V.zxx;
	BOOST_CHECK_CLOSE(zxx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxx.getE3(), V.x, 1e-4f);
	auto zxy = V.zxy;
	BOOST_CHECK_CLOSE(zxy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxy.getE3(), V.y, 1e-4f);
	auto zxz = V.zxz;
	BOOST_CHECK_CLOSE(zxz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxz.getE3(), V.z, 1e-4f);
	auto zyx = V.zyx;
	BOOST_CHECK_CLOSE(zyx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyx.getE3(), V.x, 1e-4f);
	auto zyy = V.zyy;
	BOOST_CHECK_CLOSE(zyy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyy.getE3(), V.y, 1e-4f);
	auto zyz = V.zyz;
	BOOST_CHECK_CLOSE(zyz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyz.getE3(), V.z, 1e-4f);
	auto zzx = V.zzx;
	BOOST_CHECK_CLOSE(zzx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzx.getE3(), V.x, 1e-4f);
	auto zzy = V.zzy;
	BOOST_CHECK_CLOSE(zzy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzy.getE3(), V.y, 1e-4f);
	auto zzz = V.zzz;
	BOOST_CHECK_CLOSE(zzz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzz.getE3(), V.z, 1e-4f);
	// 4d swizzles <x, y, z>
	auto xxxx = V.xxxx;
	BOOST_CHECK_CLOSE(xxxx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxx.getE4(), V.x, 1e-4f);
	auto xxxy = V.xxxy;
	BOOST_CHECK_CLOSE(xxxy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxy.getE4(), V.y, 1e-4f);
	auto xxxz = V.xxxz;
	BOOST_CHECK_CLOSE(xxxz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxxz.getE4(), V.z, 1e-4f);
	auto xxyx = V.xxyx;
	BOOST_CHECK_CLOSE(xxyx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxyx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xxyx.getE4(), V.x, 1e-4f);
	auto xxyy = V.xxyy;
	BOOST_CHECK_CLOSE(xxyy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxyy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xxyy.getE4(), V.y, 1e-4f);
	auto xxyz = V.xxyz;
	BOOST_CHECK_CLOSE(xxyz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxyz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xxyz.getE4(), V.z, 1e-4f);
	auto xxzx = V.xxzx;
	BOOST_CHECK_CLOSE(xxzx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxzx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xxzx.getE4(), V.x, 1e-4f);
	auto xxzy = V.xxzy;
	BOOST_CHECK_CLOSE(xxzy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxzy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xxzy.getE4(), V.y, 1e-4f);
	auto xxzz = V.xxzz;
	BOOST_CHECK_CLOSE(xxzz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxzz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xxzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xxzz.getE4(), V.z, 1e-4f);
	auto xyxx = V.xyxx;
	BOOST_CHECK_CLOSE(xyxx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyxx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyxx.getE4(), V.x, 1e-4f);
	auto xyxy = V.xyxy;
	BOOST_CHECK_CLOSE(xyxy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyxy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyxy.getE4(), V.y, 1e-4f);
	auto xyxz = V.xyxz;
	BOOST_CHECK_CLOSE(xyxz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyxz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyxz.getE4(), V.z, 1e-4f);
	auto xyyx = V.xyyx;
	BOOST_CHECK_CLOSE(xyyx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyyx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyyx.getE4(), V.x, 1e-4f);
	auto xyyy = V.xyyy;
	BOOST_CHECK_CLOSE(xyyy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyyy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyyy.getE4(), V.y, 1e-4f);
	auto xyyz = V.xyyz;
	BOOST_CHECK_CLOSE(xyyz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyyz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyyz.getE4(), V.z, 1e-4f);
	auto xyzx = V.xyzx;
	BOOST_CHECK_CLOSE(xyzx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyzx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xyzx.getE4(), V.x, 1e-4f);
	auto xyzy = V.xyzy;
	BOOST_CHECK_CLOSE(xyzy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyzy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xyzy.getE4(), V.y, 1e-4f);
	auto xyzz = V.xyzz;
	BOOST_CHECK_CLOSE(xyzz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xyzz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xyzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xyzz.getE4(), V.z, 1e-4f);
	auto xzxx = V.xzxx;
	BOOST_CHECK_CLOSE(xzxx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzxx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzxx.getE4(), V.x, 1e-4f);
	auto xzxy = V.xzxy;
	BOOST_CHECK_CLOSE(xzxy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzxy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzxy.getE4(), V.y, 1e-4f);
	auto xzxz = V.xzxz;
	BOOST_CHECK_CLOSE(xzxz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzxz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzxz.getE4(), V.z, 1e-4f);
	auto xzyx = V.xzyx;
	BOOST_CHECK_CLOSE(xzyx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzyx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xzyx.getE4(), V.x, 1e-4f);
	auto xzyy = V.xzyy;
	BOOST_CHECK_CLOSE(xzyy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzyy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xzyy.getE4(), V.y, 1e-4f);
	auto xzyz = V.xzyz;
	BOOST_CHECK_CLOSE(xzyz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzyz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(xzyz.getE4(), V.z, 1e-4f);
	auto xzzx = V.xzzx;
	BOOST_CHECK_CLOSE(xzzx.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzzx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzzx.getE4(), V.x, 1e-4f);
	auto xzzy = V.xzzy;
	BOOST_CHECK_CLOSE(xzzy.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzzy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzzy.getE4(), V.y, 1e-4f);
	auto xzzz = V.xzzz;
	BOOST_CHECK_CLOSE(xzzz.getE1(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(xzzz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(xzzz.getE4(), V.z, 1e-4f);
	auto yxxx = V.yxxx;
	BOOST_CHECK_CLOSE(yxxx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxxx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxxx.getE4(), V.x, 1e-4f);
	auto yxxy = V.yxxy;
	BOOST_CHECK_CLOSE(yxxy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxxy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxxy.getE4(), V.y, 1e-4f);
	auto yxxz = V.yxxz;
	BOOST_CHECK_CLOSE(yxxz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxxz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxxz.getE4(), V.z, 1e-4f);
	auto yxyx = V.yxyx;
	BOOST_CHECK_CLOSE(yxyx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxyx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxyx.getE4(), V.x, 1e-4f);
	auto yxyy = V.yxyy;
	BOOST_CHECK_CLOSE(yxyy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxyy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxyy.getE4(), V.y, 1e-4f);
	auto yxyz = V.yxyz;
	BOOST_CHECK_CLOSE(yxyz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxyz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxyz.getE4(), V.z, 1e-4f);
	auto yxzx = V.yxzx;
	BOOST_CHECK_CLOSE(yxzx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxzx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yxzx.getE4(), V.x, 1e-4f);
	auto yxzy = V.yxzy;
	BOOST_CHECK_CLOSE(yxzy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxzy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yxzy.getE4(), V.y, 1e-4f);
	auto yxzz = V.yxzz;
	BOOST_CHECK_CLOSE(yxzz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yxzz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yxzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yxzz.getE4(), V.z, 1e-4f);
	auto yyxx = V.yyxx;
	BOOST_CHECK_CLOSE(yyxx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyxx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yyxx.getE4(), V.x, 1e-4f);
	auto yyxy = V.yyxy;
	BOOST_CHECK_CLOSE(yyxy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyxy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yyxy.getE4(), V.y, 1e-4f);
	auto yyxz = V.yyxz;
	BOOST_CHECK_CLOSE(yyxz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyxz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yyxz.getE4(), V.z, 1e-4f);
	auto yyyx = V.yyyx;
	BOOST_CHECK_CLOSE(yyyx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyx.getE4(), V.x, 1e-4f);
	auto yyyy = V.yyyy;
	BOOST_CHECK_CLOSE(yyyy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyy.getE4(), V.y, 1e-4f);
	auto yyyz = V.yyyz;
	BOOST_CHECK_CLOSE(yyyz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyyz.getE4(), V.z, 1e-4f);
	auto yyzx = V.yyzx;
	BOOST_CHECK_CLOSE(yyzx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyzx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yyzx.getE4(), V.x, 1e-4f);
	auto yyzy = V.yyzy;
	BOOST_CHECK_CLOSE(yyzy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyzy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yyzy.getE4(), V.y, 1e-4f);
	auto yyzz = V.yyzz;
	BOOST_CHECK_CLOSE(yyzz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyzz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yyzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yyzz.getE4(), V.z, 1e-4f);
	auto yzxx = V.yzxx;
	BOOST_CHECK_CLOSE(yzxx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzxx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yzxx.getE4(), V.x, 1e-4f);
	auto yzxy = V.yzxy;
	BOOST_CHECK_CLOSE(yzxy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzxy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yzxy.getE4(), V.y, 1e-4f);
	auto yzxz = V.yzxz;
	BOOST_CHECK_CLOSE(yzxz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzxz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(yzxz.getE4(), V.z, 1e-4f);
	auto yzyx = V.yzyx;
	BOOST_CHECK_CLOSE(yzyx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzyx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzyx.getE4(), V.x, 1e-4f);
	auto yzyy = V.yzyy;
	BOOST_CHECK_CLOSE(yzyy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzyy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzyy.getE4(), V.y, 1e-4f);
	auto yzyz = V.yzyz;
	BOOST_CHECK_CLOSE(yzyz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzyz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzyz.getE4(), V.z, 1e-4f);
	auto yzzx = V.yzzx;
	BOOST_CHECK_CLOSE(yzzx.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzzx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzzx.getE4(), V.x, 1e-4f);
	auto yzzy = V.yzzy;
	BOOST_CHECK_CLOSE(yzzy.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzzy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzzy.getE4(), V.y, 1e-4f);
	auto yzzz = V.yzzz;
	BOOST_CHECK_CLOSE(yzzz.getE1(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(yzzz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(yzzz.getE4(), V.z, 1e-4f);
	auto zxxx = V.zxxx;
	BOOST_CHECK_CLOSE(zxxx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxxx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxxx.getE4(), V.x, 1e-4f);
	auto zxxy = V.zxxy;
	BOOST_CHECK_CLOSE(zxxy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxxy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxxy.getE4(), V.y, 1e-4f);
	auto zxxz = V.zxxz;
	BOOST_CHECK_CLOSE(zxxz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxxz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxxz.getE4(), V.z, 1e-4f);
	auto zxyx = V.zxyx;
	BOOST_CHECK_CLOSE(zxyx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxyx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zxyx.getE4(), V.x, 1e-4f);
	auto zxyy = V.zxyy;
	BOOST_CHECK_CLOSE(zxyy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxyy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zxyy.getE4(), V.y, 1e-4f);
	auto zxyz = V.zxyz;
	BOOST_CHECK_CLOSE(zxyz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxyz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zxyz.getE4(), V.z, 1e-4f);
	auto zxzx = V.zxzx;
	BOOST_CHECK_CLOSE(zxzx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxzx.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxzx.getE4(), V.x, 1e-4f);
	auto zxzy = V.zxzy;
	BOOST_CHECK_CLOSE(zxzy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxzy.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxzy.getE4(), V.y, 1e-4f);
	auto zxzz = V.zxzz;
	BOOST_CHECK_CLOSE(zxzz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxzz.getE2(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zxzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zxzz.getE4(), V.z, 1e-4f);
	auto zyxx = V.zyxx;
	BOOST_CHECK_CLOSE(zyxx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyxx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zyxx.getE4(), V.x, 1e-4f);
	auto zyxy = V.zyxy;
	BOOST_CHECK_CLOSE(zyxy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyxy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zyxy.getE4(), V.y, 1e-4f);
	auto zyxz = V.zyxz;
	BOOST_CHECK_CLOSE(zyxz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyxz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zyxz.getE4(), V.z, 1e-4f);
	auto zyyx = V.zyyx;
	BOOST_CHECK_CLOSE(zyyx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyyx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyyx.getE4(), V.x, 1e-4f);
	auto zyyy = V.zyyy;
	BOOST_CHECK_CLOSE(zyyy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyyy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyyy.getE4(), V.y, 1e-4f);
	auto zyyz = V.zyyz;
	BOOST_CHECK_CLOSE(zyyz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyyz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyyz.getE4(), V.z, 1e-4f);
	auto zyzx = V.zyzx;
	BOOST_CHECK_CLOSE(zyzx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyzx.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyzx.getE4(), V.x, 1e-4f);
	auto zyzy = V.zyzy;
	BOOST_CHECK_CLOSE(zyzy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyzy.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyzy.getE4(), V.y, 1e-4f);
	auto zyzz = V.zyzz;
	BOOST_CHECK_CLOSE(zyzz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyzz.getE2(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zyzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zyzz.getE4(), V.z, 1e-4f);
	auto zzxx = V.zzxx;
	BOOST_CHECK_CLOSE(zzxx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzxx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzxx.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zzxx.getE4(), V.x, 1e-4f);
	auto zzxy = V.zzxy;
	BOOST_CHECK_CLOSE(zzxy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzxy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzxy.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zzxy.getE4(), V.y, 1e-4f);
	auto zzxz = V.zzxz;
	BOOST_CHECK_CLOSE(zzxz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzxz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzxz.getE3(), V.x, 1e-4f);
	BOOST_CHECK_CLOSE(zzxz.getE4(), V.z, 1e-4f);
	auto zzyx = V.zzyx;
	BOOST_CHECK_CLOSE(zzyx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzyx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzyx.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zzyx.getE4(), V.x, 1e-4f);
	auto zzyy = V.zzyy;
	BOOST_CHECK_CLOSE(zzyy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzyy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzyy.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zzyy.getE4(), V.y, 1e-4f);
	auto zzyz = V.zzyz;
	BOOST_CHECK_CLOSE(zzyz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzyz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzyz.getE3(), V.y, 1e-4f);
	BOOST_CHECK_CLOSE(zzyz.getE4(), V.z, 1e-4f);
	auto zzzx = V.zzzx;
	BOOST_CHECK_CLOSE(zzzx.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzx.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzx.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzx.getE4(), V.x, 1e-4f);
	auto zzzy = V.zzzy;
	BOOST_CHECK_CLOSE(zzzy.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzy.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzy.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzy.getE4(), V.y, 1e-4f);
	auto zzzz = V.zzzz;
	BOOST_CHECK_CLOSE(zzzz.getE1(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzz.getE2(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzz.getE3(), V.z, 1e-4f);
	BOOST_CHECK_CLOSE(zzzz.getE4(), V.z, 1e-4f);
}

BOOST_AUTO_TEST_CASE(assign_op) {
	vmath::core::Vector<float, 2> V2;
	V2.x = 20.12f;
	V2.y = 100.89f;
	vmath::core::Vector<float, 3> V3;
	V3.x = 20.12f;
	V3.y = 100.89f;
	V3.z = -18.2f;
	vmath::core::Vector<float, 3> V;
	V.x = 0.0f;
	V.y = 0.0f;
	V.z = 0.0f;
	// 2d swizzle assign <x, y, z> from vector
	V.xy = V2;
	BOOST_CHECK_CLOSE(V.x, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.y, 1e-4f);
	BOOST_CHECK_SMALL(V.z, 1e-7f);
	V.xz = V2;
	BOOST_CHECK_CLOSE(V.x, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.y, 1e-4f);
	V.yx = V2;
	BOOST_CHECK_CLOSE(V.x, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.y, 1e-4f);
	V.yz = V2;
	BOOST_CHECK_CLOSE(V.x, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.y, 1e-4f);
	V.zx = V2;
	BOOST_CHECK_CLOSE(V.x, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.x, 1e-4f);
	V.zy = V2;
	BOOST_CHECK_CLOSE(V.x, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.x, 1e-4f);
	// 2d swizzle assign <x, y, z> from swizzle
	V.xy = V2.xx;
	BOOST_CHECK_CLOSE(V.x, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.x, 1e-4f);
	V.xz = V2.yy;
	BOOST_CHECK_CLOSE(V.x, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.y, 1e-4f);
	V.yx = V.xy;
	BOOST_CHECK_CLOSE(V.x, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.y, 1e-4f);
	V.yz = V.xx;
	BOOST_CHECK_CLOSE(V.x, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.x, 1e-4f);
	V.zx = V2.yy;
	BOOST_CHECK_CLOSE(V.x, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.y, 1e-4f);
	V.zy = V2.xy;
	BOOST_CHECK_CLOSE(V.x, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V2.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V2.x, 1e-4f);
	// 3d swizzle assign <x, y, z> from vector
	V.xyz = V3;
	BOOST_CHECK_CLOSE(V.x, V3.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.z, 1e-4f);
	V.xzy = V3;
	BOOST_CHECK_CLOSE(V.x, V3.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.z, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.y, 1e-4f);
	V.yxz = V3;
	BOOST_CHECK_CLOSE(V.x, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.z, 1e-4f);
	V.yzx = V3;
	BOOST_CHECK_CLOSE(V.x, V3.z, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.y, 1e-4f);
	V.zxy = V3;
	BOOST_CHECK_CLOSE(V.x, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.z, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.x, 1e-4f);
	V.zyx = V3;
	BOOST_CHECK_CLOSE(V.x, V3.z, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.x, 1e-4f);
	// 3d swizzle assign <x, y, z> from swizzle
	V.xyz = V3.xyz;
	BOOST_CHECK_CLOSE(V.x, V3.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.z, 1e-4f);
	V.xzy = V3.yyx;
	BOOST_CHECK_CLOSE(V.x, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.y, 1e-4f);
	V.yxz = V3.yxy;
	BOOST_CHECK_CLOSE(V.x, V3.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.y, 1e-4f);
	V.yzx = V3.zzz;
	BOOST_CHECK_CLOSE(V.x, V3.z, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.z, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.z, 1e-4f);
	V.zxy = V3.xyz;
	BOOST_CHECK_CLOSE(V.x, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.z, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.x, 1e-4f);
	V.zyx = V3.zyx;
	BOOST_CHECK_CLOSE(V.x, V3.x, 1e-4f);
	BOOST_CHECK_CLOSE(V.y, V3.y, 1e-4f);
	BOOST_CHECK_CLOSE(V.z, V3.z, 1e-4f);
}

BOOST_AUTO_TEST_CASE(equals) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = 18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = 15.5f;
	V2.z = 20.2f;
	vmath::core::Vector<float, 3> V3;
	V3.x = 20.12f;
	V3.y = 100.89f;
	V3.z = 18.2f;
	BOOST_CHECK(!V1.xyz.equals(V2));
	BOOST_CHECK(!V2.xyz.equals(V3));
	BOOST_CHECK(V1.xyz.equals(V1));
	BOOST_CHECK(V1.xyz.equals(V3));
}

BOOST_AUTO_TEST_CASE(equals_specify_ulp) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = 18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = 15.5f;
	V2.z = 20.2f;
	vmath::core::Vector<float, 3> V3;
	V3.x = 20.12f;
	V3.y = 100.89f;
	V3.z = 18.2f;
	BOOST_CHECK(!V1.xyz.equals(V2, 3));
	BOOST_CHECK(!V2.xyz.equals(V3, 3));
	BOOST_CHECK(V1.xyz.equals(V1, 3));
	BOOST_CHECK(V1.xyz.equals(V3, 3));
}

BOOST_AUTO_TEST_CASE(equals_op) {
	vmath::core::Vector<float, 3> V1;
	V1.x = 20.12f;
	V1.y = 100.89f;
	V1.z = 18.2f;
	vmath::core::Vector<float, 3> V2;
	V2.x = 10.34f;
	V2.y = 15.5f;
	V2.z = 20.2f;
	vmath::core::Vector<float, 3> V3;
	V3.x = 20.12f;
	V3.y = 100.89f;
	V3.z = 18.2f;
	BOOST_CHECK(V1.xyz != V2);
	BOOST_CHECK(V2.xyz != V3);
	BOOST_CHECK(V1.xyz == V1);
	BOOST_CHECK(V1.xyz == V3);
}

BOOST_AUTO_TEST_SUITE_END()


#pragma once

#include <boost/multiprecision/cpp_bin_float.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
#include <boost/multiprecision/cpp_int.hpp>

#include <array>
#include <sstream>

namespace futurehead_pow_server
{
/** A generic wrapper around multiprecision integers, adding convenience functions */
template <typename T, size_t SIZE_BYTES>
struct bigint
{
	bigint () = default;
	bigint (T val)
	{
		set (val);
	}

	bigint (std::string const & val)
	{
		from_hex (val);
	}

	void set (T const & number_a)
	{
		bytes.fill (0);
		boost::multiprecision::export_bits (number_a, bytes.rbegin (), 8, false);
	}

	T number () const
	{
		T result;
		boost::multiprecision::import_bits (result, bytes.begin (), bytes.end ());
		return result;
	}

	std::string to_dec () const
	{
		std::stringstream stream;
		stream << std::dec << std::noshowbase;
		stream << number ();
		return stream.str ();
	}

	std::string to_hex () const
	{
		std::stringstream stream;
		stream << std::hex << std::uppercase << std::noshowbase << std::setw (SIZE_BYTES * 2) << std::setfill ('0');
		stream << number ();
		return stream.str ();
	}

	void from_hex (std::string const & text)
	{
		if (!text.empty () && text.size () <= SIZE_BYTES * 2)
		{
			std::stringstream stream (text);
			stream << std::hex << std::noshowbase;

			T number;
			stream >> number;
			set (number);
			if (!stream.eof ())
			{
				throw std::runtime_error ("from_hex failed: invalid input format");
			}
		}
		else
		{
			throw std::runtime_error ("from_hex failed: invalid input size");
		}
	}

	union
	{
		std::array<uint8_t, SIZE_BYTES> bytes;
		std::array<uint64_t, SIZE_BYTES / sizeof (uint64_t)> qwords;
	};
};

using u128 = bigint<boost::multiprecision::uint128_t, 16>;
using u256 = bigint<boost::multiprecision::uint256_t, 32>;
using u512 = bigint<boost::multiprecision::uint512_t, 64>;
using bigfloat = boost::multiprecision::cpp_bin_float_100;

inline double to_multiplier (futurehead_pow_server::u128 const difficulty_a, futurehead_pow_server::u128 const base_difficulty_a)
{
	assert (difficulty_a.number () > 0);
	bigfloat res = (bigfloat (difficulty_a.number ()) / bigfloat (base_difficulty_a.number ()));
	return res.convert_to<double> ();
}

inline futurehead_pow_server::u128 from_multiplier (double const multiplier_a, futurehead_pow_server::u128 const base_difficulty_a)
{
	bigfloat res = bigfloat (base_difficulty_a.number ()) * bigfloat (multiplier_a);
	return res.convert_to<boost::multiprecision::uint128_t> ();
}
}


//////////////////////////////////////////////////////////////////////////////
// random::poisson::devroye::crtp::parameters.hpp                        	//
//                                                                          //
//                                                                          //
//  (C) Copyright 2010 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_RANDOM_POISSON_EXT_DEVROYE_CRTP_PARAMETERS_HPP_ER_2010
#define BOOST_RANDOM_POISSON_EXT_DEVROYE_CRTP_PARAMETERS_HPP_ER_2010
#include <boost/math/constants/constants.hpp>

namespace boost{
namespace random{
namespace poisson{
namespace devroye{

    // These are quantities that remain fixed throughout sampling and are only
    // modified if the poisson mean is changed.
	template<typename Int,typename T, typename P,typename IntT>
    struct parameters{
        typedef IntT converter_;

    	public:
        static const T pi;
        static const T one;
        static const T two;
        static const T eps;

        typedef Int result_type;
        typedef T input_type;

        parameters(){}
    	explicit parameters(const Int& i_mean)
        {
            using namespace boost::math;
			this->m1_ = converter_::convert( i_mean );
			this->m2_ = converter_::convert( 2 * i_mean );
			T m8  = converter_::convert( 8 * i_mean );
            T m32 = converter_::convert( 32 * i_mean );
            this->c1_ = converter_::convert(1) / m8; // Equation (6)

            // Equation (10)
            this->delta_ = 	log1p( m32 / pi, P() );
            this->delta_ = 	sqrt( this->delta() * this->m1() );

			// Below equation (7)
			this->sd1_ = sqrt( this->m1() + this->delta() / two );
            this->shape2_ = this->delta() / ( this->m2() + this->delta() );

            T a1, a2, a3, a;

			a1 = sqrt( pi * ( this->m2() + this->delta() ) );
            a1 *= exp( this->c1() );
            a2 = exp( -( this->delta() + one ) * this->shape2());
            a2 /= this->shape2();
            a3 = one;
            a = a1 + a2 + a3;
            this->p1_ = a1 / a;
            this->p2_ = a2 / a;
            this->p3_ = a3 / a;

			// Reconciliation with Fortran code
            // RL					m1
            // TWO					m2
			// CON 					c1
            // D 					delta
            // D2					delta + m2
			// D3 					1 / shape2
            // STDDEV 				sd1
            // SUM 					a1 + a2 + a3
            // PBODY 				p3 = a3/(a+a2+a3)
			// PTAIL 				p2 + p3

        }

		std::ostream& parameters_description(std::ostream& os)const{
        	return os
                << '('
                << "m1 = " 		<< this->m1()
                << ','
                << " m2 = "		<< this->m2()
                << ','
                << " delta = " 	<< this->delta()
                << ','
                << " loc1 = "	<< loc1()
                << ','
                << " sd1 = " 	<< this->sd1()
                << ','
                << " shape2 = "	<< this->shape2()
                << ','
                << " c1 = "		<< this->c1()
                << ','
                << " p1 = "		<< this->p1()
                << ','
                << " p2 = "		<< this->p2()
                << ','
                << " p3 = "		<< this->p3()
                << ')';
        }

		const T& m1()const{ return this->m1_; }
		const T& m2()const{ return this->m2_; }
		const T& delta()const{ return this->delta_; }
		static T loc1(){ return -one/two; }
		const T& sd1()const{ return this->sd1_; }
		const T& shape2()const{ return this->shape2_; }
        const T& c1()const{ return this->c1_; }
		const T& p1()const{ return this->p1_; }
		const T& p2()const{ return this->p2_; }
		const T& p3()const{ return this->p3_; }

		private:
        T inv_m1_;
		T m1_;
		T m2_;
        T delta_;
		T sd1_;
		T shape2_;

        T c1_;
        T p1_;
        T p2_;
		T p3_;

    };

    template<typename Int,typename T,typename P,typename IntT>
    const T parameters<Int,T,P,IntT>::pi = boost::math::constants::pi<T>();

    template<typename Int,typename T,typename P,typename IntT>
    const T parameters<Int,T,P,IntT>::one = IntT::convert( 1 );

    template<typename Int,typename T,typename P,typename IntT>
    const T parameters<Int,T,P,IntT>::two = IntT::convert( 2 );

    template<typename Int,typename T,typename P,typename IntT>
    const T parameters<Int,T,P,IntT>::eps
        = boost::numeric::bounds<T>::smallest();

}// devroye
}// poisson
}// random
}// boost

#endif


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2009 Jose Aparicio

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#ifndef recursive_cdo_engine_hpp
#define recursive_cdo_engine_hpp

#include <ql/math/integrals/gaussianquadratures.hpp>
#include <ql/math/distributions/normaldistribution.hpp>
#include <ql/math/matrixutilities/factorreduction.hpp>
#include <ql/experimental/credit/syntheticcdoengines.hpp>
#include <ql/experimental/credit/onefactorgaussiancopula.hpp>
#include <ql/experimental/credit/onefactorstudentcopula.hpp>
#include <boost/bind.hpp>
#include <map>
#include <algorithm>

namespace QuantLib {

    /*! Recursive STCDO pricing for a heterogeneous pool of names. The pool
        names are heterogeneous in their default probabilities, notionals
        and recovery rates. Correlations are pairwise. The recursive pricing
        algorithm used here is described in Andersen, Sidenius and Basu;
        "All your hedges in one basket", Risk, November 2003, pages 67-72

        Notice that using copulas other than Gaussian it is only an
        approximation (see remark on p.68).
    */
    template <class CDOEngine, class copulaT>
    class RecursiveCdoEngine : public CDOEngine {
      public:
        // Base constructors call default Handle constructor, the copula is to
        // be relinked by template partial specializations on the copula type

        //! Single correlation construction
        RecursiveCdoEngine(const Handle<Quote>& correl,
                           Size nbuckets  = 1,
                           Size quadOrder = 20)
        : correlQuote_(correl), copula_(), nBuckets_(nbuckets),
          integral_(quadOrder), wk_()
        {
            this->registerWith(correl);
        }

        //! Correlation name to name single factor construction
        RecursiveCdoEngine(const Handle<Quote>& correl,
                           const Matrix& correlMtrx,
                           Size nbuckets  = 1,
                           Size quadOrder = 20)
        : correlQuote_(correl), copula_(), nBuckets_(nbuckets),
          integral_(quadOrder), wk_(),
          oneFactorCorrels_(factorReduction(correlMtrx))
        {
            // at least
            QL_REQUIRE(!oneFactorCorrels_.empty(),
                "Invalid correlation parameter matrix.");
        }
      protected:
        void initialize() const;
      private:
        //! Weights the conditional portfolio loss by the mkt factor
        //    distribtion
        Real integratorLoss(const Date& date, Real mktFactor) const {
            return expectedConditionalLoss(date, mktFactor) *
               copula_->density(mktFactor);
        }
        //! Portfolio loss conditional to the market factor value
        Real expectedConditionalLoss(const Date& date,
                                     Real mktFactor) const;
      public:
        void update();

        /*  Expected tranche Loss calculation.
            This is computed from the first equation on page 70 (not numbered)
            Notice that while we want to compute:
            \f[
            EL(t) = \sum_{l_k}l_k P(l;t) =
              \sum_{l_k}l_k \int P(l_k;t|\omega) d\omega q(\omega)
            \f]
            One can invert the sumation and the integral order to:
            \f[
            EL(t) = \int\,q(\omega)\,d\omega\,\sum_{l_k}\,l_k\,P(l_k;t|\omega) =
              \int\,q(\omega)\,d\omega\,EL(t|\omega)
            \f]
            and this is the way it is integrated here. The recursion formula makes
            it easier this way.
        */
        Real expectedTrancheLoss(const Date& date) const {
            return
                integral_(boost::bind(
                    &RecursiveCdoEngine<CDOEngine, copulaT>::integratorLoss,
                    this,
                    date,
                    _1)
                );
        }
      protected:
        const Handle<Quote> correlQuote_;
        mutable RelinkableHandle<copulaT> copula_;
      private:
        // loss model descriptor members
        Size nBuckets_;
       const GaussHermiteIntegration integral_;
        mutable std::vector<Real> wk_;
        mutable Real loss_unit_;
        //! name to name factor loadings (betas). In the single factor copula:
        //    correl = beta * beta
        // When constructing through a single correlation number the factor is
        //   taken to be the positive swuare root of this number in the copula.
        mutable std::vector<Real> oneFactorCorrels_;
    };

    template <class CDOEngine, class copulaT>
    void RecursiveCdoEngine<CDOEngine, copulaT>::update() {
        oneFactorCorrels_.clear();
        CDOEngine::update();
    }

    template <class CDOEngine, class copulaT>
    void RecursiveCdoEngine<CDOEngine, copulaT>::initialize() const {
        wk_.clear();
        Date today = Settings::instance().evaluationDate();
        Date start = this->arguments_.schedule.startDate();
        boost::shared_ptr<Basket>& basket = this->arguments_.basket;
        /*
          Remove defaulted names and adjust the subordination.
        */
        std::vector<std::string> names =
            basket->remainingNames(start, today);
        std::vector<Real> notionals
            = basket->remainingNotionals(start, today);
        Real a = basket->remainingAttachmentRatio(start, today);
        Real d = basket->remainingDetachmentRatio(start, today);
        const boost::shared_ptr<Pool> pool = basket->pool();
        this->remainingBasket_ =
            boost::shared_ptr<Basket>(new Basket(names, notionals, pool,
                                                basket->remainingDefaultKeys(start, today),
                                                basket->remainingRecModels(start, today),
                                                 a, d));

        this->results_.xMin = this->remainingBasket_->attachmentAmount();
        this->results_.xMax = this->remainingBasket_->detachmentAmount();
        this->results_.remainingNotional =
            this->results_.xMax - this->results_.xMin;
        //----------------------------------------------------------------
        if(oneFactorCorrels_.empty())
            oneFactorCorrels_ = std::vector<Real>(names.size(),
                                    correlQuote_->value());
        // check size of factors:
        QL_REQUIRE(oneFactorCorrels_.size() == names.size(),
            "Size of matrix must match number of names in the basket.");
        //
        std::vector<Real> lgdsTmp, lgds = this->remainingBasket_->LGDs();
        lgdsTmp = lgds;
        lgds.erase(std::remove(lgds.begin(), lgds.end(), 0.), lgds.end());
        loss_unit_ = *(std::min_element(lgds.begin(), lgds.end()))
            / nBuckets_;

        for(Size i = 0; i<names.size(); i++)
            wk_.push_back(std::floor(lgdsTmp[i]/loss_unit_ + .5));

        // Could not check parameters at construction time because we
        //   had no arguments yet, do it now:
        if(oneFactorCorrels_.size() == 1)
            oneFactorCorrels_ =
                std::vector<Real>(pool->size(), oneFactorCorrels_[0]);
        else
            QL_REQUIRE(oneFactorCorrels_.size() == this->remainingBasket_->size(),
                "Incompatible correl matrix, pool size.");
        //----------------------------------------------------------------
        const std::vector<Date>& dates = this->arguments_.schedule.dates();
        for (Size i = 0; i < dates.size(); i++) {
            if (dates[i] <= today)
                this->results_.expectedTrancheLoss.push_back(0.0);
            else {
                Real L = expectedTrancheLoss(dates[i]);
                this->results_.expectedTrancheLoss.push_back(L);
            }
        }

    }

    //! Portfolio loss conditional to the market factor value
    template <class CDOEngine, class copulaT>
    Real RecursiveCdoEngine<CDOEngine, copulaT>::expectedConditionalLoss(
                                 const Date& date,
                                 Real mktFactor) const {
        const std::vector<std::string>& names = this->remainingBasket_->names();

        // eq. 10 p.68
        // attainable losses distribution, recursive algorithm
        std::vector<Probability> uncDefProb =
            this->remainingBasket_->probabilities(date);;
        std::map<Real, Probability> pIndepDistrib;
        // K=0
        pIndepDistrib.insert(std::make_pair(0., 1.));
        for(Size iName=0; iName<names.size(); iName++) {

            // to do: allow for matrix constructor and uncoment this
            // correlQuote_->setValue(oneFactorCorrels_[iName]);

            Probability pDef =
                copula_->conditionalProbability(uncDefProb[iName],
                                                mktFactor);
            // iterate on all possible losses in the distribution:
            std::map<Real, Probability> pDistTemp;
            std::map<Real, Probability>::iterator distIt =
                pIndepDistrib.begin();
            while(distIt != pIndepDistrib.end()) {
                // update prob if this name does not default
                std::map<Real, Probability>::iterator matchIt
                    = pDistTemp.find(distIt->first);
                if(matchIt != pDistTemp.end()) {
                    matchIt->second += distIt->second * (1.-pDef);
                }else{
                    pDistTemp.insert(std::make_pair(distIt->first,
                        distIt->second * (1.-pDef)));
                }
                // and if it does
                matchIt = pDistTemp.find(distIt->first + wk_[iName]);
                if(matchIt != pDistTemp.end()) {
                    matchIt->second += distIt->second * pDef;
                }else{
                    pDistTemp.insert(std::make_pair(
                        distIt->first+wk_[iName], distIt->second * pDef));
                }
                distIt++;
            }
            // copy back
            pIndepDistrib = pDistTemp;
        }

        // get the expected value subject to the value of the market
        //   factor.
        Real expLoss = 0.;
        //---------------------------------------------------------------
        /* This is the original (easy to read) loop which I have partially
             unroll below to take profit of the fact that once we go over
             the tranche top the loss amount is fixed:

        std::map<Real, Probability>::iterator distIt =
            pIndepDistrib.begin();
        while(distIt != pIndepDistrib.end()) {
            Real loss = distIt->first * loss_unit_
                                ;
            loss = std::max(std::min(loss,
                results_.xMax)-results_.xMin, 0.);
            expLoss += loss * distIt->second;
            distIt++;
        }
        return expLoss ;
        */
        //---------------------------------------------------------------
        Real relativeMax = this->results_.xMax / loss_unit_;
        Real relativeMin = this->results_.xMin / loss_unit_;
        Size relativeMinIdx = std::floor(relativeMin);
        std::map<Real, Probability>::iterator
            distIt = pIndepDistrib.lower_bound(relativeMinIdx),
            itTop  = pIndepDistrib.lower_bound(relativeMax);
        for(; distIt != itTop; distIt++)
            expLoss += std::max(std::min(distIt->first, relativeMax)
                                -relativeMin, 0.) * distIt->second;
        Real sumProbs = 0.;
        for(;distIt != pIndepDistrib.end(); distIt++)
            sumProbs += distIt->second;
        return expLoss * loss_unit_ + this->results_.remainingNotional * sumProbs;
    }

    // Partial specializations on the copula type. Kind
    //   of template virtual constructions. Allows to own the correlation
    //   quote and the copula for each specific copula type. It only
    //   needs to know its particular copula constructor.
    // These and the base correlation pricers use a unifactorial copula
    //   which needs to be modified for different parameters of the
    //   correlation per name to name if theres such an structure or
    //   because the correlation parameter has got a time or/and loss
    //   level surface.
    //
    // TO do: Correlation matrix constructors.

    //! Specialization for Gaussian copula, the integration still remains
    //    to be defined by the user out of the available ones in
    //    syntheticcdoengines.
    template <class CDOEngine>
    class GaussianRecursiveCdoEngine : public
        RecursiveCdoEngine<CDOEngine, OneFactorGaussianCopula> {
      public:
        //! quote constructor.
        GaussianRecursiveCdoEngine(
            const Handle<Quote>& correlQuote,
            Size nbuckets  = 1,
            Size quadOrder = 12,
            Real maxval    = 5.,
            Size steps     = 50)
            :
            RecursiveCdoEngine<CDOEngine, OneFactorGaussianCopula>(correlQuote,
                    nbuckets, quadOrder) {
                this->copula_.linkTo(boost::shared_ptr<OneFactorGaussianCopula>(
                new OneFactorGaussianCopula(correlQuote,  maxval, steps)), true);
        }
    };

    template <class CDOEngine>
    class StudentRecursiveCdoEngine : public
        RecursiveCdoEngine<CDOEngine, OneFactorStudentCopula> {
      public:
        //! quote constructor.
        StudentRecursiveCdoEngine(
            const Handle<Quote>& correlQuote,
            Size nz,
            Size nm,
            Size nbuckets  = 1,
            Size quadOrder = 12,
            Real maxval    = 5.,
            Size steps     = 50)
            :
            RecursiveCdoEngine<CDOEngine, OneFactorStudentCopula>(correlQuote,
                    nbuckets, quadOrder) {
                this->copula_.linkTo(boost::shared_ptr<OneFactorStudentCopula>(
                new OneFactorStudentCopula(correlQuote, nz, nm, maxval, steps)), true);
        }
    };

    typedef GaussianRecursiveCdoEngine<MidPointCDOEngine> GaussRecCDOEngine;
    typedef StudentRecursiveCdoEngine<MidPointCDOEngine>  StudentRecCDOEngine;

}

#endif


#include <iostream>
#include <fstream>

#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>

#include <vector>
#include <cmath>

#include "NRGclasses.hpp"
#include "NRGfunctions.hpp"
#include "TwoChQS.hpp"

#ifndef pi
#define pi 3.141592653589793238462643383279502884197169
#endif

int main (int argc, char* argv[]){

  //char ModelOption[]="Anderson";
  //int ModelNo=0;

  // command-line model options
#include"ModelOpt.cpp"

  CNRGarray Aeig(2);

  CNRGbasisarray AeigCut(2);

  CNRGbasisarray Abasis(2);

  CNRGbasisarray SingleSite(3);

  CNRGmatrix* MatArray;
  int NumNRGarrays=2;
  // MatArray 0 is nd
  // MatArray 1 is cd

  // Thermodynamics
  CNRGthermo Suscep;
  CNRGthermo Entropy;

  // STL vector

  CNRGmatrix Qm1fNQ[2];

  CNRGmatrix MQQp1;

  double U,ed,Gamma1,Gamma2;
  vector<double> Params;
  double Lambda;
  double HalfLambdaFactor;
  double Dband=1.0;
  int calcdens;

  double DN=0.0;
  double TM=0.0;
  double betabar=0.727;
  double Temp=0.0;
  double Sus=0.0;
  //vector<double> SuscepChain;
  char arqSus[32],arqname[32];

  double chi_m1,chi_N[2];
  double daux[4];

  int Nsites=0,Nsitesmax=2;

  int Ncutoff=700;
  int UpdateBefCut=0;
  int auxIn;

  // outstream
  ofstream OutFile;
  // instream
  ifstream InFile;

  int ii,jj,i1,i2;

  // STL iterator:

  vector<double>::iterator diter;

  // Phonon
  // Test!! Remove later
  double Nph=1.0;
  double w0=0.2;
  double lambda=0.4;
  double alpha=0.4;
  double tp=0.2;

  ///            ///
  /// Begin code ///
  ///            ///
  U=0.5;
  ed=-0.5*U;
  Gamma1=0.0282691;
  Gamma2=0.0;
  Lambda=2.5;

  // Steps: Input parameters
  InFile.open("nrg_input_TwoCh.dat");
  if (InFile.is_open())
    {
      InFile >> Nsitesmax;
      InFile >> Ncutoff;
      InFile >> U;
      InFile >> Gamma1;
      InFile >> Gamma2;
      InFile >> ed;
      InFile >> Lambda;
      InFile >> Dband;
      InFile >> auxIn;
      InFile >> UpdateBefCut;
      InFile >> calcdens;
    }
  else cout << "can't open nrg_input_TwoCh.dat" << endl;

  InFile.close();

  if (ModelNo==4)
    {
      InFile.open("Input_Phonon.dat");
      if (InFile.is_open())
	{
	  InFile >> Nph;
	  InFile >> w0;
	  InFile >> lambda;
	  InFile >> tp;
	  InFile >> alpha;
	}
      else cout << "can't open Input_Phonon.dat" << endl;
      InFile.close();
      Gamma1=pi*tp*tp;
      Gamma2=alpha*alpha*Gamma1;
    }
  // end if phonon

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

  // New stuff
   strcpy(Suscep.ArqName,"SuscepImp2Ch_25_726.dat");
   strcpy(Suscep.ChainArqName,"SuscepChain2Ch.dat");
   Suscep.Calc=CalcSuscep;

   strcpy(Entropy.ArqName,"EntropyImp2Ch_25_726.dat");
   strcpy(Entropy.ChainArqName,"EntropyChain2Ch.dat");
   Entropy.Calc=CalcEntropy;

   // Chain model
   if(ModelNo==3) calcdens=3;

   if (calcdens==2)
     {
       Suscep.CalcChain=false;
       Entropy.CalcChain=false;
       Suscep.Nsite0Chain=0; // Anderson chains
       Entropy.Nsite0Chain=0; // Anderson chains

       if ( (!FileExists(Suscep.ChainArqName))||
	    (!FileExists(Entropy.ChainArqName)) )
	 {
	   cout << " Can't find chain files: " << endl
		<< "   " << Suscep.ChainArqName << endl
		<< "   " << Entropy.ChainArqName << endl;
	   cout << " Exiting... " << endl;
	   exit(0);
	 }
       else
	 cout << " Found files "
	      << Suscep.ChainArqName << ", "
	      << Entropy.ChainArqName
	      << endl;
     }
   if (calcdens==3)
     {
       //strcpy(arqSus,"SuscepChain.dat");
       Suscep.CalcChain=true;
       Entropy.CalcChain=true;
       if ( (FileExists(Suscep.ChainArqName))||
	    (FileExists(Entropy.ChainArqName)) )
	 {
	   cout << " Error: Chain files already exist: " << endl
		<< "   " << Suscep.ChainArqName << endl
		<< "   " << Entropy.ChainArqName << endl;
	   cout << " Not rewriting. Exiting... " << endl;
	   exit(0);
	 }
       U=0.0;ed=0.0;Gamma1=0.0;Gamma2=0.0;
     }

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

  HalfLambdaFactor=0.5*(1.0+(1.0/Lambda));
  chi_m1=sqrt(2.0*Gamma1/pi)/(sqrt(Lambda)*HalfLambdaFactor);

  double U_tilde=0.5*U/HalfLambdaFactor;
  double ed_tilde=ed/HalfLambdaFactor;
  double Gamma1_tilde=Gamma1*(2.0/pi)/(HalfLambdaFactor*HalfLambdaFactor);
  double Gamma2_tilde=Gamma2*(2.0/pi)/(HalfLambdaFactor*HalfLambdaFactor);

  OutFile.open("NRG_in.txt");
  OutFile << "Begin NRG 2-ch calculation" << endl;
  OutFile << " Model : " << ModelOption << endl;
  OutFile << " Nsitesmax    = " << Nsitesmax-1 << endl;
  OutFile << " Ncutoff      = " << Ncutoff << endl;
  OutFile << " U            = " << U << endl;
  OutFile << " Gamma1        = " << Gamma1 << endl;
  OutFile << " Gamma2        = " << Gamma2 << endl;
  OutFile << " ed           = " << ed << endl;
  OutFile << " Lambda       = " << Lambda << endl;
  OutFile << " Dband        = " << Dband << endl;
  OutFile << " UpdateBefCut = " << UpdateBefCut << endl;
  OutFile << " calcdens     = " << calcdens << endl;
  OutFile << "=================================" << endl;
  OutFile << "U~ = " << U_tilde << endl;
  OutFile << "ed~ = " <<  ed_tilde << endl;
  OutFile << "Gamma1~ = " <<  Gamma1_tilde<< endl;
  OutFile << "Gamma2~ = " <<  Gamma2_tilde<< endl;
  OutFile << "=================================" << endl;
  if (ModelNo==4)
    {
      OutFile << "Nph = " <<  Nph << endl;
      OutFile << "w0 = " <<  w0 << endl;
      OutFile << "lambda = " <<  lambda << endl;
      OutFile << "tp = " <<  tp << endl;
      OutFile << "alpha = " <<  alpha << endl;
      OutFile << "chi_S_tilde = " <<  sqrt(Gamma1_tilde) << endl;
      OutFile << "chi_A_tilde = " <<  alpha*sqrt(Gamma1_tilde) << endl;
      OutFile << "=================================" << endl;
    }
  OutFile.close();

  // Define H0 (impurity + 1s site)
  // Output:
  //       Aeig,
  //       fd_{1sigma} and fd_{2sigma} matrix elements
  //

  // Set single site (use pointers in the subroutines!!)

  TwoChQS_SetSingleSite(&SingleSite);

  Params.push_back(U_tilde/sqrt(Lambda));
  Params.push_back(ed_tilde/sqrt(Lambda));
  Params.push_back(sqrt(Gamma1_tilde/Lambda));
  Params.push_back(sqrt(Gamma2_tilde/Lambda));

  Nsites=0;
  DN=HalfLambdaFactor*pow(Lambda,(-(Nsites-1)/2.0) );
  TM=DN/betabar;
  cout << "DN = " << DN << "  TM = " << TM << endl;

  switch (ModelNo)
    {
    case 0 :
      TwoChQS_SetH0Anderson(Params,&SingleSite,&Aeig,&Abasis);
      Suscep.dImpValue=1.0/8.0;
      Entropy.dImpValue=2.0*log(2.0);
      break;

    case 1 :
      cout << " Testing 2ch Kondo model. " << endl;
      Params.clear();
      Params.push_back(Gamma1);
      Params.push_back(Gamma2);
      TwoChQS_SetH0Kondo(Params,&SingleSite,&Aeig,&Abasis,MatArray);
      Suscep.dImpValue=1.0/4.0;
      Entropy.dImpValue=log(2.0);
      break;

    case 3 :
      TwoChQS_SetH0Chain(Params,&SingleSite,&Aeig,&Abasis);
      Suscep.dImpValue=0.0;
      Entropy.dImpValue=0.0; // Check this!!
      Suscep.CalcChain=true;
      Entropy.CalcChain=true;
      break;
    case 4 :
      Params.push_back(w0); //
      Params.push_back(lambda); //
      Params.push_back(alpha); //
      Params.push_back(Nph); //
      // Using the untransformed Hamiltonian
      //TwoChQS_SetH0CMphononwTransf(Params,&SingleSite,&Aeig,&Abasis,MatArray);
      TwoChQS_SetH0CMphonon(Params,&SingleSite,&Aeig,&Abasis,MatArray);
      // Qm1fQ needs special updates
      for (int ich=0;ich<=1;ich++)
	{
	  Qm1fNQ[ich].NeedOld=false;
	  Qm1fNQ[ich].CheckForMatEl=TwoChQS_cd_check;
	}

      Qm1fNQ[0].CalcMatEl=TwoChQS_cd_ich1_Phonon_MatEl;
      Qm1fNQ[1].CalcMatEl=TwoChQS_cd_ich2_Phonon_MatEl;
      /////////
      Suscep.dImpValue=0.0;
      Entropy.dImpValue=0.0; // Check this!!
      strcpy(Suscep.ArqName,"SuscepImp2Ch_Phonon.dat");
      strcpy(Entropy.ArqName,"EntropyImp2Ch_Phonon.dat");
      // Update with a new scheme
      cout << " Updating! " << endl;
      AeigCut.CNRGbasisarray::ClearAll();
      AeigCut=CutStates(&Aeig, Ncutoff);
      UpdateMatrices(&SingleSite,&AeigCut,
		     &Abasis,&Qm1fNQ[0], 2);

      break;
    default :
      cout << " Model not implemented. Exiting... " << endl;
      exit(0);
      break;
    }
  //end switch models

  Aeig.PrintEn();

  if (ModelNo!=4){TwoChQS_UpdateQm1fQ(&SingleSite,&Aeig,&Abasis,Qm1fNQ);}

  cout << " f0_ch1 : " << endl;
  //Qm1fNQ[0].PrintAllBlocks();
  Qm1fNQ[0].PrintMatBlock(1,3);
  Qm1fNQ[0].PrintMatBlock(2,3);
  cout << " f0_ch1 : " << endl;
  //Qm1fNQ[1].PrintAllBlocks();
  Qm1fNQ[1].PrintMatBlock(1,3);
  Qm1fNQ[1].PrintMatBlock(2,3);

  // Calculate Susceptibility at Nsites=0!

  if ( (calcdens==2)||(calcdens==3) )
    {
      TM=DN/betabar;
      Params.clear();
      Params.push_back(betabar);

      Suscep.ReadNChainValue(Nsites,0);
      Suscep.AddValue(Params,&Aeig,1,true,TM);
      Suscep.SaveNValue(Nsites,0);

      Entropy.ReadNChainValue(Nsites,0);
      Entropy.AddValue(Params,&Aeig,1,true,TM);
      Entropy.SaveNValue(Nsites,0);

    }
  //end if calcdens=2 or 3

  //TwoChQS_UpdateQm1fQ(&SingleSite,&Aeig,&Abasis,Qm1fNQ);

  // Check Matrix
//    for (int ich=1;ich<=2;ich++)
//      {
//        cout << " Printing Qm1fQ channel: " << ich << endl;
//        for (int ibl=0; ibl<Qm1fNQ[ich-1].NumMatBlocks();ibl++)
//  	{
//  	  Qm1fNQ[ich-1].PrintMatBlock(ibl);
//  	}
//      }

  // Loop on Nsites: start from Nsites=0 (imp+1)
  //

  Nsites=1;
  while (Nsites<=Nsitesmax)
    {

      DN=HalfLambdaFactor*pow(Lambda,(-(Nsites-1)/2.0) );
      TM=DN/betabar;
      cout << "DN = " << DN << "TM = " << TM << endl;

      // 0 - Update chi_N, eps_N

      daux[0]=(double)( 1.0-pow(Lambda,(-Nsites)) );
      daux[1]=(double)sqrt( 1.0-pow(Lambda,-(2*Nsites-1)) );
      daux[2]=(double)sqrt( 1.0-pow(Lambda,-(2*Nsites+1)) );
      daux[3]=0.5*(1.0+(1.0/Lambda))*(double)sqrt(Lambda);

      chi_N[0]=daux[0]/(daux[1]*daux[2]);
      chi_N[1]=daux[0]/(daux[1]*daux[2]);

      cout << "chi_N = " << chi_N[0] << "  " << chi_N[1] << endl;

      cout << "Nsites = " << Nsites << endl;
      cout << "BEG Eig Nshell = " << Aeig.Nshell << endl;

      // 1 - Eliminate states and Build Abasis

      cout << " Nstates: ";
      cout <<  Aeig.Nstates() << endl;

      cout << "Cutting states..." << endl;
      AeigCut.CNRGbasisarray::ClearAll();
      AeigCut=CutStates(&Aeig, Ncutoff);

      cout << "... done cutting states." << endl;

      // Calculate new matrix elements using the CUT basis:
      // update Qm1fNQ, Qm1cdQ, etc.

      if ( (UpdateBefCut==0)&&(Nsites>1) )
	{
	  cout << "Updating matrices after cutting... " << endl;
	  TwoChQS_UpdateMatrixAfterCutting(&SingleSite,
	  				    &AeigCut, &Abasis, Qm1fNQ, &MQQp1);
	  cout << "... done updating matrices. " << endl;
	}

      // Spec density calculation, other similar things woudl go HERE

      // This is where the loop really begins...

      QS_BuildBasis(&AeigCut,&Abasis,&SingleSite,UpdateBefCut);

      cout << "No blocks = " << Abasis.NumBlocks() << endl;

      cout << "No states = " << Abasis.Nstates() << endl;

      //Abasis.PrintAll();

      // 2 - Build and diagonalize H_N+1

      Params.clear();
      Params.push_back(chi_N[0]);
      Params.push_back(chi_N[1]);
      Params.push_back(Lambda);

      cout << "Diagonalizing HN... " << endl;

      TwoChQS_DiagHN(Params,&Abasis,&SingleSite,Qm1fNQ,&Aeig);

      Aeig.PrintEn();

      cout << "..done diagonalizing HN. " << endl;

      // 3 - Update Qm1f1NQ, Qm1f2Q, Qm1cdQ, etc.

      if ( (UpdateBefCut==1)&&(Nsites<Nsitesmax) )
	{
	  cout << "Updating matrices before cutting... " << endl;
	  TwoChQS_UpdateQm1fQ(&SingleSite,&Aeig,&Abasis,Qm1fNQ);
	  cout << "... done updating matrices. " << endl;

	}

      // Calculate Susceptibility/Entropy

      if ( (calcdens==2)||(calcdens==3) )
	{
	  TM=DN/betabar;
	  Params.clear();
	  Params.push_back(betabar);

	  Suscep.ReadNChainValue(Nsites,0);
	  Suscep.AddValue(Params,&Aeig,1,true,TM);
	  Suscep.SaveNValue(Nsites,0);

	  Entropy.ReadNChainValue(Nsites,0);
	  Entropy.AddValue(Params,&Aeig,1,true,TM);
	  Entropy.SaveNValue(Nsites,0);
	}
  //end if calcdens=2 or 3

      // 4 - Update Nsites

      Nsites++;

    }

  cout << "=== Calculation Finished! ==== "<< endl;
  OutFile.open("NRG_end.txt");
  OutFile << "END NRG calculation" << endl;
  OutFile.close();

  cout << "Calling destructors " << endl;

}
//END code

///////////////////////
//       else
// 	{
// 	  while (!InFile.eof())
// 	    {
// 	      InFile >> Temp >> daux[0] >> daux[0] >> Sus;
// 	      SuscepChain.push_back(Sus);
// 	    }
// 	  SuscepChain.pop_back();
// 	  if (SuscepChain.size()<Nsitesmax-1)
// 	    {
// 	      cout << "Nsuscep = " << SuscepChain.size() << " Nsitesmax = " << Nsitesmax << endl;
// 	      cout << " Longer SuscepChain needed! Doing it all over..." << endl;
// 	      SuscepChain.clear();
// 	      strcpy(arqSus,"SuscepChain.dat");
// 	      calcdens=3;
// 	    }
// 	}
//       InFile.close();
//////////////////////////

      //SuscepChain.clear();
//       strcpy(arqSus,"SuscepImp_25_726.dat");

//       strcpy(arqname,"SuscepChain.dat");
//       InFile.open(arqname);
//       if (InFile.fail())
// 	{
// 	  cout << "Can't find " << arqname << endl;
// 	  strcpy(arqSus,"SuscepChain.dat");
// 	  calcdens=3;
// 	}

//       Sus=CalcSuscep(Params,&Aeig,1,true);
//       double Sus0=0.0;
//       int nlines=0;
//       if (calcdens==2)
// 	{
// 	  InFile.open("SuscepChain.dat");
// 	  InFile.clear();
// 	  InFile.seekg(0, ios::beg); // rewind
// 	  if (InFile.fail())
// 	    {
// 	      cout << "Can't open  SuscepChain.dat"<< endl;
// 	    }
// 	  else
// 	    {
// 	      while ( (!InFile.eof())&&(nlines<=Nsites) )
// 		{
// 		  InFile >> daux[0] >> daux[0] >> daux[0] >> Sus0;
// 		  nlines++;
// 		}
// 	    }
// 	  InFile.close();
// 	}
//        else
// 	Sus-=1.0/8.0; // exclude dot site in the chain.
//       if (Nsites==0) OutFile.open(arqSus);
//       else OutFile.open(arqSus,ofstream::app);
//       OutFile.precision(20);
//       OutFile << scientific << TM << " " << Sus << " " << Sus0 << " " << Sus-Sus0 << endl;
//       OutFile.close();

// 	  Sus=CalcSuscep(Params,&Aeig,1,true);
// 	  double Sus0=0.0;
// 	  int nlines=0;
// 	  // Gets SuscepChain iteration by iteration
// 	  if (calcdens==2)
// 	    {
// 	      InFile.open("SuscepChain.dat");
// 	      InFile.clear();
// 	      InFile.seekg(0, ios::beg); // rewind
// 	      if (InFile.fail())
// 		{
// 		  cout << "Can't find " << arqname << endl;
// 		}
// 	      else
// 		{
// 		  while ( (!InFile.eof())&&(nlines<=Nsites) )
// 		    {
// 		      InFile >> daux[0] >> daux[0] >> daux[0] >> Sus0;
// 		      nlines++;
// 		    }
// 		}
// 	      InFile.close();
// 	    }
// 	  //if (calcdens==2) Sus0=SuscepChain[Nsites];
// 	  else Sus-=1.0/8.0; // exclude dot site in the chain.

// 	  if (Nsites==0) OutFile.open(arqSus);
// 	  else OutFile.open(arqSus,ofstream::app);
// 	  OutFile.precision(20);
// 	  OutFile << scientific << TM << " " << Sus << " " << Sus0 << " " << Sus-Sus0 << endl;
// 	  OutFile.close();


// Copyright 2017 Nicolas Mellado
//
// 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.
//
// -------------------------------------------------------------------------- //
//
// Authors: Dror Aiger, Yoni Weill, Nicolas Mellado
//
// This file is part of the implementation of the 4-points Congruent Sets (4PCS)
// algorithm presented in:
//
// 4-points Congruent Sets for Robust Surface Registration
// Dror Aiger, Niloy J. Mitra, Daniel Cohen-Or
// ACM SIGGRAPH 2008 and ACM Transaction of Graphics.
//
// Given two sets of points in 3-space, P and Q, the algorithm applies RANSAC
// in roughly O(n^2) time instead of O(n^3) for standard RANSAC, using an
// efficient method based on invariants, to find the set of all 4-points in Q
// that can be matched by rigid transformation to a given set of 4-points in P
// called a base. This avoids the need to examine all sets of 3-points in Q
// against any base of 3-points in P as in standard RANSAC.
// The algorithm can use colors and normals to speed-up the matching
// and to improve the quality. It can be easily extended to affine/similarity
// transformation but then the speed-up is smaller because of the large number
// of congruent sets. The algorithm can also limit the range of transformations
// when the application knows something on the initial pose but this is not
// necessary in general (though can speed the runtime significantly).

// Home page of the 4PCS project (containing the paper, presentations and a
// demo): http://graphics.stanford.edu/~niloy/research/fpcs/fpcs_sig_08.html
// Use google search on "4-points congruent sets" to see many related papers
// and applications.

#include "super4pcs/algorithms/match4pcsBase.h"
#include "super4pcs/shared4pcs.h"
#include "super4pcs/sampling.h"
#include "super4pcs/accelerators/kdtree.h"

#include <vector>
#include <atomic>

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

const double pi = std::acos(-1);

// Compute the closest points between two 3D line segments and obtain the two
// invariants corresponding to the closet points. This is the "intersection"
// point that determines the invariants. Since the 4 points are not exactly
// planar, we use the center of the line segment connecting the two closest
// points as the "intersection".
template < typename VectorType, typename Scalar>
static Scalar
distSegmentToSegment(const VectorType& p1, const VectorType& p2,
                     const VectorType& q1, const VectorType& q2,
                     Scalar& invariant1, Scalar& invariant2) {

  static const Scalar kSmallNumber = 0.0001;
  VectorType u = p2 - p1;
  VectorType v = q2 - q1;
  VectorType w = p1 - q1;
  Scalar a = u.dot(u);
  Scalar b = u.dot(v);
  Scalar c = v.dot(v);
  Scalar d = u.dot(w);
  Scalar e = v.dot(w);
  Scalar f = a * c - b * b;
  // s1,s2 and t1,t2 are the parametric representation of the intersection.
  // they will be the invariants at the end of this simple computation.
  Scalar s1 = 0.0;
  Scalar s2 = f;
  Scalar t1 = 0.0;
  Scalar t2 = f;

  if (f < kSmallNumber) {
    s1 = 0.0;
    s2 = 1.0;
    t1 = e;
    t2 = c;
  } else {
    s1 = (b * e - c * d);
    t1 = (a * e - b * d);
    if (s1 < 0.0) {
      s1 = 0.0;
      t1 = e;
      t2 = c;
    } else if (s1 > s2) {
      s1 = s2;
      t1 = e + b;
      t2 = c;
    }
  }

  if (t1 < 0.0) {
    t1 = 0.0;
    if (-d < 0.0)
      s1 = 0.0;
    else if (-d > a)
      s1 = s2;
    else {
      s1 = -d;
      s2 = a;
    }
  } else if (t1 > t2) {
    t1 = t2;
    if ((-d + b) < 0.0)
      s1 = 0;
    else if ((-d + b) > a)
      s1 = s2;
    else {
      s1 = (-d + b);
      s2 = a;
    }
  }
  invariant1 = (std::abs(s1) < kSmallNumber ? 0.0 : s1 / s2);
  invariant2 = (std::abs(t1) < kSmallNumber ? 0.0 : t1 / t2);

  return ( w + (invariant1 * u) - (invariant2 * v)).norm();
}

namespace GlobalRegistration{

Match4PCSBase::Match4PCSBase(  const Match4PCSOptions& options
                             , const Utils::Logger& logger
#ifdef SUPER4PCS_USE_OPENMP
                             , const int omp_nthread_congruent
#endif
                               )
  :number_of_trials_(0)
  , max_base_diameter_(-1)
  , P_mean_distance_(1.0)
  , best_LCP_(0.0)
  , options_(options)
  , randomGenerator_(options.randomSeed)
  , logger_(logger)
#ifdef SUPER4PCS_USE_OPENMP
  , omp_nthread_congruent_(omp_nthread_congruent)
#endif
{
  base_3D_.resize(4);
}

Match4PCSBase::~Match4PCSBase(){}

Match4PCSBase::Scalar
Match4PCSBase::MeanDistance() {
  const Scalar kDiameterFraction = 0.2;
  using RangeQuery = GlobalRegistration::KdTree<Scalar>::RangeQuery<>;

  int number_of_samples = 0;
  Scalar distance = 0.0;

  for (size_t i = 0; i < sampled_P_3D_.size(); ++i) {

    RangeQuery query;
    query.sqdist = P_diameter_ * kDiameterFraction;
    query.queryPoint = sampled_P_3D_[i].pos().cast<Scalar>();

    GlobalRegistration::KdTree<Scalar>::Index resId =
        kd_tree_.doQueryRestrictedClosestIndex(query , i);

    if (resId != GlobalRegistration::KdTree<Scalar>::invalidIndex()) {
      distance += (sampled_P_3D_[i].pos() - sampled_P_3D_[resId].pos()).norm();
      number_of_samples++;
    }
  }

  return distance / number_of_samples;
}

bool Match4PCSBase::SelectRandomTriangle(int &base1, int &base2, int &base3) {
      int number_of_points = sampled_P_3D_.size();
      base1 = base2 = base3 = -1;

      // Pick the first point at random.
      int first_point = randomGenerator_() % number_of_points;

      const Scalar sq_max_base_diameter_ = max_base_diameter_*max_base_diameter_;

      // Try fixed number of times retaining the best other two.
      Scalar best_wide = 0.0;
      for (int i = 0; i < kNumberOfDiameterTrials; ++i) {
        // Pick and compute
        const int second_point = randomGenerator_() % number_of_points;
        const int third_point = randomGenerator_() % number_of_points;
        const VectorType u =
                sampled_P_3D_[second_point].pos() -
                sampled_P_3D_[first_point].pos();
        const VectorType w =
                sampled_P_3D_[third_point].pos() -
                sampled_P_3D_[first_point].pos();
        // We try to have wide triangles but still not too large.
        Scalar how_wide = (u.cross(w)).norm();
        if (how_wide > best_wide &&
                u.squaredNorm() < sq_max_base_diameter_ &&
                w.squaredNorm() < sq_max_base_diameter_) {
          best_wide = how_wide;
          base1 = first_point;
          base2 = second_point;
          base3 = third_point;
        }
      }
      return base1 != -1 && base2 != -1 && base3 != -1;
}

// Try the current base in P and obtain the best pairing, i.e. the one that
// gives the smaller distance between the two closest points. The invariants
// corresponding the the base pairing are computed.
bool Match4PCSBase::TryQuadrilateral(Scalar &invariant1, Scalar &invariant2,
                                     int& id1, int& id2, int& id3, int& id4) {

  Scalar min_distance = std::numeric_limits<Scalar>::max();
  int best1, best2, best3, best4;
  best1 = best2 = best3 = best4 = -1;
  for (int i = 0; i < 4; ++i) {
    for (int j = 0; j < 4; ++j) {
      if (i == j) continue;
      int k = 0;
      while (k == i || k == j) k++;
      int l = 0;
      while (l == i || l == j || l == k) l++;
      double local_invariant1;
      double local_invariant2;
      // Compute the closest points on both segments, the corresponding
      // invariants and the distance between the closest points.
      Scalar segment_distance = distSegmentToSegment(
                  base_3D_[i].pos(), base_3D_[j].pos(),
                  base_3D_[k].pos(), base_3D_[l].pos(),
                  local_invariant1, local_invariant2);
      // Retail the smallest distance and the best order so far.
      if (segment_distance < min_distance) {
        min_distance = segment_distance;
        best1 = i;
        best2 = j;
        best3 = k;
        best4 = l;
        invariant1 = local_invariant1;
        invariant2 = local_invariant2;
      }
    }
  }

  if(best1 < 0 || best2 < 0 || best3 < 0 || best4 < 0 ) return false;

  std::vector<Point3D> tmp = base_3D_;
  base_3D_[0] = tmp[best1];
  base_3D_[1] = tmp[best2];
  base_3D_[2] = tmp[best3];
  base_3D_[3] = tmp[best4];

  std::array<int, 4> tmpId = {id1, id2, id3, id4};
  id1 = tmpId[best1];
  id2 = tmpId[best2];
  id3 = tmpId[best3];
  id4 = tmpId[best4];

  return true;
}

// Selects a good base from P and computes its invariants. Returns false if
// a good planar base cannot can be found.
bool Match4PCSBase::SelectQuadrilateral(Scalar& invariant1, Scalar& invariant2,
                                        int& base1, int& base2, int& base3,
                                        int& base4) {

  const Scalar kBaseTooSmall (0.2);
  int current_trial = 0;

  // Try fix number of times.
  while (current_trial < kNumberOfDiameterTrials) {
    // Select a triangle if possible. otherwise fail.
    if (!SelectRandomTriangle(base1, base2, base3)){
      return false;
    }

    base_3D_[0] = sampled_P_3D_[base1];
    base_3D_[1] = sampled_P_3D_[base2];
    base_3D_[2] = sampled_P_3D_[base3];

    // The 4th point will be a one that is close to be planar to the other 3
    // while still not too close to them.
    const double x1 = base_3D_[0].x();
    const double y1 = base_3D_[0].y();
    const double z1 = base_3D_[0].z();
    const double x2 = base_3D_[1].x();
    const double y2 = base_3D_[1].y();
    const double z2 = base_3D_[1].z();
    const double x3 = base_3D_[2].x();
    const double y3 = base_3D_[2].y();
    const double z3 = base_3D_[2].z();

    // Fit a plan.
    Scalar denom = (-x3 * y2 * z1 + x2 * y3 * z1 + x3 * y1 * z2 - x1 * y3 * z2 -
                    x2 * y1 * z3 + x1 * y2 * z3);

    if (denom != 0) {
      Scalar A =
          (-y2 * z1 + y3 * z1 + y1 * z2 - y3 * z2 - y1 * z3 + y2 * z3) / denom;
      Scalar B =
          (x2 * z1 - x3 * z1 - x1 * z2 + x3 * z2 + x1 * z3 - x2 * z3) / denom;
      Scalar C =
          (-x2 * y1 + x3 * y1 + x1 * y2 - x3 * y2 - x1 * y3 + x2 * y3) / denom;
      base4 = -1;
      Scalar best_distance = std::numeric_limits<Scalar>::max();
      // Go over all points in P.
      const Scalar too_small = std::pow(max_base_diameter_ * kBaseTooSmall, 2);
      for (unsigned int i = 0; i < sampled_P_3D_.size(); ++i) {
        if ((sampled_P_3D_[i].pos()- sampled_P_3D_[base1].pos()).squaredNorm() >= too_small &&
            (sampled_P_3D_[i].pos()- sampled_P_3D_[base2].pos()).squaredNorm() >= too_small &&
            (sampled_P_3D_[i].pos()- sampled_P_3D_[base3].pos()).squaredNorm() >= too_small) {
          // Not too close to any of the first 3.
          const Scalar distance =
              std::abs(A * sampled_P_3D_[i].x() + B * sampled_P_3D_[i].y() +
                   C * sampled_P_3D_[i].z() - 1.0);
          // Search for the most planar.
          if (distance < best_distance) {
            best_distance = distance;
            base4 = int(i);
          }
        }
      }
      // If we have a good one we can quit.
      if (base4 != -1) {
        base_3D_[3] = sampled_P_3D_[base4];
        if(TryQuadrilateral(invariant1, invariant2, base1, base2, base3, base4))
            return true;
      }
    }
    current_trial++;
  }

  // We failed to find good enough base..
  return false;
}

void Match4PCSBase::initKdTree(){
  size_t number_of_points = sampled_P_3D_.size();

  // Build the kdtree.
  kd_tree_ = GlobalRegistration::KdTree<Scalar>(number_of_points);

  for (size_t i = 0; i < number_of_points; ++i) {
    kd_tree_.add(sampled_P_3D_[i].pos());
  }
  kd_tree_.finalize();
}

bool Match4PCSBase::ComputeRigidTransformation(
        const std::array<Point3D, 4>& ref,
        const std::array<Point3D, 4>& candidate,
        const Eigen::Matrix<Scalar, 3, 1>& centroid1,
        Eigen::Matrix<Scalar, 3, 1> centroid2,
        Scalar max_angle,
        Eigen::Ref<MatrixType> transform,
        Scalar& rms_,
        bool computeScale ) const {

  rms_ = kLargeNumber;

  Scalar kSmallNumber = 1e-6;

  // We only use the first 3 pairs. This simplifies the process considerably
  // because it is the planar case.

  const VectorType& p0 = ref[0].pos();
  const VectorType& p1 = ref[1].pos();
  const VectorType& p2 = ref[2].pos();
        VectorType  q0 = candidate[0].pos();
        VectorType  q1 = candidate[1].pos();
        VectorType  q2 = candidate[2].pos();

  Scalar scaleEst (1.);

  // Compute scale factor if needed
  if (computeScale){
      const VectorType& p3 = ref[3].pos();
      const VectorType& q3 = candidate[3].pos();

      const Scalar ratio1 = (p1 - p0).norm() / (q1 - q0).norm();
      const Scalar ratio2 = (p3 - p2).norm() / (q3 - q2).norm();

      const Scalar ratioDev  = std::abs(ratio1/ratio2 - Scalar(1.));  // deviation between the two
      const Scalar ratioMean = (ratio1+ratio2)/Scalar(2.);            // mean of the two

      if ( ratioDev > Scalar(0.1) )
          return kLargeNumber;

      //Log<LogLevel::Verbose>( ratio1, " ", ratio2, " ", ratioDev, " ", ratioMean);
      scaleEst = ratioMean;

      // apply scale factor to q
      q0 = q0*scaleEst;
      q1 = q1*scaleEst;
      q2 = q2*scaleEst;
      centroid2 *= scaleEst;
  }

  VectorType vector_p1 = p1 - p0;
  if (vector_p1.squaredNorm() == 0) return kLargeNumber;
  vector_p1.normalize();
  VectorType vector_p2 = (p2 - p0) - ((p2 - p0).dot(vector_p1)) * vector_p1;
  if (vector_p2.squaredNorm() == 0) return kLargeNumber;
  vector_p2.normalize();
  VectorType vector_p3 = vector_p1.cross(vector_p2);

  VectorType vector_q1 = q1 - q0;
  if (vector_q1.squaredNorm() == 0) return kLargeNumber;
  vector_q1.normalize();
  VectorType vector_q2 = (q2 - q0) - ((q2 - q0).dot(vector_q1)) * vector_q1;
  if (vector_q2.squaredNorm() == 0) return kLargeNumber;
  vector_q2.normalize();
  VectorType vector_q3 = vector_q1.cross(vector_q2);

  //cv::Mat rotation = cv::Mat::eye(3, 3, CV_64F);
  Eigen::Matrix<Scalar, 3, 3> rotation = Eigen::Matrix<Scalar, 3, 3>::Identity();

  Eigen::Matrix<Scalar, 3, 3> rotate_p;
  rotate_p.row(0) = vector_p1;
  rotate_p.row(1) = vector_p2;
  rotate_p.row(2) = vector_p3;

  Eigen::Matrix<Scalar, 3, 3> rotate_q;
  rotate_q.row(0) = vector_q1;
  rotate_q.row(1) = vector_q2;
  rotate_q.row(2) = vector_q3;

  rotation = rotate_p.transpose() * rotate_q;

  // Discard singular solutions. The rotation should be orthogonal.
  if (((rotation * rotation).diagonal().array() - Scalar(1) > kSmallNumber).any())
      return false;

  //FIXME
  if (max_angle >= 0) {
      // Discard too large solutions (todo: lazy evaluation during boolean computation
      if (! (
                  std::abs(std::atan2(rotation(2, 1), rotation(2, 2)))
                  <= max_angle &&

                  std::abs(std::atan2(-rotation(2, 0),
                                      std::sqrt(std::pow(rotation(2, 1),2) +
                                                std::pow(rotation(2, 2),2))))
                  <= max_angle &&

                  std::abs(atan2(rotation(1, 0), rotation(0, 0)))
                  <= max_angle
             ))
          return false;
  }

  //FIXME
  // Compute rms and return it.
  rms_ = Scalar(0.0);
  {
      VectorType first, transformed;

      //cv::Mat first(3, 1, CV_64F), transformed;
      for (int i = 0; i < 3; ++i) {
          first = scaleEst*candidate[i].pos() - centroid2;
          transformed = rotation * first;
          rms_ += (transformed - ref[i].pos() + centroid1).norm();
      }
  }

  rms_ /= Scalar(ref.size());

  Eigen::Transform<Scalar, 3, Eigen::Affine> etrans (Eigen::Transform<Scalar, 3, Eigen::Affine>::Identity());
  transform = etrans
      .scale(scaleEst)
      .translate(centroid1)
      .rotate(rotation)
      .translate(-centroid2)
      .matrix();

  return true;
}

// Verify a given transformation by computing the number of points in P at
// distance at most (normalized) delta from some point in Q. In the paper
// we describe randomized verification. We apply deterministic one here with
// early termination. It was found to be fast in practice.
Match4PCSBase::Scalar
Match4PCSBase::Verify(const Eigen::Ref<const MatrixType> &mat) const {
  using RangeQuery = GlobalRegistration::KdTree<Scalar>::RangeQuery<>;

#ifdef TEST_GLOBAL_TIMINGS
    Timer t_verify (true);
#endif

  // We allow factor 2 scaling in the normalization.
  const Scalar epsilon = options_.delta;
  std::atomic_uint good_points(0);
  const size_t number_of_points = sampled_Q_3D_.size();
  const size_t terminate_value = best_LCP_ * number_of_points;

  const Scalar sq_eps = epsilon*epsilon;

  for (size_t i = 0; i < number_of_points; ++i) {

    // Use the kdtree to get the nearest neighbor
#ifdef TEST_GLOBAL_TIMINGS
    Timer t (true);
#endif

    RangeQuery query;
    query.queryPoint = (mat * sampled_Q_3D_[i].pos().homogeneous()).head<3>();
    query.sqdist     = sq_eps;

    GlobalRegistration::KdTree<Scalar>::Index resId =
    kd_tree_.doQueryRestrictedClosestIndex( query );

#ifdef TEST_GLOBAL_TIMINGS
    kdTreeTime += Scalar(t.elapsed().count()) / Scalar(CLOCKS_PER_SEC);
#endif

    if ( resId != GlobalRegistration::KdTree<Scalar>::invalidIndex() ) {
//      Point3D& q = sampled_P_3D_[near_neighbor_index[0]];
//      bool rgb_good =
//          (p.rgb()[0] >= 0 && q.rgb()[0] >= 0)
//              ? cv::norm(p.rgb() - q.rgb()) < options_.max_color_distance
//              : true;
//      bool norm_good = norm(p.normal()) > 0 && norm(q.normal()) > 0
//                           ? fabs(p.normal().ddot(q.normal())) >= cos_dist
//                           : true;
//      if (rgb_good && norm_good) {
        good_points++;
//      }
    }

    // We can terminate if there is no longer chance to get better than the
    // current best LCP.
    if (number_of_points - i + good_points < terminate_value) {
      break;
    }
  }

#ifdef TEST_GLOBAL_TIMINGS
  verifyTime += Scalar(t_verify.elapsed().count()) / Scalar(CLOCKS_PER_SEC);
#endif
  return Scalar(good_points) / Scalar(number_of_points);
}

} // namespace Super4PCS


#include <gtest/gtest.h>
#include <carl/core/Variable.h>
#include <carl/core/Monomial.h>
#include <carl/core/MonomialPool.h>
#include <list>
#include <boost/variant.hpp>

#include "../Common.h"

TEST(Monomial, Constructor)
{
	auto x = carl::freshRealVariable("x");

	auto m = carl::createMonomial(x, 3);
	EXPECT_TRUE(m->exponents().size() == 1);
	EXPECT_TRUE(m->exponents().front().first == x);
	EXPECT_TRUE(m->exponents().front().second == 3);
}

TEST(Monomial, tdeg)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");

	carl::Monomial::Arg m1 = x*x*x;
	EXPECT_TRUE(m1->tdeg() == 3);
	carl::Monomial::Arg m2 = x*x*y;
	EXPECT_TRUE(m2->tdeg() == 3);
	carl::Monomial::Arg m3 = x*y*y*y;
	EXPECT_TRUE(m3->tdeg() == 4);
}

TEST(Monomial, degreeCategories)
{
	auto x = carl::freshRealVariable("x");

	carl::Monomial::Arg m1 = carl::createMonomial(x, 1);
	EXPECT_FALSE(m1->isConstant());
	EXPECT_TRUE(m1->isLinear());
	EXPECT_TRUE(m1->isAtMostLinear());
	EXPECT_FALSE(m1->isSquare());
	carl::Monomial::Arg m2 = carl::createMonomial(x, 2);
	EXPECT_FALSE(m2->isConstant());
	EXPECT_FALSE(m2->isLinear());
	EXPECT_FALSE(m2->isAtMostLinear());
	EXPECT_TRUE(m2->isSquare());
}

TEST(Monomial, hasNoOtherVariable)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");
	carl::Monomial::Arg m1 = carl::createMonomial(x, 1);
	carl::Monomial::Arg m2 = x*x;
	carl::Monomial::Arg m3 = x*x*y;
	carl::Monomial::Arg m4 = carl::createMonomial(y, 1);
	carl::Monomial::Arg m5 = y*y;
	carl::Monomial::Arg m6 = y*y*x;

	EXPECT_TRUE(m1->hasNoOtherVariable(x));
	EXPECT_FALSE(m1->hasNoOtherVariable(y));
	EXPECT_TRUE(m2->hasNoOtherVariable(x));
	EXPECT_FALSE(m2->hasNoOtherVariable(y));
	EXPECT_FALSE(m3->hasNoOtherVariable(x));
	EXPECT_FALSE(m3->hasNoOtherVariable(y));
	EXPECT_FALSE(m4->hasNoOtherVariable(x));
	EXPECT_TRUE(m4->hasNoOtherVariable(y));
	EXPECT_FALSE(m5->hasNoOtherVariable(x));
	EXPECT_TRUE(m5->hasNoOtherVariable(y));
	EXPECT_FALSE(m6->hasNoOtherVariable(x));
	EXPECT_FALSE(m6->hasNoOtherVariable(y));
}

TEST(Monomial, Operators)
{
	auto v0 = carl::freshRealVariable("a");
	auto v1 = carl::freshRealVariable("b");
	auto v2 = carl::freshRealVariable("c");

	carl::Monomial::Arg m0 = carl::createMonomial(v0, 1);
	m0 = m0 * v1;
	EXPECT_EQ((unsigned)1,m0->exponentOfVariable(v1));
	m0 = m0 * v1;
	EXPECT_EQ((unsigned)2,m0->exponentOfVariable(v1));
	EXPECT_EQ((unsigned)3,m0->tdeg());
	EXPECT_EQ((unsigned)0,m0->exponentOfVariable(v2));
	m0 = m0 * v2;
	EXPECT_EQ((unsigned)4,m0->tdeg());
	EXPECT_EQ((unsigned)3,m0->nrVariables());

	carl::Monomial::Arg m3 = carl::createMonomial(v1, 1);
	carl::Monomial::Arg m2 = carl::createMonomial(v1, 1);
	m2 = m2 * v1;
	m3 = m3 * v1;
	EXPECT_EQ(m2, m3);
}

TEST(Monomial, VariableMultiplication)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");
	//EXPECT_EQ(carl::createMonomial(std::initializer_list<std::pair<Variable, exponent>>({std::make_pair(x, 1)})), x);
	EXPECT_EQ(carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 1), std::make_pair(y, 1)})), x * y);
	//EXPECT_EQ(carl::createMonomial(std::initializer_list<std::pair<Variable, exponent>>({std::make_pair(x, 2), std::make_pair(y, 1)})), x * x * y);
	//EXPECT_EQ(carl::createMonomial(std::initializer_list<std::pair<Variable, exponent>>({std::make_pair(x, 1), std::make_pair(y, 2)})), y * x * y);
	//EXPECT_EQ(carl::createMonomial(std::initializer_list<std::pair<Variable, exponent>>({std::make_pair(x, 3)})), x * x * x);
}

TEST(Monomial, MonomialMultiplication)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");
	EXPECT_EQ(
		carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 2), std::make_pair(y, 3)})),
		carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 1), std::make_pair(y, 2)})) * carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 1), std::make_pair(y, 1)}))
	);
	EXPECT_EQ(
		carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 2), std::make_pair(y, 3)})),
		carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 2)})) * carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(y, 3)}))
	);
	EXPECT_EQ(
		carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 5), std::make_pair(y, 3)})),
		carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 2)})) * carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 3), std::make_pair(y, 3)}))
	);
}

TEST(Monomial, derivative)
{
	auto v0 = carl::freshRealVariable("x");
	auto v1 = carl::freshRealVariable("y");
	auto m0 = v0 * v1;
	auto d1 = m0->derivative(v0);
	EXPECT_EQ(1, d1.first);
	EXPECT_EQ(v1, d1.second);
}

TEST(Monomial, division)
{
	auto v0 = carl::freshRealVariable("x");
	auto v1 = carl::freshRealVariable("y");
	auto v2 = carl::freshRealVariable("z");

	carl::Monomial::Arg m0 = v0 * v0 * v1 * v1 * v2;
	carl::Monomial::Arg m1 = v0 * v0 * v0;
	carl::Monomial::Arg m2 = v0 * v0 * v1 * v2;
	carl::Monomial::Arg m0x = v0 * v0 * v1 * v2;
	carl::Monomial::Arg m0y = v0 * v0 * v1 * v1;
	carl::Monomial::Arg m0z = v0 * v1;
	carl::Monomial::Arg tmp;
	carl::Monomial::Arg one;
	EXPECT_TRUE(m0->divide(one, tmp));
	EXPECT_EQ(m0, tmp);
	EXPECT_TRUE(m1->divide(one, tmp));
	EXPECT_EQ(m1, tmp);
	EXPECT_FALSE(m0->divide(m1, tmp));
	EXPECT_FALSE(m1->divide(m0, tmp));
	EXPECT_TRUE(m0->divide(v1, tmp));
	EXPECT_EQ(m0x, tmp);
	EXPECT_TRUE(m0->divide(v2, tmp));
	EXPECT_EQ(m0y, tmp);
	EXPECT_TRUE(m0->divide(m2, tmp));
	EXPECT_EQ(v1, tmp);
}

TEST(Monomial, divisible)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");
	auto m1 = carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(y, 2), std::make_pair(x, 2)}));
	auto m2 = carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(x, 1), std::make_pair(y, 1)}));
	auto m3 = carl::createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(y, 1), std::make_pair(x, 1)}));
//	std::cout << m1 << " divisible by " << m2 << std::endl;
//	std::cout << m2 << " == " << m3 << std::endl;
	EXPECT_TRUE(m2==m3);
	EXPECT_TRUE(m1->divisible(m2));

	{
		auto m1 = createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(y, 2)}));
		auto m2 = createMonomial(std::initializer_list<std::pair<carl::Variable, carl::exponent>>({std::make_pair(y, 2), std::make_pair(x, 2)}));
		EXPECT_TRUE(m2->divisible(m1));
	}
}

TEST(Monomial, Comparison)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");
	auto z = carl::freshRealVariable("z");

	ComparisonList<carl::Monomial::Arg> monomials;
	monomials.push_back(x * x * x);
	monomials.push_back(x * x * y);
	monomials.push_back(x * y * y);
	monomials.push_back(x * y * z);
	monomials.push_back(y * y * y);
	monomials.push_back(x * x * z * z);
	monomials.push_back(x * y * y * z);

	expectRightOrder(monomials);
}

TEST(Monomial, OtherComparison)
{
	ComparisonList<carl::Variable,carl::Monomial::Arg> list;

	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");

	list.push_back(x);
	list.push_back(y);
	list.push_back(x * x);
	list.push_back(x * y);
	list.push_back(y * y);
	list.push_back(x * x * x);
	list.push_back(x * x * y);
	list.push_back(x * x * x * x);

	expectRightOrder(list);
}

TEST(Monomial, sqrt)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");
	carl::Monomial::Arg m1 = x*x*y*y*y*y;
	carl::Monomial::Arg m2 = x*y*y;
	EXPECT_EQ(m2, m1->sqrt());
}

TEST(Monomial, pow)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");
	carl::Monomial::Arg one;
	carl::Monomial::Arg m1 = x*y*y;
	carl::Monomial::Arg m2 = x*x*y*y*y*y;
	EXPECT_EQ(one, m1->pow(0));
	EXPECT_EQ(m1, m1->pow(1));
	EXPECT_EQ(m2, m1->pow(2));
}

TEST(Monomial, CalcLCM)
{
	auto x = carl::freshRealVariable("x");
	auto y = carl::freshRealVariable("y");
	carl::Monomial::Arg m1 = y*y;
	carl::Monomial::Arg m2 = x*x*y;
	EXPECT_EQ(y, carl::Monomial::calcLcmAndDivideBy(m1, m2));
}