bullet3

#ifdef BT_ENABLE_MUJOCO
#include "MuJoCoPhysicsServerCommandProcessor.h"
#include "mujoco.h"
#include <stdio.h>
#include "../SharedMemoryCommands.h"
#include "LinearMath/btQuickprof.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "LinearMath/btMinMax.h"
#include "Bullet3Common/b3FileUtils.h"
#include "../../Utils/b3ResourcePath.h"

struct MuJoCoPhysicsServerCommandProcessorInternalData
{
	bool m_isConnected;
	bool m_verboseOutput;
	double m_physicsDeltaTime;
	int m_numSimulationSubSteps;

	mjModel* m_mujocoModel;
	mjData* m_mujocoData;

	b3AlignedObjectArray<int> m_mjcfRecentLoadedBodies;
	MuJoCoPhysicsServerCommandProcessorInternalData()
		: m_isConnected(false),
		  m_verboseOutput(false),
		  m_mujocoModel(0),
		  m_mujocoData(0),
		  m_physicsDeltaTime(1. / 240.),
		  m_numSimulationSubSteps(0)
	{
	}
};

MuJoCoPhysicsServerCommandProcessor::MuJoCoPhysicsServerCommandProcessor()
{
	m_data = new MuJoCoPhysicsServerCommandProcessorInternalData;
}

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

bool MuJoCoPhysicsServerCommandProcessor::connect()
{
	if (m_data->m_isConnected)
	{
		printf("already connected\n");
		return true;
	}

	printf("MuJoCo Pro library version %.2lf\n", 0.01 * mj_version());
	if (mjVERSION_HEADER != mj_version())
		mju_error("Headers and library have different versions");

	// activate MuJoCo license
	int result = mj_activate("mjkey.txt");
	if (result == 1)
	{
		m_data->m_isConnected = true;
		return true;
	}

	return false;
}

void MuJoCoPhysicsServerCommandProcessor::resetSimulation()
{
	if (m_data->m_mujocoModel)
	{
		mj_deleteModel(m_data->m_mujocoModel);
		m_data->m_mujocoModel = 0;
	}
	if (m_data->m_mujocoData)
	{
		mj_deleteData(m_data->m_mujocoData);
		m_data->m_mujocoData = 0;
	}
}

void MuJoCoPhysicsServerCommandProcessor::disconnect()
{
	resetSimulation();

	m_data->m_isConnected = false;
}

bool MuJoCoPhysicsServerCommandProcessor::isConnected() const
{
	return m_data->m_isConnected;
}

bool MuJoCoPhysicsServerCommandProcessor::processCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	//	BT_PROFILE("processCommand");

	int sz = sizeof(SharedMemoryStatus);
	int sz2 = sizeof(SharedMemoryCommand);

	bool hasStatus = false;

	serverStatusOut.m_type = CMD_INVALID_STATUS;
	serverStatusOut.m_numDataStreamBytes = 0;
	serverStatusOut.m_dataStream = 0;

	//consume the command
	switch (clientCmd.m_type)
	{
		case CMD_REQUEST_INTERNAL_DATA:
		{
			hasStatus = processRequestInternalDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		};

		case CMD_SYNC_BODY_INFO:
		{
			hasStatus = processSyncBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_SYNC_USER_DATA:
		{
			hasStatus = processSyncUserDataCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_LOAD_MJCF:
		{
			hasStatus = processLoadMJCFCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_REQUEST_BODY_INFO:
		{
			hasStatus = processRequestBodyInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}
		case CMD_STEP_FORWARD_SIMULATION:
		{
			hasStatus = processForwardDynamicsCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
		{
			hasStatus = processSendPhysicsParametersCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		};

		case CMD_REQUEST_ACTUAL_STATE:
		{
			hasStatus = processRequestActualStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		case CMD_RESET_SIMULATION:
		{
			hasStatus = processResetSimulationCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
			break;
		}

		default:
		{
			BT_PROFILE("CMD_UNKNOWN");
			printf("Unknown command encountered: %d", clientCmd.m_type);
			SharedMemoryStatus& serverCmd = serverStatusOut;
			serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED;
			hasStatus = true;
		}

#if 0
	case CMD_STATE_LOGGING:
		{
			hasStatus = processStateLoggingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_SET_VR_CAMERA_STATE:
		{
			hasStatus = processSetVRCameraStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_VR_EVENTS_DATA:
		{
			hasStatus = processRequestVREventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};
	case CMD_REQUEST_MOUSE_EVENTS_DATA:
		{
			hasStatus = processRequestMouseEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};
	case CMD_REQUEST_KEYBOARD_EVENTS_DATA:
		{
			hasStatus = processRequestKeyboardEventsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};

	case CMD_REQUEST_RAY_CAST_INTERSECTIONS:
		{

			hasStatus = processRequestRaycastIntersectionsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};
	case CMD_REQUEST_DEBUG_LINES:
		{
			hasStatus = processRequestDebugLinesCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}

	case CMD_REQUEST_CAMERA_IMAGE_DATA:
		{
			hasStatus = processRequestCameraImageCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	
	case CMD_REQUEST_BODY_INFO:
		{
			hasStatus = processRequestBodyInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_SAVE_WORLD:
		{
			hasStatus = processSaveWorldCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_LOAD_SDF:
		{
			hasStatus = processLoadSDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CREATE_COLLISION_SHAPE:
		{
			hasStatus = processCreateCollisionShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CREATE_VISUAL_SHAPE:
		{
			hasStatus = processCreateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CREATE_MULTI_BODY:
		{
			hasStatus = processCreateMultiBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_SET_ADDITIONAL_SEARCH_PATH:
		{
			hasStatus = processSetAdditionalSearchPathCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_LOAD_URDF:
		{
			hasStatus = processLoadURDFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_LOAD_SOFT_BODY:
		{
			hasStatus = processLoadSoftBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CREATE_SENSOR:
		{
			hasStatus = processCreateSensorCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_PROFILE_TIMING:
		{
			hasStatus = processProfileTimingCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}

	case CMD_SEND_DESIRED_STATE:
		{
			hasStatus = processSendDesiredStateCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_COLLISION_INFO:
		{
			hasStatus = processRequestCollisionInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}

	
	
	case CMD_CHANGE_DYNAMICS_INFO:
		{
			hasStatus = processChangeDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		};
	case CMD_GET_DYNAMICS_INFO:
		{
			hasStatus = processGetDynamicsInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}

	case CMD_REQUEST_PHYSICS_SIMULATION_PARAMETERS:
		{
			hasStatus = processRequestPhysicsSimulationParametersCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}

	
	case CMD_INIT_POSE:
		{
			hasStatus = processInitPoseCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	
	case CMD_CREATE_RIGID_BODY:
		{
			hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CREATE_BOX_COLLISION_SHAPE:
		{
			//for backward compatibility, CMD_CREATE_BOX_COLLISION_SHAPE is the same as CMD_CREATE_RIGID_BODY
			hasStatus = processCreateRigidBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_PICK_BODY:
		{
			hasStatus = processPickBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_MOVE_PICKED_BODY:
		{
			hasStatus = processMovePickedBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REMOVE_PICKING_CONSTRAINT_BODY:
		{
			hasStatus = processRemovePickingConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_AABB_OVERLAP:
		{
			hasStatus = processRequestAabbOverlapCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_OPENGL_VISUALIZER_CAMERA:
		{
			hasStatus = processRequestOpenGLVisualizeCameraCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CONFIGURE_OPENGL_VISUALIZER:
		{
			hasStatus = processConfigureOpenGLVisualizerCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_CONTACT_POINT_INFORMATION:
		{
			hasStatus = processRequestContactpointInformationCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CALCULATE_INVERSE_DYNAMICS:
		{
			hasStatus = processInverseDynamicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CALCULATE_JACOBIAN:
		{
			hasStatus = processCalculateJacobianCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CALCULATE_MASS_MATRIX:
		{
			hasStatus = processCalculateMassMatrixCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_APPLY_EXTERNAL_FORCE:
		{
			hasStatus = processApplyExternalForceCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REMOVE_BODY:
		{
			hasStatus = processRemoveBodyCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_USER_CONSTRAINT:
		{
			hasStatus = processCreateUserConstraintCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CALCULATE_INVERSE_KINEMATICS:
		{
			hasStatus = processCalculateInverseKinematicsCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_VISUAL_SHAPE_INFO:
		{
			hasStatus = processRequestVisualShapeInfoCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REQUEST_COLLISION_SHAPE_INFO:
	{
		hasStatus = processRequestCollisionShapeInfoCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}
	case CMD_UPDATE_VISUAL_SHAPE:
		{
			hasStatus = processUpdateVisualShapeCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_CHANGE_TEXTURE:
		{
			hasStatus = processChangeTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_LOAD_TEXTURE:
		{
			hasStatus = processLoadTextureCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_RESTORE_STATE:
	{
		hasStatus = processRestoreStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}

	case CMD_SAVE_STATE:
	{
		hasStatus = processSaveStateCommand(clientCmd, serverStatusOut, bufferServerToClient, bufferSizeInBytes);
		break;
	}

	case CMD_LOAD_BULLET:
		{
			hasStatus = processLoadBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_SAVE_BULLET:
		{
			hasStatus = processSaveBulletCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_LOAD_MJCF:
		{
			hasStatus = processLoadMJCFCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_USER_DEBUG_DRAW:
		{
			hasStatus = processUserDebugDrawCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;        
		}
	case CMD_CUSTOM_COMMAND:
		{
			hasStatus = processCustomCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	
	case CMD_REQUEST_USER_DATA:
		{
			hasStatus = processRequestUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_ADD_USER_DATA:
		{
			hasStatus = processAddUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
	case CMD_REMOVE_USER_DATA:
		{
			hasStatus = processRemoveUserDataCommand(clientCmd,serverStatusOut,bufferServerToClient, bufferSizeInBytes);
			break;
		}
#endif
	};

	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processRequestInternalDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_REQUEST_INTERNAL_DATA");
	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_REQUEST_INTERNAL_DATA_COMPLETED;
	serverCmd.m_numDataStreamBytes = 0;
	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processSyncBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_SYNC_BODY_INFO");
	int actualNumBodies = 0;
	serverStatusOut.m_sdfLoadedArgs.m_numBodies = 0;
	serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
	serverStatusOut.m_type = CMD_SYNC_BODY_INFO_COMPLETED;
	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processSyncUserDataCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_SYNC_USER_DATA");
	int numIdentifiers = 0;
	serverStatusOut.m_syncUserDataArgs.m_numUserDataIdentifiers = numIdentifiers;
	serverStatusOut.m_type = CMD_SYNC_USER_DATA_COMPLETED;
	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processLoadMJCFCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;

	BT_PROFILE("CMD_LOAD_MJCF");
	serverStatusOut.m_type = CMD_MJCF_LOADING_FAILED;
	const MjcfArgs& mjcfArgs = clientCmd.m_mjcfArguments;
	if (m_data->m_verboseOutput)
	{
		printf("Processed CMD_LOAD_MJCF:%s", mjcfArgs.m_mjcfFileName);
	}
	bool useMultiBody = (clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (mjcfArgs.m_useMultiBody != 0) : true;
	int flags = 0;
	if (clientCmd.m_updateFlags & URDF_ARGS_HAS_CUSTOM_URDF_FLAGS)
	{
		flags |= clientCmd.m_mjcfArguments.m_flags;
	}

	const char* fileName = mjcfArgs.m_mjcfFileName;

	if (strlen(fileName) > 0)
	{
		char relativeFileName[1024];
		b3FileUtils fu;
		//bool fileFound = fu.findFile(fileName, relativeFileName, 1024);
		bool fileFound = (b3ResourcePath::findResourcePath(fileName, relativeFileName, 1024) > 0);
		if (!fileFound)
		{
			printf("MJCF file not found: %s\n", fileName);
		}
		else
		{
			int maxPathLen = 1024;
			char pathPrefix[1024];
			fu.extractPath(relativeFileName, pathPrefix, maxPathLen);

			{
				char error[1000] = "could not load binary model";
				mjModel* mnew = 0;
				if (strlen(relativeFileName) > 4 && !strcmp(relativeFileName + strlen(relativeFileName) - 4, ".mjb"))
				{
					mnew = mj_loadModel(relativeFileName, 0);
				}
				else
				{
					mnew = mj_loadXML(relativeFileName, 0, error, 1000);
					if (mnew)
					{
						//replace old one for now
						if (m_data->m_mujocoModel)
						{
							mj_deleteModel(m_data->m_mujocoModel);
						}
						if (m_data->m_mujocoData)
						{
							mj_deleteData(m_data->m_mujocoData);
						}
						m_data->m_mujocoModel = mnew;
						m_data->m_mujocoData = mj_makeData(m_data->m_mujocoModel);
						mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
					}
				}
				if (!mnew)
				{
					printf("%s\n", error);
				}
				else
				{
					int maxBodies = btMin(MAX_SDF_BODIES, mnew->nbody);

					serverStatusOut.m_sdfLoadedArgs.m_numBodies = maxBodies;
					for (int i = 0; i < maxBodies; i++)
					{
						serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = i;
					}
					serverStatusOut.m_sdfLoadedArgs.m_numUserConstraints = 0;
					serverStatusOut.m_type = CMD_MJCF_LOADING_COMPLETED;
				}
			}
		}
	}

	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processRequestBodyInfoCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_REQUEST_BODY_INFO");

	const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs;
	//stream info into memory
	int streamSizeInBytes = 0;  //createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes);

	serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED;
	serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId;
	serverStatusOut.m_dataStreamArguments.m_bodyName[0] = 0;

	if (m_data->m_mujocoModel && sdfInfoArgs.m_bodyUniqueId >= 0 && sdfInfoArgs.m_bodyUniqueId < m_data->m_mujocoModel->nbody)
	{
		const char* name = m_data->m_mujocoModel->names + m_data->m_mujocoModel->name_bodyadr[sdfInfoArgs.m_bodyUniqueId];
		strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, name);
	}

	serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;

	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processForwardDynamicsCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;

	BT_PROFILE("CMD_STEP_FORWARD_SIMULATION");

	if (m_data->m_mujocoModel)
	{
		if (m_data->m_verboseOutput)
		{
			b3Printf("Step simulation request");
			b3Printf("CMD_STEP_FORWARD_SIMULATION clientCmd = %d\n", clientCmd.m_sequenceNumber);
		}

		btScalar deltaTimeScaled = m_data->m_physicsDeltaTime;

		if (m_data->m_numSimulationSubSteps > 0)
		{
			for (int i = 0; i < m_data->m_numSimulationSubSteps; i++)
			{
				m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps;
				mj_step(m_data->m_mujocoModel, m_data->m_mujocoData);
				mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
			}
		}
		else
		{
			m_data->m_mujocoModel->opt.timestep = m_data->m_physicsDeltaTime;
			mj_step(m_data->m_mujocoModel, m_data->m_mujocoData);
			mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
		}
	}
	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED;
	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;

	BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");

	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_DELTA_TIME)
	{
		m_data->m_physicsDeltaTime = clientCmd.m_physSimParamArgs.m_deltaTime;
	}

#if 0
	if (clientCmd.m_updateFlags & SIM_PARAM_ENABLE_CONE_FRICTION)
	{
		if (clientCmd.m_physSimParamArgs.m_enableConeFriction)
		{
			m_data->m_dynamicsWorld->getSolverInfo().m_solverMode &=~SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
		} else
		{
			m_data->m_dynamicsWorld->getSolverInfo().m_solverMode |=SOLVER_DISABLE_IMPLICIT_CONE_FRICTION;
		}
	}
	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DETERMINISTIC_OVERLAPPING_PAIRS)
	{
		m_data->m_dynamicsWorld->getDispatchInfo().m_deterministicOverlappingPairs = (clientCmd.m_physSimParamArgs.m_deterministicOverlappingPairs!=0);
	}

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CCD_ALLOWED_PENETRATION)
	{
		m_data->m_dynamicsWorld->getDispatchInfo().m_allowedCcdPenetration = clientCmd.m_physSimParamArgs.m_allowedCcdPenetration;
	}
	
	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_JOINT_FEEDBACK_MODE)
	{
		gJointFeedbackInWorldSpace = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_WORLD_SPACE)!=0;
		gJointFeedbackInJointFrame = (clientCmd.m_physSimParamArgs.m_jointFeedbackMode&JOINT_FEEDBACK_IN_JOINT_FRAME)!=0;
	}

	
	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_REAL_TIME_SIMULATION)
	{
		m_data->m_useRealTimeSimulation = (clientCmd.m_physSimParamArgs.m_useRealTimeSimulation!=0);
	}
					
	//see 
	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS)
	{
		//these flags are for internal/temporary/easter-egg/experimental demo purposes, use at own risk
		gInternalSimFlags = clientCmd.m_physSimParamArgs.m_internalSimFlags;
	}

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_GRAVITY)
	{
		btVector3 grav(clientCmd.m_physSimParamArgs.m_gravityAcceleration[0],
						clientCmd.m_physSimParamArgs.m_gravityAcceleration[1],
						clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]);
		this->m_data->m_dynamicsWorld->setGravity(grav);
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
		m_data->m_dynamicsWorld->getWorldInfo().m_gravity=grav;

#endif
		if (m_data->m_verboseOutput)
		{
			b3Printf("Updated Gravity: %f,%f,%f",grav[0],grav[1],grav[2]);
		}

	}
	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations;
	}

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_SOLVER_RESIDULAL_THRESHOLD)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = clientCmd.m_physSimParamArgs.m_solverResidualThreshold;
	}
	

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD)
	{
		gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold;
	}
	
	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_SLOP)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = clientCmd.m_physSimParamArgs.m_contactSlop;
	}

	if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_SAT)
	{
		m_data->m_dynamicsWorld->getDispatchInfo().m_enableSatConvex = clientCmd.m_physSimParamArgs.m_enableSAT!=0;
	}


	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE)
	{
		m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode;
	}

	if (clientCmd.m_updateFlags & SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulse = clientCmd.m_physSimParamArgs.m_useSplitImpulse;
	}
	if (clientCmd.m_updateFlags &SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD)
	{
		m_data->m_dynamicsWorld->getSolverInfo().m_splitImpulsePenetrationThreshold = clientCmd.m_physSimParamArgs.m_splitImpulsePenetrationThreshold;
	}
					

    if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_NUM_SIMULATION_SUB_STEPS)
    {
        m_data->m_numSimulationSubSteps = clientCmd.m_physSimParamArgs.m_numSimulationSubSteps;
    }

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_CONTACT_ERP)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = clientCmd.m_physSimParamArgs.m_defaultContactERP;
    }

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_NON_CONTACT_ERP)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_erp = clientCmd.m_physSimParamArgs.m_defaultNonContactERP;
    }

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_ERP)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = clientCmd.m_physSimParamArgs.m_frictionERP;
    }

    if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_GLOBAL_CFM)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_globalCfm = clientCmd.m_physSimParamArgs.m_defaultGlobalCFM;
    }

    if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DEFAULT_FRICTION_CFM)
    {
		m_data->m_dynamicsWorld->getSolverInfo().m_frictionCFM = clientCmd.m_physSimParamArgs.m_frictionCFM;
    }

	if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_RESTITUTION_VELOCITY_THRESHOLD)
    {
        m_data->m_dynamicsWorld->getSolverInfo().m_restitutionVelocityThreshold = clientCmd.m_physSimParamArgs.m_restitutionVelocityThreshold;
    }

					

	if (clientCmd.m_updateFlags&SIM_PARAM_ENABLE_FILE_CACHING)
    {
		b3EnableFileCaching(clientCmd.m_physSimParamArgs.m_enableFileCaching);
    }

#endif

	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processRequestActualStateCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;

	if (m_data->m_mujocoModel)
	{
		BT_PROFILE("CMD_REQUEST_ACTUAL_STATE");
		if (m_data->m_verboseOutput)
		{
			b3Printf("Sending the actual state (Q,U)");
		}
		int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;

		if (bodyUniqueId >= 0 && bodyUniqueId < m_data->m_mujocoModel->nbody)
		{
			SharedMemoryStatus& serverCmd = serverStatusOut;
			serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;

			serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
			serverCmd.m_sendActualStateArgs.m_numLinks = 0;  //todo body->m_multiBody->getNumLinks();

			int totalDegreeOfFreedomQ = 0;
			int totalDegreeOfFreedomU = 0;

			if (serverCmd.m_sendActualStateArgs.m_numLinks >= MAX_DEGREE_OF_FREEDOM)
			{
				serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_FAILED;
				hasStatus = true;
				return hasStatus;
			}

			bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS) != 0);
			bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY) != 0);

			if (computeForwardKinematics || computeLinkVelocities)
			{
				//todo:check this
				mj_forward(m_data->m_mujocoModel, m_data->m_mujocoData);
			}

			//always add the base, even for static (non-moving objects)
			//so that we can easily move the 'fixed' base when needed
			//do we don't use this conditional "if (!mb->hasFixedBase())"
			{
				int rootLink = 0;  //todo check
				int type = (m_data->m_mujocoModel->jnt_type + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
				//assume mjJNT_FREE?
				int qposAdr = (m_data->m_mujocoModel->jnt_qposadr + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[rootLink];
				mjtNum* pos = m_data->m_mujocoData->xipos + bodyUniqueId * 3;

				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[2] = 0;

				mjtNum* orn = m_data->m_mujocoData->xquat + bodyUniqueId * 4;
				mjtNum* cvel = m_data->m_mujocoData->cvel + bodyUniqueId * 6;

				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[3] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[4] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[5] = 0;
				serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[6] = 1;

				//base position in world space, carthesian
				serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = pos[0];
				serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = pos[1];
				serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = pos[2];

				//base orientation, quaternion x,y,z,w, in world space, carthesian
				serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = orn[0];
				serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = orn[1];
				serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = orn[2];
				serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = orn[3];
				totalDegreeOfFreedomQ += 7;  //pos + quaternion

				//base linear velocity (in world space, carthesian)
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = cvel[3];  //mb->getBaseVel()[0];
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = cvel[4];  //mb->getBaseVel()[1];
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = cvel[5];  //mb->getBaseVel()[2];

				//base angular velocity (in world space, carthesian)
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = cvel[0];  //mb->getBaseOmega()[0];
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = cvel[1];  //mb->getBaseOmega()[1];
				serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = cvel[2];  //mb->getBaseOmega()[2];
				totalDegreeOfFreedomU += 6;                                      //3 linear and 3 angular DOF
			}

			//btAlignedObjectArray<btVector3> omega;
			//btAlignedObjectArray<btVector3> linVel;

			int numLinks = m_data->m_mujocoModel->body_jntnum[bodyUniqueId];
			for (int l = 0; l < numLinks; l++)
			{
				int type = (m_data->m_mujocoModel->jnt_type + m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l];
				//int type=(m_data->m_mujocoModel->jnt_type+m_data->m_mujocoModel->body_jntnum[bodyUniqueId])[l];

#if 0
				mjtNum* xpos = 
				for (int d=0;d<mb->getLink(l).m_posVarCount;d++)
				{
					serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = 0;
				}
				for (int d=0;d<mb->getLink(l).m_dofCount;d++)
				{
					serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = 0;
				}

				if (0 == mb->getLink(l).m_jointFeedback)
				{
					for (int d=0;d<6;d++)
					{
						serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+d]=0;
					}
				} else
				{

					serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = 0;
					serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = 0;
					serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = 0;

					serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = 0;
					serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = 0;
					serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = 0;
				}

				serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
#if 0
				if (supportsJointMotor(mb,l))
				{
					if (motor && m_data->m_physicsDeltaTime>btScalar(0))
					{
						serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
					}
				}
#endif
				//btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
				//btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();

				//btVector3 linkCOMOrigin =  mb->getLink(l).m_cachedWorldTransform.getOrigin();
				//btQuaternion linkCOMRotation =  mb->getLink(l).m_cachedWorldTransform.getRotation();

				serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = 0;//linkCOMOrigin.getX();
				serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = 0;//linkCOMOrigin.getY();
				serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = 0;//linkCOMOrigin.getZ();
				serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = 0;//linkCOMRotation.x();
				serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = 0;//linkCOMRotation.y();
				serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = 0;//linkCOMRotation.z();
				serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = 1;//linkCOMRotation.w();

#if 0
				btVector3 worldLinVel(0,0,0);
				btVector3 worldAngVel(0,0,0);
								
				if (computeLinkVelocities)
				{
					const btMatrix3x3& linkRotMat = mb->getLink(l).m_cachedWorldTransform.getBasis();
					worldLinVel = linkRotMat * linVel[l+1];
					worldAngVel = linkRotMat * omega[l+1];
				}
#endif
				serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+0] = 0;//worldLinVel[0];
				serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+1] = 0;//worldLinVel[1];
				serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+2] = 0;//worldLinVel[2];
				serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+3] = 0;//worldAngVel[0];
				serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+4] = 0;//worldAngVel[1];
				serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l*6+5] = 0;//worldAngVel[2];
								
				serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = 0;//linkLocalInertialOrigin.getX();
				serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = 0;//linkLocalInertialOrigin.getY();
				serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = 0;//linkLocalInertialOrigin.getZ();

				serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = 0;//linkLocalInertialRotation.x();
				serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = 0;//linkLocalInertialRotation.y();
				serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = 0;//linkLocalInertialRotation.z();
				serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = 1;//linkLocalInertialRotation.w();
#endif
			}

			serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
			serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU;

			hasStatus = true;
		}
	}
	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::processResetSimulationCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	bool hasStatus = true;
	BT_PROFILE("CMD_RESET_SIMULATION");

	resetSimulation();

	SharedMemoryStatus& serverCmd = serverStatusOut;
	serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED;
	return hasStatus;
}

bool MuJoCoPhysicsServerCommandProcessor::receiveStatus(struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes)
{
	return false;
}

#endif  //BT_ENABLE_MUJOCO