~robot3d-team/robot3d/trunk

/*
 * Robot3D Physical realistic 3D simulator for robots
 *
 * @license GNU Lesser General Public License
 *
 * @author Lutz Winkler
 * @author Anne C. van Rossum
 */

// General libraries
#include <dlfcn.h>
#include <sys/stat.h>

// Delta3D libraries
#include <dtUtil/log.h>
#include <dtDAL/librarymanager.h>

// Robot3D files
#include <srCore/export.h>
#include <srCore/simulationEntryPoint.h>
#include <srCore/robotCreator.h>
#include <srCore/robot/robotActorBase.h>
#include <srCore/loadComponents.h>

/************************************************************************************************
 *                      Global properties
 ***********************************************************************************************/

#define TIME_SCALE 							0.1				//1
#define GRAVITY								200				//n*(5cm)/s²

/************************************************************************************************
 *                      Default strings / names / paths
 ***********************************************************************************************/

std::string DEFAULT_COMPONENT_PATH 		= "../data/components";
std::string DEFAULT_STAGEPROJECT_PATH	= "../data/StageProject";

/************************************************************************************************
 *                      Shortcuts
 ***********************************************************************************************/

using dtCore::RefPtr;
using namespace srCore;

/************************************************************************************************
 *                      C Hooks for Delta3d
 ***********************************************************************************************/

/**
 * This function is obligated by the Delta3D library. It makes it possible to dynamically load the
 * Robot3D library.
 */
extern "C" ROBOT_EXPORT dtGame::GameEntryPoint* CreateGameEntryPoint() {
	return new SimulationEntryPoint();
}

/**
 * This function is obligated by the Delta3D library. It makes it possible to dynamically unload the
 * Robot3D library.
 */
extern "C" ROBOT_EXPORT void DestroyGameEntryPoint(dtGame::GameEntryPoint* entryPoint) {
	delete entryPoint;
}

/************************************************************************************************
 *                      Implementation of SimulationEntryPoint
 ***********************************************************************************************/

/**
 * The constructor of the Robot3D simulator. It sets the default log level to LOG_INFO and
 * displays a welcome message.
 */
SimulationEntryPoint::SimulationEntryPoint() {
	dtUtil::Log::GetInstance().SetLogLevel(dtUtil::Log::LOG_INFO);
	LOG_ALWAYS("Welcome to the Robot3D simulator");

	osg::DisplaySettings::instance()->setNumMultiSamples(4);
}

/**
 * Destroy the entire Robot3D simulator.
 * @return the void
 * @todo Check what can be deallocated...
 */
SimulationEntryPoint::~SimulationEntryPoint() {
	LOG_INFO("Unload everything...");
}


/**
 * The start up routine. It loads a predefined XML configuration file and initiates the simulator
 * with the data found in it. It is not the idea that this function needs to know the different types
 * of robot, sensor, and actuator classes that can be defined. This needs to be registered by the
 * user.
 */
void SimulationEntryPoint::OnStartup(dtGame::GameApplication &app)
{
	//	app.GetGameManager()->DebugStatisticsTurnOn(true,true,1,true,"gameManagerDebugInfo.txt");
	bool withRobotControlEnabled = true;
	bool withGUI = false;
	bool freeCamState = true;
	int  robotFocus = 0;
	osg::Vec3f camPos;

	std::ostringstream msg;	msg.clear(); msg.str("");
	dtUtil::Log::GetInstance().SetLogLevel(dtUtil::Log::LOG_INFO);
	LOG_INFO("Beginning of Robot3D simulator startup");

	app.GetWindow()->SetWindowTitle("Robot3D Simulation");
	app.GetWindow()->ShowCursor(false);

	// Set stage project path
	std::string stageprojectPath=app.GetConfigPropertyValue("StageProject", DEFAULT_STAGEPROJECT_PATH);
	SimulationUtils::setStageProjectPath(stageprojectPath);

	msg.clear(); msg.str("");
	msg << "Set StageProject path to: " << SimulationUtils::getStageProjectPath();
	LOG_INFO(msg.str());


	// Register new message types
	SimulationMessageType::RegisterMessageTypes( app.GetGameManager()->GetMessageFactory() );

	LOG_WARNING("Load actors explicitly from libRobot3D.so library");
	dtDAL::LibraryManager::GetInstance().LoadActorRegistry("Robot3D");

	// Get configuration

	// audio
#ifdef AUDIO_ON
	dtAudio::AudioManager::Instantiate();
	dtAudio::AudioManager::GetInstance().Config();
#endif

	std::string file = srCore::SimulationUtils::getSceneFile();

	file = app.GetConfigPropertyValue("ScenarioFile", "../data/scenarios/scene.xml");
	srCore::SimulationUtils::setSceneFile(file);

	try {
		msg.clear(); msg.str("");
		msg << "Reading \"scene.xml\" file: " << srCore::SimulationUtils::getSceneFile();
		LOG_INFO(msg.str());
	} catch(std::runtime_error error) {
		LOG_ERROR("Configuration file does not exist!");
		return;
	}

	appConfig.readConfigFile(srCore::SimulationUtils::getSceneFile());

	msg.clear(); msg.str("");
	msg << "Map: " << appConfig.getMap();
	LOG_INFO(msg.str());
	msg.clear(); msg.str("");
	msg << "With Shadows: " << appConfig.getWithShadows();
	LOG_INFO(msg.str());
	msg.clear(); msg.str("");
	msg << "With Detailed Models: " << appConfig.getWithDetailedModels();
	LOG_INFO(msg.str());

	msg.clear(); msg.str("");
	msg << "Load robots...";
	LOG_INFO(msg.str());

	for (unsigned int i=0; i< appConfig.getRobots().size(); i++)
	{
		msg.clear(); msg.str("");
		msg << "Named " << appConfig.getRobots()[i].name << " with type " << appConfig.getRobots()[i].type;
		msg << " at position: [" << appConfig.getRobots()[i].pos.x() << "," <<
				appConfig.getRobots()[i].pos.y() << "," << appConfig.getRobots()[i].pos.z() << "]";
		msg << " oriented as: [" << appConfig.getRobots()[i].quat << "] " << " with joint angle: " << appConfig.getRobots()[i].jointAngles[0];
		LOG_INFO(msg.str());

	}

	msg.clear(); msg.str("");
	msg << "Load controllers...";
	LOG_INFO(msg.str());

	for (unsigned int i=0; i< appConfig.getControllers().size(); i++)
	{
		if (appConfig.getControllers().at(i).name == NULL) {
			LOG_WARNING("There seems to be something wrong with the vector with ControllerInitData");
			continue;
		}
		msg.clear(); msg.str("");
		msg << "A " << appConfig.getControllers()[i].type << " controller";
		if (strcmp(appConfig.getControllers()[i].name, "")) {
			msg << " named \"" << appConfig.getControllers()[i].name << "\"";
		}
		if (strncmp(appConfig.getControllers()[i].type, "Input", strlen("Input")) == 0)
		{
			withRobotControlEnabled = atof(appConfig.getControllers()[i].parameter);
			msg <<" withRobotControlEnabled="<< withRobotControlEnabled;

		}

		if (strncmp(appConfig.getControllers()[i].type, "GUI", strlen("GUI")) == 0)
			withGUI = true;

		if (strncmp(appConfig.getControllers()[i].type, "FreeCam", strlen("FreeCam")) == 0)
		{
			robotFocus = atoi(appConfig.getControllers()[i].robot);
			camPos = appConfig.getControllers()[i].pos;

			msg << " at position: [" << camPos.x() << "," << camPos.y() << "," << camPos.z() << "]";

			freeCamState = true;
		}
		else if (strncmp(appConfig.getControllers()[i].type, "FollowCam", strlen("FollowCam")) == 0)
		{
			robotFocus = atoi(appConfig.getControllers()[i].robot);
			camPos = appConfig.getControllers()[i].pos;

			msg << " at position: [" << camPos.x() << "," << camPos.y() << "," << camPos.z() << "]";

			freeCamState = false;
		}

		if (strcmp(appConfig.getControllers()[i].robot, ""))
			msg << " controlling robot \"" << appConfig.getControllers()[i].robot << "\" ";
		LOG_INFO(msg.str());

	}

	SimulationUtils::setDetailedGraphics(appConfig.getWithDetailedModels());
	SimulationUtils::setWithShadows(appConfig.getWithShadows());
	SimulationUtils::setCollisionGeometryDetails(appConfig.getCollisionGeometryDetails());
	SimulationUtils::setConnectorsAsJoints(appConfig.getConnectorsAsJoints());
	SimulationUtils::setWithWheels(appConfig.getRobotsWithWheels());

	//Load Map and apply Light
	LOG_INFO("Set context, load all information from StageProject directory");
	dtDAL::Project::GetInstance().SetContext(SimulationUtils::getStageProjectPath());

	//Set log level temporary to WARNING to get less log messages
	dtUtil::Log::LogMessageType lmt = dtUtil::Log::GetInstance().GetLogLevel();
	if (lmt < dtUtil::Log::LOG_WARNING)
		dtUtil::Log::GetInstance().SetLogLevel(dtUtil::Log::LOG_WARNING);
	dtDAL::Map &map = dtDAL::Project::GetInstance().GetMap(appConfig.getMap());
	dtUtil::Log::GetInstance().SetLogLevel(lmt);

	dtDAL::Project::GetInstance().LoadMapIntoScene(map, app.GetGameManager()->GetScene(), false);
	LOG_INFO("Map loaded");

	// SetOSGParameters
	app.GetView(0)->GetDatabasePager()->SetTargetFrameRate(60);		//20

	SetPhysicalParameters(app);

	//----------------------Components----------------------------------------------------------------------------------------

	LOG_INFO("Creating components");

	//Add DefaultMessageProcessor
	dtGame::DefaultMessageProcessor *dmp = new dtGame::DefaultMessageProcessor("DefaultMessageProcessor");
	app.GetGameManager()->AddComponent(*dmp,dtGame::GameManager::ComponentPriority::HIGHEST);

	//Add Arena Component - doing all the important stuff
	dtCore::RefPtr<SimulationArenaComponent> arenaComp = new SimulationArenaComponent(SimulationArenaComponent::DEFAULT_NAME);
	app.GetGameManager()->AddComponent(*arenaComp, dtGame::GameManager::ComponentPriority::LOWEST);

	arenaComp->createRobotsFromSceneFile(appConfig);

	//Add all GUI elements
	guiComp = NULL;
	if (withGUI) {
		LoadGUIComponents(app);
	}

	//Add Camera Component
	dtCore::RefPtr<SimulationCameraController> cameraContr = new SimulationCameraController(
			"SimulationCameraController", camPos, freeCamState);
	app.GetGameManager()->AddComponent(*cameraContr, dtGame::GameManager::ComponentPriority::LOWER);
	if (withGUI) {
		cameraContr->init(guiComp->GetMouse(), guiComp->GetKeyboard());
	} else {
		cameraContr->init(app.GetMouse(), app.GetKeyboard());
	}

#ifdef USE_SEPARATE_GUI
	cameraContr->setViewport(false);
#else
	cameraContr->setViewport(true);
#endif

	//Add Input Component
	dtCore::RefPtr<SimulationInputComponent> inputComp = new SimulationInputComponent("SimulationInputComponent", withRobotControlEnabled);
	app.GetGameManager()->AddComponent(*inputComp, dtGame::GameManager::ComponentPriority::LOWER);

	//-----------------------------------------------------------------------------------------------------------------------
	// Shadows

	if (SimulationUtils::getWithShadows()) {
		app.GetView(0)->GetOsgViewerView()->setSceneData(srCore::SimulationUtils::getShadowScene().get());
		app.GetView(0)->GetOsgViewerView()->init();
	}

	std::vector<dtGame::GMComponent*> allComp;
	app.GetGameManager()->GetAllComponents(allComp);

	for (unsigned int i=0; i< allComp.size(); i++) {
		if (dynamic_cast<dtCore::Light*>(allComp[i])) {
			LOG_INFO("Found light (as component)");
		}
	}

	LOG_INFO("Load all plugins from ComponentPath");
	loadComp = new LoadComponents();
	std::string comp_path = app.GetConfigPropertyValue("ComponentPath", DEFAULT_COMPONENT_PATH);
	loadComp->SetLibPath(comp_path);
	loadComp->GetLibraries();
	loadComp->LoadLibs(app);

	LOG_INFO("End of Robot3D simulator startup");

}

/**
 * Sets a bunch of parameters for the physics engine.
 *
 * @todo Move Physics related stuff to a separate component. The component may be build on
 * dtPhysics, so it enables us to switch physics engines.
 */
void SimulationEntryPoint::SetPhysicalParameters(dtGame::GameApplication &app) {

	//Setting Collision NearCallback function 
//	void* data = NULL;
//	app.GetScene()->SetUserCollisionCallback (SimulationUtils::NearCallback, data);

	//Setting PhysicsStepSize and TargetFrameRate for stabilizing the simulation environment:
	app.GetGameManager()->GetScene().SetPhysicsStepSize(0.05);		//0.05

	// The Constraint Force Mixing is invented by the ODE guys. It allows for relaxing of the
	// constraint equation for joints. The original hard constraint is 0.
	// @see <a>http://opende.sourceforge.net/wiki/index.php/Manual_%28Concepts%29#Constraint_Force_Mixing_.28CFM.29</a>
	dWorldSetCFM(app.GetGameManager()->GetScene().GetWorldID(), 0.005);			//0.05

	// The Error Reduction Parameter is also invented by the ODE guys. It is for example
	// possible to set the position of the multiple bodies that make up a robot in slightly
	// wrong relative locations. Each joint can apply a force to bring its bodies back into
	// the right positions.
	// <a>@see http://opende.sourceforge.net/wiki/index.php/Manual_%28Concepts%29#Joint_error_and_the_error_reduction_parameter_.28ERP.29</a>
	dWorldSetERP(app.GetGameManager()->GetScene().GetWorldID(), 0.9);			//0.9

	//Gravity
	app.GetGameManager()->GetScene().SetGravity(0.f, 0.f, -GRAVITY);

	// If you see a robot slowly sinking into the surface and then suddenly popping out in an
	// incredibly high speed, that speed is set by this parameter. Of course, we normally don't
	// want that to happen at all...
	dWorldSetContactMaxCorrectingVel(app.GetGameManager()->GetScene().GetWorldID(), 250);			//200

	// This parameter tells how much the robots can sink into the ground, as described
	// by the ODE lib, increasing this to some small value (e.g. 0.001) can help prevent
	// jittering problems due to contacts being repeatedly made and broken.
	dWorldSetContactSurfaceLayer (app.GetGameManager()->GetScene().GetWorldID(), 0.005);			//0.001 (0.003)

	app.GetGameManager()->ChangeTimeSettings(app.GetGameManager()->GetSimulationTime(), TIME_SCALE,
			app.GetGameManager()->GetSimulationClockTime());

}

/**
 * Loads GUI components in the same dtCore::DeltaWin window, but using a different camera, a
 * different view and a different scene.
 * The GUI adds the information panel at the right. Where you will see information about the
 * selected robot, the output of its sensors and the input to its actuators. It also adds a
 * menu at the top to select certain components in the scene.
 * @todo Freecam state or not is not the business of the simulationguicomponent
 * @param app
 */
void SimulationEntryPoint::LoadGUIComponents(dtGame::GameApplication &app) {
	// The default application view contains everything
	dtCore::RefPtr<dtCore::View> appView = app.GetView();
	dtCore::RefPtr<dtCore::Camera> appCamera = app.GetCamera();
	dtCore::RefPtr<dtCore::Scene> appScene = app.GetScene();
	dtCore::RefPtr<dtCore::DeltaWin> appWindow = app.GetWindow();
#ifdef USE_SEPARATE_GUI
	// In this case not everything is added to the default application view
	// The gui components are maintained in a separate guiView and guiScene and can be seen through a guiCamera
	// This makes it possible for robots NOT to notice them
	dtCore::RefPtr<dtCore::View> guiView;
	dtCore::RefPtr<dtCore::Camera> guiCamera;
	dtCore::RefPtr<dtCore::Scene> guiScene;

	//Create also a new view and a new scene
	LOG_INFO("Create a new view and a new scene for the GUI");
	guiView = new dtCore::View("GUI View");
	guiCamera = new dtCore::Camera("GUI Camera");
	guiScene = new dtCore::Scene("GUI Scene");

	// Set some default parameters
	dtCore::Transform transform(20.0f, -30.0f, 15.0f);
	guiCamera->SetTransform(transform);
	guiCamera->SetClearColor(0.f, 0.f, 0.f, 0.f);
	guiView->SetCamera(guiCamera);

	// Add the view to the application
	app.AddView(*guiView);
	guiCamera->SetWindow(appWindow);
	guiView->SetScene(guiScene);
	guiCamera->GetOSGCamera()->setClearMask( GL_DEPTH_BUFFER_BIT );

	//Load the CEGUI window layout
	LOG_INFO("Load CEGUI window layout");
	guiComp = new SimulationGUIComponent("SimulationGUIComponent", *appWindow,
			*guiView->GetKeyboard(), *guiView->GetMouse());

	// Create a new camera that sees the GUI
	LOG_INFO("Create a new camera that sees the GUI (the default one will only see the scene with robots)");
	guiScene->AddDrawable(guiComp->GetGUIDrawable());

	// Adjust render order, so the GUI is rendered after the overall scene
	app.GetView()->SetRenderOrder(1);
	guiView->SetRenderOrder(app.GetView()->GetRenderOrder() + 1);
	CEGUI::System::getSingleton().getGUISheet()->setAlpha(0.0f);

	// Add component to game manager for later retrieval
	app.GetGameManager()->AddComponent(*guiComp, dtGame::GameManager::ComponentPriority::LOWER);
#else
	//Load the CEGUI window layout
	LOG_INFO("Load CEGUI window layout");
	guiComp = new SimulationGUIComponent("SimulationGUIComponent", *appWindow,
			*appView->GetKeyboard(), *appView->GetMouse());

	// Add to scene
	app.GetScene()->AddDrawable(guiComp->GetGUIDrawable());
	guiComp->GetGUIDrawable()->GetOSGNode()->setNodeMask(0x1);

	app.GetGameManager()->AddComponent(*guiComp, dtGame::GameManager::ComponentPriority::LOWER);
#endif
}