#include "b3RobotSimulatorClientAPI_NoGUI.h"

int main(int argc, char* argv[])
{
	b3RobotSimulatorClientAPI_NoGUI* sim = new b3RobotSimulatorClientAPI_NoGUI();

	bool isConnected = sim->connect(eCONNECT_SHARED_MEMORY);

	if (!isConnected)
	{
		printf("Using Direct mode\n");
		isConnected = sim->connect(eCONNECT_DIRECT);
	}
	else
	{
		printf("Using shared memory\n");
	}

	//remove all existing objects (if any)
	sim->resetSimulation();
	sim->setGravity(btVector3(0, 0, -9.8));
	sim->setNumSolverIterations(100);
	b3RobotSimulatorSetPhysicsEngineParameters args;
	sim->getPhysicsEngineParameters(args);

	int planeUid = sim->loadURDF("plane.urdf");
	printf("planeUid = %d\n", planeUid);

	int r2d2Uid = sim->loadURDF("r2d2.urdf");
	printf("r2d2 #joints = %d\n", sim->getNumJoints(r2d2Uid));

	btVector3 basePosition = btVector3(0, 0, 0.5);
	btQuaternion baseOrientation = btQuaternion(0, 0, 0, 1);

	sim->resetBasePositionAndOrientation(r2d2Uid, basePosition, baseOrientation);

	while (sim->isConnected())
	{
		btVector3 basePos;
		btQuaternion baseOrn;
		sim->getBasePositionAndOrientation(r2d2Uid, basePos, baseOrn);
		printf("r2d2 basePosition = [%f,%f,%f]\n", basePos[0], basePos[1], basePos[2]);

		sim->stepSimulation();
	}
	delete sim;
}