#ifdef WITH_TESTS

#include "../../Tests.h"

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

TEST(TurnDetection, rotationMatrix) {

	Eigen::Vector3f dst; dst << 0, 0, 1;
	Eigen::Vector3f src; src << 1, 1, 0; src.normalize();

	// get a matrix that rotates "src" into "dst"
	Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(src, dst);

	Eigen::Vector3f 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);

}

TEST(TurnDetection, gyroRotate) {


	Eigen::Vector3f zAxis; zAxis << 0, 0, 1;
	Eigen::Vector3f acc; acc << 0, 7.0, 7.0;

	Eigen::Matrix3f rot = TurnDetection::getRotationMatrix(acc, zAxis);

	Eigen::Vector3f gyro; gyro << 0, 60, 60;

	Eigen::Vector3f gyro2; gyro2 << 0, 0, 84;

	Eigen::Vector3f gyro3 = rot * gyro;

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

}


TEST(TurnDetection, xx) {

	Eigen::Vector3f dst; dst << 0, 0, 1;
	Eigen::Vector3f src; src << 0.0, 2.9, -10.0; src.normalize();		// sample accelerometer readings

	Eigen::Matrix3f rot = TurnDetection::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;

	Eigen::Vector3f 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);

	Eigen::Vector3f gyro; gyro << 0, 10, 30;

	Eigen::Vector3f gyro2 = rot * gyro;
	int i = 0; (void) i;

}


#endif