/// create a bullet btMultiBody model of a tree structured multibody system,
/// convert that model to a MultiBodyTree model.
/// Then - run inverse dynamics on random input data (q, u, dot_u) to get forces
///      - run forward dynamics on (q,u, forces) to get accelerations
///      - compare input accelerations to inverse dynamics to output from forward dynamics

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <functional>
#include <string>

#include <btBulletDynamicsCommon.h>
#include <btMultiBodyTreeCreator.hpp>
#include <BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h>
#include <BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h>
#include <BulletDynamics/Featherstone/btMultiBodyPoint2Point.h>
#include <BulletDynamics/Featherstone/btMultiBodyLinkCollider.h>
#include <gtest/gtest.h>
#include "../../examples/CommonInterfaces/CommonGUIHelperInterface.h"
#include "../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../../examples/Importers/ImportURDFDemo/URDF2Bullet.h"
#include "../../examples/Utils/b3ResourcePath.h"
#include <invdyn_bullet_comparison.hpp>
#include <btMultiBodyFromURDF.hpp>
#include <MultiBodyTreeCreator.hpp>
#include <MultiBodyTreeDebugGraph.hpp>
#include "Bullet3Common/b3CommandLineArgs.h"
#include "Bullet3Common/b3Random.h"

using namespace btInverseDynamics;

bool FLAGS_verbose = false;

static btVector3 gravity(0, 0, -10);
static const bool kBaseFixed = false;
static const char kUrdfFile[] = "r2d2.urdf";

/// this test loads the a urdf model with fixed, floating, prismatic and rotational joints,
/// converts in to an inverse dynamics model and compares forward to inverse dynamics for
/// random input
TEST(InvDynCompare, bulletUrdfR2D2)
{
	MyBtMultiBodyFromURDF mb_load(gravity, kBaseFixed);

	char relativeFileName[1024];

	ASSERT_TRUE(b3ResourcePath::findResourcePath(kUrdfFile, relativeFileName, 1024,0));

	mb_load.setFileName(relativeFileName);
	mb_load.init();

	btMultiBodyTreeCreator id_creator;
	btMultiBody *btmb = mb_load.getBtMultiBody();
	ASSERT_EQ(id_creator.createFromBtMultiBody(btmb), 0);

	MultiBodyTree *id_tree = CreateMultiBodyTree(id_creator);
	ASSERT_EQ(0x0 != id_tree, true);

	vecx q(id_tree->numDoFs());
	vecx u(id_tree->numDoFs());
	vecx dot_u(id_tree->numDoFs());
	vecx joint_forces(id_tree->numDoFs());

	const int kNLoops = 10;
	double max_pos_error = 0;
	double max_acc_error = 0;

	b3Srand(0);

	for (int loop = 0; loop < kNLoops; loop++)
	{
		for (int i = 0; i < q.size(); i++)
		{
			q(i) = b3RandRange(-B3_PI, B3_PI);
			u(i) = b3RandRange(-B3_PI, B3_PI);
			dot_u(i) = b3RandRange(-B3_PI, B3_PI);
		}

		double pos_error;
		double acc_error;
		btmb->clearForcesAndTorques();
		id_tree->clearAllUserForcesAndMoments();
		// call inverse dynamics once, to get global position & velocity of root body
		// (fixed, so q, u, dot_u arbitrary)
		EXPECT_EQ(id_tree->calculateInverseDynamics(q, u, dot_u, &joint_forces), 0);

		EXPECT_EQ(compareInverseAndForwardDynamics(q, u, dot_u, gravity, FLAGS_verbose, btmb, id_tree,
												   &pos_error, &acc_error),
				  0);

		if (pos_error > max_pos_error)
		{
			max_pos_error = pos_error;
		}
		if (acc_error > max_acc_error)
		{
			max_acc_error = acc_error;
		}
	}

	if (FLAGS_verbose)
	{
		printf("max_pos_error= %e\n", max_pos_error);
		printf("max_acc_error= %e\n", max_acc_error);
	}

	EXPECT_LT(max_pos_error, std::numeric_limits<idScalar>::epsilon() * 1e4);
	EXPECT_LT(max_acc_error, std::numeric_limits<idScalar>::epsilon() * 1e5);
}

int main(int argc, char **argv)
{
	b3CommandLineArgs myArgs(argc, argv);
	FLAGS_verbose = myArgs.CheckCmdLineFlag("verbose");
	::testing::InitGoogleTest(&argc, argv);
	return RUN_ALL_TESTS();
}