#include "b3RobotSimulatorClientAPI_NoGUI.h"


#include "../SharedMemory/PhysicsClientC_API.h"
#include "b3RobotSimulatorClientAPI_InternalData.h"

#ifdef BT_ENABLE_ENET
#include "../SharedMemory/PhysicsClientUDP_C_API.h"
#endif  //PHYSICS_UDP

#ifdef BT_ENABLE_CLSOCKET
#include "../SharedMemory/PhysicsClientTCP_C_API.h"
#endif  //PHYSICS_TCP

#include "../SharedMemory/PhysicsDirectC_API.h"

#include "../SharedMemory/SharedMemoryPublic.h"
#include "Bullet3Common/b3Logging.h"

static void scalarToDouble3(btScalar a[3], double b[3]) 
{
	for (int i = 0; i < 3; i++) 
	{
		b[i] = a[i];
	}
}

static void scalarToDouble4(btScalar a[4], double b[4]) 
{
	for (int i = 0; i < 4; i++) 
	{
		b[i] = a[i];
	}
}


b3RobotSimulatorClientAPI_NoGUI::b3RobotSimulatorClientAPI_NoGUI()
{
	m_data = new b3RobotSimulatorClientAPI_InternalData();
}

b3RobotSimulatorClientAPI_NoGUI::~b3RobotSimulatorClientAPI_NoGUI()
{
	delete m_data;
}


bool b3RobotSimulatorClientAPI_NoGUI::connect(int mode, const std::string& hostName, int portOrKey)
{
	if (m_data->m_physicsClientHandle)
	{
		b3Warning("Already connected, disconnect first.");
		return false;
	}
	b3PhysicsClientHandle sm = 0;

	int udpPort = 1234;
	int tcpPort = 6667;
	int key = SHARED_MEMORY_KEY;
	bool connected = false;

	switch (mode)
	{
	
	case eCONNECT_DIRECT:
		{
			sm = b3ConnectPhysicsDirect();
			break;
		}
	case eCONNECT_SHARED_MEMORY:
		{
			if (portOrKey >= 0)
			{
				key = portOrKey;
			}
			sm = b3ConnectSharedMemory(key);
			break;
		}
	case eCONNECT_UDP:
		{
			if (portOrKey >= 0)
			{
				udpPort = portOrKey;
			}
#ifdef BT_ENABLE_ENET

			sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
#else
			b3Warning("UDP is not enabled in this build");
#endif  //BT_ENABLE_ENET

			break;
		}
	case eCONNECT_TCP:
		{
			if (portOrKey >= 0)
			{
				tcpPort = portOrKey;
			}
#ifdef BT_ENABLE_CLSOCKET

			sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
#else
			b3Warning("TCP is not enabled in this pybullet build");
#endif  //BT_ENABLE_CLSOCKET
			break;
		}

	default:
		{
			b3Warning("connectPhysicsServer unexpected argument");
		}
	};

	if (sm)
	{
		m_data->m_physicsClientHandle = sm;
		if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
		{
			disconnect();
			return false;
		}
		return true;
	}
	return false;
}

bool b3RobotSimulatorClientAPI_NoGUI::isConnected() const
{
	return (m_data->m_physicsClientHandle != 0);
}

void b3RobotSimulatorClientAPI_NoGUI::setTimeOut(double timeOutInSec)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	b3SetTimeOut(m_data->m_physicsClientHandle,timeOutInSec);

}


void b3RobotSimulatorClientAPI_NoGUI::disconnect()
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3DisconnectSharedMemory(m_data->m_physicsClientHandle);
	m_data->m_physicsClientHandle = 0;
}

void b3RobotSimulatorClientAPI_NoGUI::syncBodies()
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	statusType = b3GetStatusType(statusHandle);
}

void b3RobotSimulatorClientAPI_NoGUI::resetSimulation()
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	b3SharedMemoryStatusHandle statusHandle;
	statusHandle = b3SubmitClientCommandAndWaitStatus(
		m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
}

bool b3RobotSimulatorClientAPI_NoGUI::canSubmitCommand() const
{
	if (!isConnected())
	{
		return false;
	}
	return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
}

void b3RobotSimulatorClientAPI_NoGUI::stepSimulation()
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
	{
		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
		statusType = b3GetStatusType(statusHandle);
		//btAssert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
	}
}

void b3RobotSimulatorClientAPI_NoGUI::setGravity(const btVector3& gravityAcceleration)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle));

	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
	b3SharedMemoryStatusHandle statusHandle;
	b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	//	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}

btQuaternion b3RobotSimulatorClientAPI_NoGUI::getQuaternionFromEuler(const btVector3& rollPitchYaw)
{
	btQuaternion q;
	q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
	return q;
}

btVector3 b3RobotSimulatorClientAPI_NoGUI::getEulerFromQuaternion(const btQuaternion& quat)
{
	btScalar roll,pitch,yaw;
	quat.getEulerZYX(yaw,pitch,roll);
	btVector3 rpy2 = btVector3(roll,pitch,yaw);
	return rpy2;
}

int b3RobotSimulatorClientAPI_NoGUI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
{
	int robotUniqueId = -1;

	if (!isConnected())
	{
		b3Warning("Not connected");
		return robotUniqueId;
	}

	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());

	//setting the initial position, orientation and other arguments are optional

	b3LoadUrdfCommandSetFlags(command,args.m_flags);

	b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
		args.m_startPosition[1],
		args.m_startPosition[2]);
	b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
	if (args.m_forceOverrideFixedBase)
	{
		b3LoadUrdfCommandSetUseFixedBase(command, true);
	}
	b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	statusType = b3GetStatusType(statusHandle);

	btAssert(statusType == CMD_URDF_LOADING_COMPLETED);
	if (statusType == CMD_URDF_LOADING_COMPLETED)
	{
		robotUniqueId = b3GetStatusBodyIndex(statusHandle);
	}
	return robotUniqueId;
}

bool b3RobotSimulatorClientAPI_NoGUI::loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	b3SharedMemoryCommandHandle command;

	command = b3LoadMJCFCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	statusType = b3GetStatusType(statusHandle);
	if (statusType != CMD_MJCF_LOADING_COMPLETED)
	{
		b3Warning("Couldn't load .mjcf file.");
		return false;
	}
	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
	if (numBodies)
	{
		results.m_uniqueObjectIds.resize(numBodies);
		int numBodies;
		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
	}

	return true;
}

bool b3RobotSimulatorClientAPI_NoGUI::loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	b3SharedMemoryCommandHandle command;

	command = b3LoadBulletCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	statusType = b3GetStatusType(statusHandle);
	if (statusType != CMD_BULLET_LOADING_COMPLETED)
	{
		return false;
	}
	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
	if (numBodies)
	{
		results.m_uniqueObjectIds.resize(numBodies);
		int numBodies;
		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
	}

	return true;
}

bool b3RobotSimulatorClientAPI_NoGUI::loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	bool statusOk = false;

	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
	b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	statusType = b3GetStatusType(statusHandle);
	btAssert(statusType == CMD_SDF_LOADING_COMPLETED);
	if (statusType == CMD_SDF_LOADING_COMPLETED)
	{
		int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
		if (numBodies)
		{
			results.m_uniqueObjectIds.resize(numBodies);
			int numBodies;
			numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
		}
		statusOk = true;
	}

	return statusOk;
}

bool b3RobotSimulatorClientAPI_NoGUI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* bodyInfo)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
	return (result != 0);
}

bool b3RobotSimulatorClientAPI_NoGUI::getBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation) const
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryCommandHandle cmd_handle =
		b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
	b3SharedMemoryStatusHandle status_handle =
		b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);

	const int status_type = b3GetStatusType(status_handle);
	const double* actualStateQ;

	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
	{
		return false;
	}

	b3GetStatusActualState(
		status_handle, 0 /* body_unique_id */,
		0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
		0 /*root_local_inertial_frame*/, &actualStateQ,
		0 /* actual_state_q_dot */, 0 /* joint_reaction_forces */);

	basePosition[0] = actualStateQ[0];
	basePosition[1] = actualStateQ[1];
	basePosition[2] = actualStateQ[2];

	baseOrientation[0] = actualStateQ[3];
	baseOrientation[1] = actualStateQ[4];
	baseOrientation[2] = actualStateQ[5];
	baseOrientation[3] = actualStateQ[6];
	return true;
}

bool b3RobotSimulatorClientAPI_NoGUI::resetBasePositionAndOrientation(int bodyUniqueId, btVector3& basePosition, btQuaternion& baseOrientation)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryCommandHandle commandHandle;

	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);

	b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
	b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
		baseOrientation[2], baseOrientation[3]);

	b3SharedMemoryStatusHandle statusHandle;
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);

	return true;
}

bool b3RobotSimulatorClientAPI_NoGUI::getBaseVelocity(int bodyUniqueId, btVector3& baseLinearVelocity, btVector3& baseAngularVelocity) const
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);

	const int status_type = b3GetStatusType(statusHandle);
	const double* actualStateQdot;

	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
	{
		return false;
	}

	b3GetStatusActualState(statusHandle, 0 /* body_unique_id */,
		0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
		0 /*root_local_inertial_frame*/, 0,
		&actualStateQdot, 0 /* joint_reaction_forces */);

	baseLinearVelocity[0] = actualStateQdot[0];
	baseLinearVelocity[1] = actualStateQdot[1];
	baseLinearVelocity[2] = actualStateQdot[2];

	baseAngularVelocity[0] = actualStateQdot[3];
	baseAngularVelocity[1] = actualStateQdot[4];
	baseAngularVelocity[2] = actualStateQdot[5];
	return true;
}

bool b3RobotSimulatorClientAPI_NoGUI::resetBaseVelocity(int bodyUniqueId, const btVector3& linearVelocity, const btVector3& angularVelocity) const
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;

	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);

	btVector3DoubleData linVelDouble;
	linearVelocity.serializeDouble(linVelDouble);
	b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats);

	btVector3DoubleData angVelDouble;
	angularVelocity.serializeDouble(angVelDouble);
	b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats);

	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	return true;
}

void b3RobotSimulatorClientAPI_NoGUI::setInternalSimFlags(int flags)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	{
		b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
		b3SharedMemoryStatusHandle statusHandle;
		b3PhysicsParamSetInternalSimFlags(command, flags);
		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	}
}


void b3RobotSimulatorClientAPI_NoGUI::setRealTimeSimulation(bool enableRealTimeSimulation)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
	b3SharedMemoryStatusHandle statusHandle;

	int ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation);

	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);

	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}

int b3RobotSimulatorClientAPI_NoGUI::getNumJoints(int bodyUniqueId) const
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}
	return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
}

bool b3RobotSimulatorClientAPI_NoGUI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}
	return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
}

int b3RobotSimulatorClientAPI_NoGUI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return -1;
	}
	b3SharedMemoryStatusHandle statusHandle;
	btAssert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
	{
		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
		int statusType = b3GetStatusType(statusHandle);
		if (statusType == CMD_USER_CONSTRAINT_COMPLETED)
		{
			int userConstraintUid = b3GetStatusUserConstraintUniqueId(statusHandle);
			return userConstraintUid;
		}
	}
	return -1;
}

int b3RobotSimulatorClientAPI_NoGUI::changeConstraint(int constraintId, b3JointInfo* jointInfo)
{

	if (!isConnected())
	{
		b3Warning("Not connected");
		return -1;
	}
	b3SharedMemoryCommandHandle commandHandle = b3InitChangeUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);

	if (jointInfo->m_flags & eJointChangeMaxForce)
	{
		b3InitChangeUserConstraintSetMaxForce(commandHandle, jointInfo->m_jointMaxForce);
	}

	if (jointInfo->m_flags & eJointChangeChildFramePosition)
	{
		b3InitChangeUserConstraintSetPivotInB(commandHandle, &jointInfo->m_childFrame[0]);
	}
	if (jointInfo->m_flags & eJointChangeChildFrameOrientation)
	{
		b3InitChangeUserConstraintSetFrameInB(commandHandle, &jointInfo->m_childFrame[3]);
	}

	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	int statusType = b3GetStatusType(statusHandle);
	return statusType;
}

void b3RobotSimulatorClientAPI_NoGUI::removeConstraint(int constraintId)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	b3SharedMemoryCommandHandle commandHandle = b3InitRemoveUserConstraintCommand(m_data->m_physicsClientHandle, constraintId);
	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	int statusType = b3GetStatusType(statusHandle);
}


bool b3RobotSimulatorClientAPI_NoGUI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	int statusType = b3GetStatusType(statusHandle);
	if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
	{
		if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
		{
			return true;
		}
	}
	return false;
}

bool b3RobotSimulatorClientAPI_NoGUI::getJointStates(int bodyUniqueId, b3JointStates2& state)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);

	if (statusHandle)
	{
		//   double rootInertialFrame[7];
		const double* rootLocalInertialFrame;
		const double* actualStateQ;
		const double* actualStateQdot;
		const double* jointReactionForces;

		int stat = b3GetStatusActualState(statusHandle,
			&state.m_bodyUniqueId,
			&state.m_numDegreeOfFreedomQ,
			&state.m_numDegreeOfFreedomU,
			&rootLocalInertialFrame,
			&actualStateQ,
			&actualStateQdot,
			&jointReactionForces);
		if (stat)
		{
			state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
			state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);

			for (int i=0;i<state.m_numDegreeOfFreedomQ;i++)
			{
				state.m_actualStateQ[i] = actualStateQ[i];
			}
			for (int i=0;i<state.m_numDegreeOfFreedomU;i++)
			{
				state.m_actualStateQdot[i] = actualStateQdot[i];
			}
			int numJoints = getNumJoints(bodyUniqueId);
			state.m_jointReactionForces.resize(6*numJoints);
			for (int i=0;i<numJoints*6;i++)
			{
				state.m_jointReactionForces[i] = jointReactionForces[i];
			}

			return true;
		}
		//rootInertialFrame,
		//              &state.m_actualStateQ[0],
		//            &state.m_actualStateQdot[0],
		//          &state.m_jointReactionForces[0]);


	}
	return false;
}

bool b3RobotSimulatorClientAPI_NoGUI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;
	int numJoints;

	numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
	if ((jointIndex >= numJoints) || (jointIndex < 0))
	{
		return false;
	}

	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);

	b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
		targetValue);

	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	return false;
}

void b3RobotSimulatorClientAPI_NoGUI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	b3SharedMemoryStatusHandle statusHandle;
	switch (args.m_controlMode)
	{
	case CONTROL_MODE_VELOCITY:
		{
			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
			b3JointInfo jointInfo;
			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
			int uIndex = jointInfo.m_uIndex;
			b3JointControlSetKd(command, uIndex, args.m_kd);
			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
			break;
		}
	case CONTROL_MODE_POSITION_VELOCITY_PD:
		{
			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
			b3JointInfo jointInfo;
			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
			int uIndex = jointInfo.m_uIndex;
			int qIndex = jointInfo.m_qIndex;

			b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
			b3JointControlSetKp(command, uIndex, args.m_kp);
			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
			b3JointControlSetKd(command, uIndex, args.m_kd);
			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
			break;
		}
	case CONTROL_MODE_TORQUE:
		{
			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
			b3JointInfo jointInfo;
			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
			int uIndex = jointInfo.m_uIndex;
			b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
			break;
		}
	default:
		{
			b3Error("Unknown control command in b3RobotSimulationClientAPI::setJointMotorControl");
		}
	}
}

void b3RobotSimulatorClientAPI_NoGUI::setNumSolverIterations(int numIterations)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
	b3SharedMemoryStatusHandle statusHandle;
	b3PhysicsParamSetNumSolverIterations(command, numIterations);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}

void b3RobotSimulatorClientAPI_NoGUI::setContactBreakingThreshold(double threshold)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
	b3SharedMemoryStatusHandle statusHandle;
	b3PhysicsParamSetContactBreakingThreshold(command,threshold);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}


void b3RobotSimulatorClientAPI_NoGUI::setTimeStep(double timeStepInSeconds)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
	b3SharedMemoryStatusHandle statusHandle;
	int ret;
	ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}

void b3RobotSimulatorClientAPI_NoGUI::setNumSimulationSubSteps(int numSubSteps)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
	b3SharedMemoryStatusHandle statusHandle;
	b3PhysicsParamSetNumSubSteps(command, numSubSteps);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	btAssert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}

bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}
	btAssert(args.m_endEffectorLinkIndex >= 0);
	btAssert(args.m_bodyUniqueId >= 0);

	b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId);
	if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
	{
		b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
	}
	else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
	{
		b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
	}
	else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
	{
		b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
	}
	else
	{
		b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
	}

	if (args.m_flags & B3_HAS_JOINT_DAMPING)
	{
		b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
	}

	b3SharedMemoryStatusHandle statusHandle;
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);

	int numPos = 0;

	bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
		&results.m_bodyUniqueId,
		&numPos,
		0) != 0;
	if (result && numPos)
	{
		results.m_calculatedJointPositions.resize(numPos);
		result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
			&results.m_bodyUniqueId,
			&numPos,
			&results.m_calculatedJointPositions[0]) != 0;
	}
	return result;
}

bool b3RobotSimulatorClientAPI_NoGUI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);

	if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
	{
		int dofCount;
		b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian);
		return true;
	}
	return false;
}


bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState)
{
	bool computeLinkVelocity = true;
	bool computeForwardKinematics = true;

	return getLinkState(bodyUniqueId, linkIndex, computeLinkVelocity, computeForwardKinematics, linkState);
}

bool b3RobotSimulatorClientAPI_NoGUI::getLinkState(int bodyUniqueId, int linkIndex, int computeLinkVelocity, int computeForwardKinematics, b3LinkState* linkState)
{
	if (!isConnected()) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);

	if (computeLinkVelocity) {
		b3RequestActualStateCommandComputeLinkVelocity(command, computeLinkVelocity);
	}

	if (computeForwardKinematics) {
		b3RequestActualStateCommandComputeForwardKinematics(command, computeForwardKinematics);
	}

	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);


	if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
		b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
		return true;
	}
	return false;
}

void b3RobotSimulatorClientAPI_NoGUI::configureDebugVisualizer(b3ConfigureDebugVisualizerEnum flag, int enable)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}
	b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
	b3ConfigureOpenGLVisualizerSetVisualizationFlags(commandHandle, flag, enable);
	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}

void b3RobotSimulatorClientAPI_NoGUI::getVREvents(b3VREventsData* vrEventsData,int deviceTypeFilter)
{
	vrEventsData->m_numControllerEvents = 0;
	vrEventsData->m_controllerEvents = 0;
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
	b3VREventsSetDeviceTypeFilter(commandHandle, deviceTypeFilter);
	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
}

void b3RobotSimulatorClientAPI_NoGUI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
{
	keyboardEventsData->m_numKeyboardEvents = 0;
	keyboardEventsData->m_keyboardEvents = 0;
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
}

int b3RobotSimulatorClientAPI_NoGUI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const btAlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return -1;
	}
	int loggingUniqueId = -1;
	b3SharedMemoryCommandHandle commandHandle;
	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);

	b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());

	for (int i = 0; i < objectUniqueIds.size(); i++)
	{
		int objectUid = objectUniqueIds[i];
		b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
	}

	if (maxLogDof > 0)
	{
		b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
	}

	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	statusType = b3GetStatusType(statusHandle);
	if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
	{
		loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
	}
	return loggingUniqueId;
}

void b3RobotSimulatorClientAPI_NoGUI::stopStateLogging(int stateLoggerUniqueId)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;
	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
	b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}

void b3RobotSimulatorClientAPI_NoGUI::resetDebugVisualizerCamera(double cameraDistance, double cameraPitch, double cameraYaw, const btVector3& targetPos)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle commandHandle = b3InitConfigureOpenGLVisualizer(m_data->m_physicsClientHandle);
	if (commandHandle)
	{
		if ((cameraDistance >= 0))
		{
			btVector3FloatData camTargetPos;
			targetPos.serializeFloat(camTargetPos);
			b3ConfigureOpenGLVisualizerSetViewMatrix(commandHandle, cameraDistance, cameraPitch, cameraYaw, camTargetPos.m_floats);			
		}
		b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
	}
}

void b3RobotSimulatorClientAPI_NoGUI::submitProfileTiming(const std::string&  profileName, int durationInMicroSeconds)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle	commandHandle = b3ProfileTimingCommandInit(m_data->m_physicsClientHandle, profileName.c_str());
	if (durationInMicroSeconds>=0)
	{
		b3SetProfileTimingDuractionInMicroSeconds(commandHandle, durationInMicroSeconds);
	}
	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
}

void b3RobotSimulatorClientAPI_NoGUI::loadSoftBody(const std::string& fileName, double scale, double mass, double collisionMargin)
{
	if (!isConnected())
	{
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle command = b3LoadSoftBodyCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
	b3LoadSoftBodySetScale(command, scale);
	b3LoadSoftBodySetMass(command, mass);
	b3LoadSoftBodySetCollisionMargin(command, collisionMargin);
	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
}

void b3RobotSimulatorClientAPI_NoGUI::getMouseEvents(b3MouseEventsData* mouseEventsData)
{
	mouseEventsData->m_numMouseEvents = 0;
	mouseEventsData->m_mouseEvents = 0;
	if (!isConnected()) {
		b3Warning("Not connected");
		return;
	}

	b3SharedMemoryCommandHandle command = b3RequestMouseEventsCommandInit(m_data->m_physicsClientHandle);
	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
	b3GetMouseEventsData(m_data->m_physicsClientHandle, mouseEventsData);
}


bool b3RobotSimulatorClientAPI_NoGUI::getCameraImage(int width, int height, struct b3RobotSimulatorGetCameraImageArgs args, struct b3CameraImageData &imageData)
{
	if (!isConnected()) {
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryCommandHandle command;

	command = b3InitRequestCameraImage(m_data->m_physicsClientHandle);

	b3RequestCameraImageSetPixelResolution(command, width, height);

	// Check and apply optional arguments
	if (args.m_viewMatrix && args.m_projectionMatrix) {
		b3RequestCameraImageSetCameraMatrices(command, args.m_viewMatrix, args.m_projectionMatrix);
	}

	if (args.m_lightDirection != NULL)	{
		b3RequestCameraImageSetLightDirection(command, args.m_lightDirection);
	}

	if (args.m_lightColor != NULL) {
		b3RequestCameraImageSetLightColor(command, args.m_lightColor);
	}

	if (args.m_lightDistance>=0) {
		b3RequestCameraImageSetLightDistance(command, args.m_lightDistance);
	}

	if (args.m_hasShadow>=0) {
		b3RequestCameraImageSetShadow(command, args.m_hasShadow);
	}

	if (args.m_lightAmbientCoeff>=0) {
		b3RequestCameraImageSetLightAmbientCoeff(command, args.m_lightAmbientCoeff);
	}

	if (args.m_lightDiffuseCoeff>=0) {
		b3RequestCameraImageSetLightDiffuseCoeff(command, args.m_lightDiffuseCoeff);
	}

	if (args.m_lightSpecularCoeff>=0) {
		b3RequestCameraImageSetLightSpecularCoeff(command, args.m_lightSpecularCoeff);
	}

	if (args.m_renderer>=0) {
		b3RequestCameraImageSelectRenderer(command, args.m_renderer);
	}

	// Actually retrieve the image
	if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) {
		b3SharedMemoryStatusHandle statusHandle;
		int statusType;

		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
		statusType = b3GetStatusType(statusHandle);
		if (statusType == CMD_CAMERA_IMAGE_COMPLETED) {
			b3GetCameraImageData(m_data->m_physicsClientHandle, &imageData);
		}
	} else {
		return false;
	}
	return true;
}

bool b3RobotSimulatorClientAPI_NoGUI::calculateInverseDynamics(int bodyUniqueId, double *jointPositions, double *jointVelocities,
	double *jointAccelerations, double *jointForcesOutput) 
{
	if (!isConnected()) {
		b3Warning("Not connected");
		return false;
	}

	int numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, jointPositions,
		jointVelocities, jointAccelerations);

	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);

	statusType = b3GetStatusType(statusHandle);

	if (statusType == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) {
		int bodyUniqueId;
		int dofCount;

		b3GetStatusInverseDynamicsJointForces(statusHandle, &bodyUniqueId, &dofCount, 0);

		if (dofCount) {
			b3GetStatusInverseDynamicsJointForces(statusHandle, 0, 0,	jointForcesOutput);
			return true;
		}
	}
	return false;
}

int b3RobotSimulatorClientAPI_NoGUI::getNumBodies() const
{
	if (!isConnected()) {
		b3Warning("Not connected");
		return false;
	}
	return b3GetNumBodies(m_data->m_physicsClientHandle);
}

int b3RobotSimulatorClientAPI_NoGUI::getBodyUniqueId(int bodyId) const
{
	if (!isConnected()) {
		b3Warning("Not connected");
		return false;
	}
	return b3GetBodyUniqueId(m_data->m_physicsClientHandle, bodyId);
}

bool b3RobotSimulatorClientAPI_NoGUI::removeBody(int bodyUniqueId) 
{
	if (!isConnected()) {
		b3Warning("Not connected");
		return false;
	}

	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) {
		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitRemoveBodyCommand(m_data->m_physicsClientHandle, bodyUniqueId));
		statusType = b3GetStatusType(statusHandle);
		if (statusType == CMD_REMOVE_BODY_COMPLETED) {
			return true;
		} else {
			b3Warning("getDynamicsInfo did not complete");
			return false;
		}
	}
	b3Warning("removeBody could not submit command");
	return false;
}

bool b3RobotSimulatorClientAPI_NoGUI::getDynamicsInfo(int bodyUniqueId, int linkIndex, b3DynamicsInfo *dynamicsInfo) {
	if (!isConnected()) {
		b3Warning("Not connected");
		return false;
	}
	int status_type = 0;
	b3SharedMemoryCommandHandle cmd_handle;
	b3SharedMemoryStatusHandle status_handle;
	//  struct b3DynamicsInfo info;

	if (bodyUniqueId < 0) {
		b3Warning("getDynamicsInfo failed; invalid bodyUniqueId");
		return false;
	}
	if (linkIndex < -1) {
		b3Warning("getDynamicsInfo failed; invalid linkIndex");
		return false;
	}

	if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) {
		cmd_handle = b3GetDynamicsInfoCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex);
		status_handle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, cmd_handle);
		status_type = b3GetStatusType(status_handle);
		if (status_type == CMD_GET_DYNAMICS_INFO_COMPLETED) {
			return true;
		} else {
			b3Warning("getDynamicsInfo did not complete");
			return false;
		}
	} 
	b3Warning("getDynamicsInfo could not submit command");
	return false;
}


bool b3RobotSimulatorClientAPI_NoGUI::changeDynamics(int bodyUniqueId, int linkIndex, struct b3RobotSimulatorChangeDynamicsArgs &args)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected to physics server.");
		return false;
	}
	b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(sm);
	b3SharedMemoryStatusHandle statusHandle;

	if (args.m_mass >= 0) {
		b3ChangeDynamicsInfoSetMass(command, bodyUniqueId, linkIndex, args.m_mass);
	}

	if (args.m_lateralFriction >= 0) {
		b3ChangeDynamicsInfoSetLateralFriction(command, bodyUniqueId, linkIndex, args.m_lateralFriction);
	}

	if (args.m_spinningFriction>=0) {
		b3ChangeDynamicsInfoSetSpinningFriction(command, bodyUniqueId, linkIndex, args.m_spinningFriction);
	}

	if (args.m_rollingFriction>=0) {
		b3ChangeDynamicsInfoSetRollingFriction(command, bodyUniqueId, linkIndex, args.m_rollingFriction);
	}

	if (args.m_linearDamping>=0)	{
		b3ChangeDynamicsInfoSetLinearDamping(command, bodyUniqueId, args.m_linearDamping);
	}

	if (args.m_angularDamping>=0) {
		b3ChangeDynamicsInfoSetAngularDamping(command, bodyUniqueId, args.m_angularDamping);
	}

	if (args.m_restitution>=0) {
		b3ChangeDynamicsInfoSetRestitution(command, bodyUniqueId, linkIndex, args.m_restitution);
	}

	if (args.m_contactStiffness>=0 && args.m_contactDamping >=0) {
		b3ChangeDynamicsInfoSetContactStiffnessAndDamping(command, bodyUniqueId, linkIndex, args.m_contactStiffness, args.m_contactDamping);
	}

	if (args.m_frictionAnchor>=0) {
		b3ChangeDynamicsInfoSetFrictionAnchor(command, bodyUniqueId,linkIndex, args.m_frictionAnchor);
	}
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	return true;
}

int b3RobotSimulatorClientAPI_NoGUI::addUserDebugParameter(char * paramName, double rangeMin, double rangeMax, double startValue) {
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected to physics server.");
		return -1;
	}
	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	commandHandle = b3InitUserDebugAddParameter(sm, paramName, rangeMin, rangeMax, startValue);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
	statusType = b3GetStatusType(statusHandle);

	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
		return  debugItemUniqueId;
	}
	b3Warning("addUserDebugParameter failed.");
	return -1;
}


double b3RobotSimulatorClientAPI_NoGUI::readUserDebugParameter(int itemUniqueId) {
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected to physics server.");
		return 0;
	}
	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	commandHandle = b3InitUserDebugReadParameter(sm, itemUniqueId);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
	statusType = b3GetStatusType(statusHandle);

	if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) {
		double paramValue = 0.f;
		int ok = b3GetStatusDebugParameterValue(statusHandle, &paramValue);
		if (ok) {
			return paramValue;
		} 
	}
	b3Warning("readUserDebugParameter failed.");
	return 0;
}


bool b3RobotSimulatorClientAPI_NoGUI::removeUserDebugItem(int itemUniqueId) {
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected to physics server.");
		return false;
	}
	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	commandHandle = b3InitUserDebugDrawRemove(sm, itemUniqueId);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
	statusType = b3GetStatusType(statusHandle);
	return true;
}


int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, double *posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected to physics server.");
		return -1;
	}
	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	commandHandle = b3InitUserDebugDrawAddText3D(sm, text, posXYZ, &args.m_colorRGB[0], args.m_size, args.m_lifeTime);

	if (args.m_parentObjectUniqueId>=0) {
		b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
	}

	if (args.m_flags & DEBUG_TEXT_HAS_ORIENTATION) {
		b3UserDebugTextSetOrientation(commandHandle, &args.m_textOrientation[0]);
	}

	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
	statusType = b3GetStatusType(statusHandle);

	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
		return  debugItemUniqueId;
	}
	b3Warning("addUserDebugText3D failed.");
	return -1;
}

int b3RobotSimulatorClientAPI_NoGUI::addUserDebugText(char *text, btVector3 &posXYZ, struct b3RobotSimulatorAddUserDebugTextArgs & args)
{
	double dposXYZ[3];
	dposXYZ[0] = posXYZ.x();
	dposXYZ[1] = posXYZ.y();
	dposXYZ[2] = posXYZ.z();

	return addUserDebugText(text, &dposXYZ[0], args);
}

int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(double *fromXYZ, double *toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected to physics server.");
		return -1;
	}
	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	commandHandle = b3InitUserDebugDrawAddLine3D(sm, fromXYZ, toXYZ, &args.m_colorRGB[0], args.m_lineWidth, args.m_lifeTime);

	if (args.m_parentObjectUniqueId>=0) {
		b3UserDebugItemSetParentObject(commandHandle, args.m_parentObjectUniqueId, args.m_parentLinkIndex);
	}

	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
	statusType = b3GetStatusType(statusHandle);

	if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) {
		int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle);
		return  debugItemUniqueId;
	}
	b3Warning("addUserDebugLine failed.");
	return -1;
}

int b3RobotSimulatorClientAPI_NoGUI::addUserDebugLine(btVector3 &fromXYZ, btVector3 &toXYZ, struct b3RobotSimulatorAddUserDebugLineArgs & args)
{
	double dfromXYZ[3];
	double dtoXYZ[3];
	dfromXYZ[0] = fromXYZ.x();
	dfromXYZ[1] = fromXYZ.y();
	dfromXYZ[2] = fromXYZ.z();

	dtoXYZ[0] = toXYZ.x();
	dtoXYZ[1] = toXYZ.y();
	dtoXYZ[2] = toXYZ.z();
	return addUserDebugLine(&dfromXYZ[0], &dtoXYZ[0], args);
}



bool b3RobotSimulatorClientAPI_NoGUI::setJointMotorControlArray(int bodyUniqueId, struct b3RobotSimulatorJointMotorArrayArgs &args)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected to physics server.");
		return false;
	}
	int numJoints = b3GetNumJoints(sm, bodyUniqueId);

	b3SharedMemoryCommandHandle commandHandle;
	b3SharedMemoryStatusHandle statusHandle;
	//  int statusType;
	struct b3JointInfo info;

	commandHandle = b3JointControlCommandInit2(sm, bodyUniqueId, args.m_controlMode);

	for (int i=0;i<args.m_numControlledDofs;i++) {
		double targetVelocity = 0.0;
		double targetPosition = 0.0;
		double force = 100000.0;
		double kp = 0.1;
		double kd = 1.0;
		int jointIndex;

		if (args.m_jointIndices) {
			jointIndex = args.m_jointIndices[i];
		} else {
			jointIndex = i;
		}

		if (args.m_targetVelocities) {
			targetVelocity = args.m_targetVelocities[i];
		}

		if (args.m_targetPositions) {
			targetPosition = args.m_targetPositions[i];
		}

		if (args.m_forces) {
			force = args.m_forces[i];
		}

		if (args.m_kps) {
			kp = args.m_kps[i];
		}

		if (args.m_kds) {
			kd = args.m_kds[i];
		}

		b3GetJointInfo(sm, bodyUniqueId, jointIndex, &info);

		switch (args.m_controlMode) {
		case CONTROL_MODE_VELOCITY: {
			b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
			b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
			b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
			break;
									}

		case CONTROL_MODE_TORQUE: {
			b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, force);
			break;
								  }

		case CONTROL_MODE_POSITION_VELOCITY_PD: {
			b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
			b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
			b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
			b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
			b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force);
			break;
												}

		default: {}
		};
	}
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
	return true;
}


bool b3RobotSimulatorClientAPI_NoGUI::setPhysicsEngineParameter(struct b3RobotSimulatorSetPhysicsEngineParameters &args)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
	b3SharedMemoryStatusHandle statusHandle;

	if (args.m_numSolverIterations >= 0) {
		b3PhysicsParamSetNumSolverIterations(command, args.m_numSolverIterations);
	}

	if (args.m_collisionFilterMode >= 0) {
		b3PhysicsParamSetCollisionFilterMode(command, args.m_collisionFilterMode);
	}

	if (args.m_numSubSteps >= 0)	{
		b3PhysicsParamSetNumSubSteps(command, args.m_numSubSteps);
	}

	if (args.m_fixedTimeStep >= 0) {
		b3PhysicsParamSetTimeStep(command, args.m_fixedTimeStep);
	}

	if (args.m_useSplitImpulse >= 0) {
		b3PhysicsParamSetUseSplitImpulse(command, args.m_useSplitImpulse);
	}

	if (args.m_splitImpulsePenetrationThreshold >= 0) {
		b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, args.m_splitImpulsePenetrationThreshold);
	}

	if (args.m_contactBreakingThreshold >= 0) {
		b3PhysicsParamSetContactBreakingThreshold(command, args.m_contactBreakingThreshold);
	}

	if (args.m_maxNumCmdPer1ms >= -1) {
		b3PhysicsParamSetMaxNumCommandsPer1ms(command, args.m_maxNumCmdPer1ms);
	}

	if (args.m_restitutionVelocityThreshold>=0) {
		b3PhysicsParamSetRestitutionVelocityThreshold(command, args.m_restitutionVelocityThreshold);
	}

	if (args.m_enableFileCaching>=0) {
		b3PhysicsParamSetEnableFileCaching(command, args.m_enableFileCaching);
	}

	if (args.m_erp>=0) {
		b3PhysicsParamSetDefaultNonContactERP(command,args.m_erp);
	}

	if (args.m_contactERP>=0) {
		b3PhysicsParamSetDefaultContactERP(command,args.m_contactERP);
	}

	if (args.m_frictionERP >=0) {
		b3PhysicsParamSetDefaultFrictionERP(command,args.m_frictionERP);
	}

	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	return true;
}


bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, double *force, double *position, int flags) 
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;

	command = b3ApplyExternalForceCommandInit(sm);
	b3ApplyExternalForce(command, objectUniqueId, linkIndex, force, position, flags);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	return true;
}

bool b3RobotSimulatorClientAPI_NoGUI::applyExternalForce(int objectUniqueId, int linkIndex, btVector3 &force, btVector3 &position, int flags)
{
	double dforce[3];
	double dposition[3];

	dforce[0] = force.x();
	dforce[1] = force.y();
	dforce[2] = force.z();

	dposition[0] = position.x();
	dposition[1] = position.y();
	dposition[2] = position.z();

	return applyExternalForce(objectUniqueId, linkIndex, &dforce[0], &dposition[0], flags);
}


bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, double *torque, int flags) 
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;

	command = b3ApplyExternalForceCommandInit(sm);
	b3ApplyExternalTorque(command, objectUniqueId, linkIndex, torque, flags);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	return true;
}

bool b3RobotSimulatorClientAPI_NoGUI::applyExternalTorque(int objectUniqueId, int linkIndex, btVector3 &torque, int flags)
{
	double dtorque[3];

	dtorque[0] = torque.x();
	dtorque[1] = torque.y();
	dtorque[2] = torque.z();

	return applyExternalTorque(objectUniqueId, linkIndex, &dtorque[0], flags);
}


bool  b3RobotSimulatorClientAPI_NoGUI::enableJointForceTorqueSensor(int bodyUniqueId, int jointIndex, bool enable)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	int numJoints = b3GetNumJoints(sm, bodyUniqueId);
	if ((jointIndex < 0) || (jointIndex >= numJoints)) {
		b3Warning("Error: invalid jointIndex.");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	command = b3CreateSensorCommandInit(sm, bodyUniqueId);
	b3CreateSensorEnable6DofJointForceTorqueSensor(command, jointIndex, enable);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	statusType = b3GetStatusType(statusHandle);
	if (statusType == CMD_CLIENT_COMMAND_COMPLETED) {
		return true;
	}
	return false;
}

bool b3RobotSimulatorClientAPI_NoGUI::getDebugVisualizerCamera(struct b3OpenGLVisualizerCameraInfo *cameraInfo)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	command = b3InitRequestOpenGLVisualizerCameraCommand(sm);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	statusType = b3GetStatusOpenGLVisualizerCamera(statusHandle, cameraInfo);

	if (statusType) {
		return true;
	}
	return false;
}

bool b3RobotSimulatorClientAPI_NoGUI::getContactPoints(struct b3RobotSimulatorGetContactPointsArgs &args, struct b3ContactInformation *contactInfo)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	command = b3InitRequestContactPointInformation(sm);

	if (args.m_bodyUniqueIdA>=0) {
		b3SetContactFilterBodyA(command, args.m_bodyUniqueIdA);
	}
	if (args.m_bodyUniqueIdB>=0) {
		b3SetContactFilterBodyB(command, args.m_bodyUniqueIdB);
	}
	if (args.m_linkIndexA>=-1) {
		b3SetContactFilterLinkA(command, args.m_linkIndexA);
	}
	if (args.m_linkIndexB >=-1) {
		b3SetContactFilterLinkB(command, args.m_linkIndexB);
	}

	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	statusType = b3GetStatusType(statusHandle);

	if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
		b3GetContactPointInformation(sm, contactInfo);
		return true;
	}
	return false;
}

bool b3RobotSimulatorClientAPI_NoGUI::getClosestPoints(struct b3RobotSimulatorGetContactPointsArgs &args, double distance, struct b3ContactInformation *contactInfo)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	command = b3InitClosestDistanceQuery(sm);

	b3SetClosestDistanceFilterBodyA(command, args.m_bodyUniqueIdA);
	b3SetClosestDistanceFilterBodyB(command, args.m_bodyUniqueIdB);
	b3SetClosestDistanceThreshold(command, distance);

	if (args.m_linkIndexA>=-1) {
		b3SetClosestDistanceFilterLinkA(command, args.m_linkIndexA);
	}
	if (args.m_linkIndexB >=-1) {
		b3SetClosestDistanceFilterLinkB(command, args.m_linkIndexB);
	}

	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	statusType = b3GetStatusType(statusHandle);

	if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) {
		b3GetContactPointInformation(sm, contactInfo);
		return true;
	}
	return false;
}


bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(double *aabbMin, double *aabbMax, struct b3AABBOverlapData *overlapData)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	//  int statusType;

	command = b3InitAABBOverlapQuery(sm, aabbMin, aabbMax);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	b3GetAABBOverlapResults(sm, overlapData);

	return true;
}


bool b3RobotSimulatorClientAPI_NoGUI::getOverlappingObjects(btVector3 &aabbMin, btVector3 &aabbMax, struct b3AABBOverlapData *overlapData)
{
	double daabbMin[3];
	double daabbMax[3];

	daabbMin[0] = aabbMin.x();
	daabbMin[1] = aabbMin.y();
	daabbMin[2] = aabbMin.z();

	daabbMax[0] = aabbMax.x();
	daabbMax[1] = aabbMax.y();
	daabbMax[2] = aabbMax.z();

	return getOverlappingObjects(&daabbMin[0], &daabbMax[0], overlapData);
}



bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, double *aabbMin, double *aabbMax)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;

	if (bodyUniqueId < 0) {
		b3Warning("Invalid bodyUniqueId");
		return false;
	}

	if (linkIndex < -1) {
		b3Warning("Invalid linkIndex");
		return false;
	}

	if (aabbMin == NULL || aabbMax == NULL) {
		b3Warning("Output AABB matrix is NULL");
		return false;
	}    

	command = b3RequestCollisionInfoCommandInit(sm, bodyUniqueId);
	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);

	statusType = b3GetStatusType(statusHandle);
	if (statusType != CMD_REQUEST_COLLISION_INFO_COMPLETED) {
		return false;
	}
	if (b3GetStatusAABB(statusHandle, linkIndex, aabbMin, aabbMax)) {
		return true;
	}
	return false;
}

bool b3RobotSimulatorClientAPI_NoGUI::getAABB(int bodyUniqueId, int linkIndex, btVector3 &aabbMin, btVector3 &aabbMax)
{
	double daabbMin[3];
	double daabbMax[3];

	bool status = getAABB(bodyUniqueId, linkIndex, &daabbMin[0], &daabbMax[0]);

	aabbMin[0] = (float)daabbMin[0];
	aabbMin[1] = (float)daabbMin[1];
	aabbMin[2] = (float)daabbMin[2];

	aabbMax[0] = (float)daabbMax[0];
	aabbMax[1] = (float)daabbMax[1];
	aabbMax[2] = (float)daabbMax[2];

	return status;
}


int b3RobotSimulatorClientAPI_NoGUI::createCollisionShape(int shapeType, struct b3RobotSimulatorCreateCollisionShapeArgs &args)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType;
	int shapeIndex = -1;

	command = b3CreateCollisionShapeCommandInit(sm);

	if (shapeType==GEOM_SPHERE && args.m_radius>0) {
		shapeIndex = b3CreateCollisionShapeAddSphere(command, args.m_radius);
	}
	if (shapeType==GEOM_BOX)  {
		double halfExtents[3];
		scalarToDouble3(args.m_halfExtents, halfExtents);
		shapeIndex = b3CreateCollisionShapeAddBox(command, halfExtents);
	}
	if (shapeType==GEOM_CAPSULE && args.m_radius>0 && args.m_height>=0) {
		shapeIndex = b3CreateCollisionShapeAddCapsule(command, args.m_radius, args.m_height);
	}
	if (shapeType==GEOM_CYLINDER && args.m_radius>0 && args.m_height>=0) {
		shapeIndex = b3CreateCollisionShapeAddCylinder(command, args.m_radius, args.m_height);
	}
	if (shapeType==GEOM_MESH && args.m_fileName) {
		double meshScale[3];
		scalarToDouble3(args.m_meshScale, meshScale);
		shapeIndex = b3CreateCollisionShapeAddMesh(command, args.m_fileName, meshScale);
	}
	if (shapeType==GEOM_PLANE) {
		double planeConstant=0;
		double planeNormal[3];
		scalarToDouble3(args.m_planeNormal, planeNormal);
		shapeIndex = b3CreateCollisionShapeAddPlane(command, planeNormal, planeConstant);
	}
	if (shapeIndex>=0 && args.m_flags) {
		b3CreateCollisionSetFlag(command, shapeIndex, args.m_flags);
	}

	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	statusType = b3GetStatusType(statusHandle);
	if (statusType == CMD_CREATE_COLLISION_SHAPE_COMPLETED) {
		int uid = b3GetStatusCollisionShapeUniqueId(statusHandle);
		return uid;
	}
	return -1;
}

int b3RobotSimulatorClientAPI_NoGUI::createMultiBody(struct b3RobotSimulatorCreateMultiBodyArgs &args)
{
	b3PhysicsClientHandle sm = m_data->m_physicsClientHandle;
	if (sm == 0) {
		b3Warning("Not connected");
		return false;
	}
	b3SharedMemoryCommandHandle command;
	b3SharedMemoryStatusHandle statusHandle;
	int statusType, baseIndex;

	double doubleBasePosition[3];
	double doubleBaseInertialFramePosition[3];
	scalarToDouble3(args.m_basePosition.m_floats, doubleBasePosition);
	scalarToDouble3(args.m_baseInertialFramePosition.m_floats, doubleBaseInertialFramePosition);

	double doubleBaseOrientation[4];
	double doubleBaseInertialFrameOrientation[4];
	scalarToDouble4(args.m_baseOrientation, doubleBaseOrientation);
	scalarToDouble4(args.m_baseInertialFrameOrientation, doubleBaseInertialFrameOrientation);

	command = b3CreateMultiBodyCommandInit(sm);

	baseIndex = b3CreateMultiBodyBase(command, args.m_baseMass, args.m_baseCollisionShapeIndex, args.m_baseVisualShapeIndex,
		doubleBasePosition, doubleBaseOrientation, doubleBaseInertialFramePosition, doubleBaseInertialFrameOrientation);

	for (int i = 0; i < args.m_numLinks; i++) {
		double linkMass = args.m_linkMasses[i];
		int linkCollisionShapeIndex = args.m_linkCollisionShapeIndices[i];
		int linkVisualShapeIndex = args.m_linkVisualShapeIndices[i];
		btVector3 linkPosition = args.m_linkPositions[i];
		btQuaternion linkOrientation = args.m_linkOrientations[i];
		btVector3 linkInertialFramePosition = args.m_linkInertialFramePositions[i];
		btQuaternion linkInertialFrameOrientation = args.m_linkInertialFrameOrientations[i];
		int linkParentIndex = args.m_linkParentIndices[i];
		int linkJointType = args.m_linkJointTypes[i];
		btVector3 linkJointAxis = args.m_linkJointAxes[i];

		double doubleLinkPosition[3];
		double doubleLinkInertialFramePosition[3];
		double doubleLinkJointAxis[3];
		scalarToDouble3(linkPosition.m_floats, doubleLinkPosition);
		scalarToDouble3(linkInertialFramePosition.m_floats, doubleLinkInertialFramePosition);
		scalarToDouble3(linkJointAxis.m_floats, doubleLinkJointAxis);

		double doubleLinkOrientation[4];
		double doubleLinkInertialFrameOrientation[4];
		scalarToDouble4(linkOrientation, doubleLinkOrientation);
		scalarToDouble4(linkInertialFrameOrientation, doubleLinkInertialFrameOrientation);

		b3CreateMultiBodyLink(command, 
			linkMass, 
			linkCollisionShapeIndex, 
			linkVisualShapeIndex, 
			doubleLinkPosition, 
			doubleLinkOrientation,
			doubleLinkInertialFramePosition,
			doubleLinkInertialFrameOrientation,
			linkParentIndex,
			linkJointType,
			doubleLinkJointAxis
			);    
	}

	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
	statusType = b3GetStatusType(statusHandle);
	if (statusType == CMD_CREATE_MULTI_BODY_COMPLETED) {
		int uid = b3GetStatusBodyIndex(statusHandle);
		return uid;
	}
	return -1;
}

int b3RobotSimulatorClientAPI_NoGUI::getNumConstraints() const
{
	if (!isConnected()) {
		b3Warning("Not connected");
		return -1;
	}
	return b3GetNumUserConstraints(m_data->m_physicsClientHandle);
}

int b3RobotSimulatorClientAPI_NoGUI::getConstraintUniqueId(int serialIndex)
{
	if (!isConnected()) {
		b3Warning("Not connected");
		return -1;
	}
	int userConstraintId = -1;
	userConstraintId = b3GetUserConstraintId(m_data->m_physicsClientHandle, serialIndex);
	return userConstraintId;
}



void b3RobotSimulatorClientAPI_NoGUI::setGuiHelper(struct GUIHelperInterface* guiHelper)
{
	m_data->m_guiHelper = guiHelper;
}

struct GUIHelperInterface* b3RobotSimulatorClientAPI_NoGUI::getGuiHelper()
{
	return m_data->m_guiHelper;
}