#ifdef WITH_TESTS

#ifdef TODO
#include "../../Tests.h"

#include "../../../sensors/imu/TurnDetection.h"

TEST(TurnDetection, rotationMatrix) {

	Vector3 dst(0, 0, 1);
	Vector3 src(1, 1, 0); src = src.normalized();

	// get a matrix that rotates "src" into "dst"
	Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);

	Vector3 res = rot * src;

	ASSERT_NEAR(dst.x, res.x, 0.01);
	ASSERT_NEAR(dst.y, res.y, 0.01);
	ASSERT_NEAR(dst.z, res.z, 0.01);

}

TEST(TurnDetection, gyroRotate) {


	Vector3 zAxis(0, 0, 1);
	Vector3 acc(0, 7.0, 7.0);

	Matrix3 rot = PoseDetection::getRotationMatrix(acc, zAxis);

	Vector3 gyro(0, 60, 60);

	Vector3 gyro2(0, 0, 84);

	Vector3 gyro3 = rot * gyro;

	ASSERT_NEAR(0, (gyro2-gyro3).norm(), 1.0);

}


TEST(TurnDetection, xx) {

	Vector3 dst(0, 0, 1);
	Vector3 src(0.0, 2.9, -10.0); src = src.normalized();		// sample accelerometer readings

	Matrix3 rot = PoseDetection::getRotationMatrix(src, dst);

//	Eigen::Vector3f x; x << 1, 0, 0;
//	Eigen::Vector3f z = src.normalized();
//	Eigen::Vector3f y = z.cross(x);

//	Eigen::Matrix3f rot;
//	rot.row(0) = x;
//	rot.row(1) = y;
//	rot.row(2) = z;

	Vector3 res = rot * src;
//	ASSERT_NEAR(dst(0), res(0), 0.01);
//	ASSERT_NEAR(dst(1), res(1), 0.01);
//	ASSERT_NEAR(dst(2), res(2), 0.01);

	Vector3 gyro(0, 10, 30);

	Vector3 gyro2 = rot * gyro;
	int i = 0; (void) i;

}

#endif
#endif