#include "CloneTreeCreator.hpp"

#include <cstdio>

namespace btInverseDynamics
{
#define CHECK_NULLPTR()                                  \
	do                                                   \
	{                                                    \
		if (m_reference == 0x0)                          \
		{                                                \
			bt_id_error_message("m_reference == 0x0\n"); \
			return -1;                                   \
		}                                                \
	} while (0)

#define TRY(x)                                             \
	do                                                     \
	{                                                      \
		if (x == -1)                                       \
		{                                                  \
			bt_id_error_message("error calling " #x "\n"); \
			return -1;                                     \
		}                                                  \
	} while (0)
CloneTreeCreator::CloneTreeCreator(const MultiBodyTree* reference)
{
	m_reference = reference;
}

CloneTreeCreator::~CloneTreeCreator() {}

int CloneTreeCreator::getNumBodies(int* num_bodies) const
{
	CHECK_NULLPTR();
	*num_bodies = m_reference->numBodies();
	return 0;
}

int CloneTreeCreator::getBody(const int body_index, int* parent_index, JointType* joint_type,
							  vec3* parent_r_parent_body_ref, mat33* body_T_parent_ref,
							  vec3* body_axis_of_motion, idScalar* mass, vec3* body_r_body_com,
							  mat33* body_I_body, int* user_int, void** user_ptr) const
{
	CHECK_NULLPTR();
	TRY(m_reference->getParentIndex(body_index, parent_index));
	TRY(m_reference->getJointType(body_index, joint_type));
	TRY(m_reference->getParentRParentBodyRef(body_index, parent_r_parent_body_ref));
	TRY(m_reference->getBodyTParentRef(body_index, body_T_parent_ref));
	TRY(m_reference->getBodyAxisOfMotion(body_index, body_axis_of_motion));
	TRY(m_reference->getBodyMass(body_index, mass));
	TRY(m_reference->getBodyFirstMassMoment(body_index, body_r_body_com));
	TRY(m_reference->getBodySecondMassMoment(body_index, body_I_body));
	TRY(m_reference->getUserInt(body_index, user_int));
	TRY(m_reference->getUserPtr(body_index, user_ptr));

	return 0;
}
}  // namespace btInverseDynamics