/**
 * @file testVelocityConstraint
 * @author Alex Cunningham
 */

#include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/dynamics/imuSystem.h>

using namespace gtsam;
using namespace imu;

const double tol=1e-5;

const Key x1 = 1, x2 = 2;
const double dt = 1.0;

PoseRTV origin,
				pose1(Point3(0.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0)),
				pose1a(Point3(0.5, 0.0, 0.0)),
				pose2(Point3(1.5, 0.0, 0.0), Rot3::identity(), Vector_(3, 1.0, 0.0, 0.0));

/* ************************************************************************* */
TEST( testVelocityConstraint, trapezoidal ) {
	// hard constraints don't need a noise model
	VelocityConstraint constraint(x1, x2, dynamics::TRAPEZOIDAL, dt);

	// verify error function
	EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, pose1), tol));
	EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
	EXPECT(assert_equal(delta(3, 0,-1.0), constraint.evaluateError(pose1, pose1), tol));
	EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
}

/* ************************************************************************* */
TEST( testEulerVelocityConstraint, euler_start ) {
	// hard constraints create their own noise model
	VelocityConstraint constraint(x1, x2, dynamics::EULER_START, dt);

	// verify error function
	EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1), tol));
	EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
	EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol));
	EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
}

/* ************************************************************************* */
TEST( testEulerVelocityConstraint, euler_end ) {
	// hard constraints create their own noise model
	VelocityConstraint constraint(x1, x2, dynamics::EULER_END, dt);

	// verify error function
	EXPECT(assert_equal(delta(3, 0,-0.5), constraint.evaluateError(origin, pose1), tol));
	EXPECT(assert_equal(zero(3), constraint.evaluateError(origin, origin), tol));
	EXPECT(assert_equal(zero(3), constraint.evaluateError(pose1, pose2), tol));
	EXPECT(assert_equal(delta(3, 0, 0.5), constraint.evaluateError(origin, pose1a), tol));
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */