#include <iostream>

#include <hrpUtil/Tvmet3d.h>
#include <hrpModel/Body.h>
#include <hrpModel/Link.h>
#include <hrpModel/ModelLoaderUtil.h>

#include <hrpCorba/ModelLoader.hh>
#include <hrpCorba/DynamicsSimulator.hh>
#include "hrpCorba/CollisionDetector.hh"

#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

// OpenHRP3 補助関数
template <typename X, typename X_ptr>
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt);

static void getTransformData(OpenHRP::DynamicsSimulator_var dynamicsSimulator, const char* charname, const char* linkname, hrp::Vector3& p, hrp::Matrix33& R);
void getBodyData(hrp::BodyPtr body,  OpenHRP::DynamicsSimulator_var dynamicsSimulator);
void setBodyData(hrp::BodyPtr body,  OpenHRP::DynamicsSimulator_var dynamicsSimulator);

// ロボット描写
void drawLink(hrp::Link* link);
void drawLinkPoint(hrp::Link* link);
void drawRobot(hrp::BodyPtr body);

// drawstuff 補助関数
void R9d_to_R12f(const double R9[9], float R12[12]);
void drawBox(const double pos[3], const double R[9], const double sides[3]);
void drawSphere(const double pos[3], const double R[9], const float radius);
void drawCylinder(const double pos[3], const double R[9], float length, float radius);
void drawCapsule(const double pos[3], const double R[9], float length, float radius);
void drawLine(const double pos1[3], const double pos2[3]);

using namespace std;
using namespace hrp;
using namespace OpenHRP;

OpenHRP::DynamicsSimulator_var dynamicsSimulator;
hrp::BodyPtr floorBody;
hrp::BodyPtr robotBody;
hrp::dvector ini_angle;	// ロボットの初期関節角度

void start()
{

}

void simLoop(int pause)
{
	double time;
	WorldState_var state;

	// ================== simulate one step ==============
	dynamicsSimulator->stepSimulation();
	dynamicsSimulator->getWorldState(state);
	time = state->time;	// 経過時間の取得

	// ------- データの取得 ---------
	// 床
	hrp::Vector3 floor_p;
	hrp::Matrix33 floor_R;
	getTransformData(dynamicsSimulator, floorBody->name().c_str(), floorBody->rootLink()->name.c_str(), floor_p, floor_R);

	// ロボット
	getBodyData(robotBody, dynamicsSimulator);
	robotBody->calcForwardKinematics(true, true);	// 位置、速度、加速度の更新

	// -------    描写     ---------
	// 床
	const double floor_sides[3] = {4.0, 4.0, 0.2};
	dsSetColor(0.0, 0.0, 1.0);
	drawBox(floor_p.data(), floor_R.data(), floor_sides);

	// ロボット本体
	drawRobot(robotBody);

	// ------ ロボットの制御 --------
	double Kq = 4000.0;		// 適当
	double Kdq = 200.0;

	// 初期姿勢を維持
	int nJ = robotBody->numJoints();
	for (int i = 0; i < nJ; ++i) {
		Link *joint = robotBody->joint(i);
		joint->u = - Kdq * joint->dq - Kq * (joint->q - ini_angle(i));
	}

	// ------ 動力学シミュレータにデータを設定 --------
	setBodyData(robotBody, dynamicsSimulator);

}

void command(int cmd)
{

}

int main(int argc, char *argv[])
{
	cout << "testHRP" << endl;

    const double timeStep = 0.002;
    const double world_gravity = 9.8;

	std::string model_floor_url = "/usr/share/OpenHRP-3.1/sample/model/floor.wrl";
	std::string model_body_url = "/usr/share/OpenHRP-3.1/sample/model/sample.wrl";

	// CORBA初期化
	CORBA::ORB_var orb;
	orb = CORBA::ORB_init(argc, argv);

	CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
	PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
	PortableServer::POAManager_var manager = rootPOA -> the_POAManager();

	CosNaming::NamingContext_var cxt;
	{
		CORBA::Object_var	nS = orb->resolve_initial_references("NameService");
		cxt = CosNaming::NamingContext::_narrow(nS);
	}

	// DynamicsSimulatorFactoryの取得
	OpenHRP::DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
	dynamicsSimulatorFactory = checkCorbaServer <DynamicsSimulatorFactory, DynamicsSimulatorFactory_var> ("DynamicsSimulatorFactory", cxt);

	if (CORBA::is_nil(dynamicsSimulatorFactory)) {
		std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
	}

//	DynamicsSimulator_var dynamicsSimulator = dynamicsSimulatorFactory->create();
	dynamicsSimulator = dynamicsSimulatorFactory->create();

	// ModelLoaderの取得
	ModelLoader_var modelLoader = checkCorbaServer <ModelLoader, ModelLoader_var> ("ModelLoader", cxt);

	if (CORBA::is_nil(modelLoader)) {
		std::cerr << "ModelLoader not found" << std::endl;
	}

    // モデルの読み込み
    BodyInfo_ptr info_floor = modelLoader->loadBodyInfo(model_floor_url.c_str());
    BodyInfo_ptr info_body = modelLoader->loadBodyInfo(model_body_url.c_str());
    dynamicsSimulator->registerCharacter(info_floor->name(), info_floor);
    dynamicsSimulator->registerCharacter(info_body->name(), info_body);

    // DynamicsSimulatorの初期化
    dynamicsSimulator->init(timeStep, DynamicsSimulator::RUNGE_KUTTA, DynamicsSimulator::ENABLE_SENSOR);

    // 重力ベクトルを設定
    OpenHRP::DblSequence3 g;
    g.length(3);
    g[0] = 0.0;
    g[1] = 0.0;
    g[2] = world_gravity;
    dynamicsSimulator->setGVector(g);

    // ロボットの初期位置と姿勢の設定
    hrp::Vector3  waist_p;
    hrp::Matrix33 waist_R;
    waist_p = 0, 0, 1.7135;
//    waist_p = 0, 0, 0.7135 + 0.01;
    waist_R = tvmet::identity<hrp::Matrix33>();
    OpenHRP::DblSequence trans;
    trans.length(12);
    for(int i=0; i<3; i++) trans[i]   = waist_p(i);
    for(int i=0; i<3; i++){
    	for(int j=0; j<3; j++) trans[3+3*i+j] = waist_R(i,j);
    }
    dynamicsSimulator->setCharacterLinkData( info_body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, trans );

    // 関節の初期角度 設定
    OpenHRP::DblSequence angle;
    angle.length(29);
    angle[0] = 0.0;         angle[1] = -0.0360373;  angle[2] = 0.0;         angle[3] = 0.0785047;
    angle[4] = -0.0424675;  angle[5] = 0.0;         angle[6] = 0.174533;    angle[7] = -0.00349066;
    angle[8] = 0.0;         angle[9] = -1.5708;     angle[10] = 0.0;        angle[11] = 0.0;
    angle[12] = 0.0;        angle[13] = 0.0;        angle[14] = -0.0360373; angle[15] = 0.0;
    angle[16] = 0.0785047;  angle[17] = -0.0424675; angle[18] = 0.0;        angle[19] = 0.174533;
    angle[20] = -0.00349066;angle[21] = 0.0;        angle[22] = -1.5708;    angle[23] = 0.0;
    angle[24] = 0.0;        angle[25] = 0.0;        angle[26] = 0.0;        angle[27] = 0.0;
    angle[28] = 0.0;
    dynamicsSimulator->setCharacterAllLinkData( info_body->name(), DynamicsSimulator::JOINT_VALUE, angle );

	// 初期関節角度の保存
	int nJ = 29; // 29はsample.wrlのnumJoints()
	ini_angle.resize(nJ);
	for (int i = 0; i < nJ; ++i) {
		ini_angle(i) = angle[i];
	}

    // 関節駆動モードの設定
    dynamicsSimulator->setCharacterAllJointModes(info_body->name(), DynamicsSimulator::TORQUE_MODE);
    //dynamicsSimulator->setCharacterAllJointModes(info_body->name(), DynamicsSimulator::HIGH_GAIN_MODE);

    // ロボットの初期姿勢を決定
    dynamicsSimulator->calcWorldForwardKinematics();

    // 衝突検出ペアの登録
    ::CORBA::Double statFric = 0.5;
    ::CORBA::Double slipFric = 0.5;
    ::CORBA::Double culling_thresh = 0.0; // ?
    OpenHRP::DblSequence6 K, C;
    K.length(0);
    C.length(0);
    // 床-ロボット 衝突検出ペアの登録
    dynamicsSimulator->registerCollisionCheckPair(info_floor->name(), "", info_body->name(), "", statFric, slipFric, K, C, culling_thresh);

    // 初期化メソッド
    dynamicsSimulator->initSimulation();

    // モデルロード  //
	floorBody = new hrp::Body(); // 床
	hrp::loadBodyFromBodyInfo(floorBody, info_floor);

	robotBody = new hrp::Body();// ロボット本体
	hrp::loadBodyFromBodyInfo(robotBody, info_body);

	floorBody->setName(info_floor->name());
	robotBody->setName(info_body->name());

	// drawstuffの設定
	dsFunctions fn;
	fn.version = DS_VERSION;
	fn.start = &start;
	fn.step = &simLoop;
	fn.command = &command;
	fn.path_to_textures = "/usr/local/share/drawstuff/textures";

	dsSimulationLoop(argc, argv, 640, 480, &fn);

	return 0;
}

template <typename X, typename X_ptr>
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
{
  CosNaming::Name ncName;
  ncName.length(1);
  ncName[0].id = CORBA::string_dup(n.c_str());
  ncName[0].kind = CORBA::string_dup("");
  X_ptr srv = NULL;
  try {
    srv = X::_narrow(cxt->resolve(ncName));
  } catch(const CosNaming::NamingContext::NotFound &exc) {
    std::cerr << n << " not found: ";
    switch(exc.why) {
    case CosNaming::NamingContext::missing_node:
      std::cerr << "Missing Node" << std::endl;
    case CosNaming::NamingContext::not_context:
      std::cerr << "Not Context" << std::endl;
      break;
    case CosNaming::NamingContext::not_object:
      std::cerr << "Not Object" << std::endl;
      break;
    }
    return (X_ptr)NULL;
  } catch(CosNaming::NamingContext::CannotProceed &exc) {
    std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
  } catch(CosNaming::NamingContext::AlreadyBound &exc) {
    std::cerr << "Resolve " << n << " InvalidName" << std::endl;
  }
  return srv;
}

static void getTransformData(OpenHRP::DynamicsSimulator_var dynamicsSimulator, const char* charname, const char* linkname, hrp::Vector3& p, hrp::Matrix33& R)
{
	OpenHRP::DblSequence_var var_pR;  // position and attitude
	dynamicsSimulator->getCharacterLinkData(charname, linkname, DynamicsSimulator::ABS_TRANSFORM, var_pR);
    for(int i=0; i<3; i++) p(i) = var_pR[i];
    for(int i=0; i<3; i++){
    	for(int j=0; j<3; j++) R(i,j) = var_pR[3+3*i+j];
    }
}

void getBodyData(hrp::BodyPtr body,  OpenHRP::DynamicsSimulator_var dynamicsSimulator)
{
	Link* root = body->rootLink();

	// ロボットの位置、姿勢の取得
	OpenHRP::DblSequence_var waist_pR;  // position and attitude
	OpenHRP::DblSequence_var waist_vw;  // linear and angular velocities
//	OpenHRP::DblSequence_var waist_dvw; // linear and angular accelerations
	dynamicsSimulator->getCharacterLinkData(body->name().c_str(), root->name.c_str(), DynamicsSimulator::ABS_TRANSFORM, waist_pR);
	dynamicsSimulator->getCharacterLinkData(body->name().c_str(), root->name.c_str(), DynamicsSimulator::ABS_VELOCITY,  waist_vw);
//	dynamicsSimulator->getCharacterLinkData(body->name().c_str(), root->name.c_str(), DynamicsSimulator::ABS_ACCELERATION,  waist_dvw);
    for(int i=0; i<3; i++) {
    	root->p(i) = waist_pR[i];
    	root->v(i) = waist_vw[i];
    	root->w(i) = waist_vw[i+3];
//    	root->dv(i) = waist_dvw[i];
//    	root->dw(i) = waist_dvw[i+3];
    }
    for(int i=0; i<3; i++){
    	for(int j=0; j<3; j++) root->R(i,j) = waist_pR[3+3*i+j];
    }

    OpenHRP::DblSequence_var angle;
    OpenHRP::DblSequence_var jointVel;
    OpenHRP::DblSequence_var jointAcc;
    OpenHRP::DblSequence_var jointU;
    dynamicsSimulator->getCharacterAllLinkData( body->name().c_str(), DynamicsSimulator::JOINT_VALUE, angle );
    dynamicsSimulator->getCharacterAllLinkData( body->name().c_str(), DynamicsSimulator::JOINT_VELOCITY, jointVel);
    dynamicsSimulator->getCharacterAllLinkData( body->name().c_str(), DynamicsSimulator::JOINT_ACCELERATION, jointAcc);
    dynamicsSimulator->getCharacterAllLinkData( body->name().c_str(), DynamicsSimulator::JOINT_TORQUE, jointU);

	int nJ = body->numJoints();
    for (int i = 0; i < nJ; ++i) {
    	Link* joint = body->joint(i);
    	joint->q = angle[i];
    	joint->dq = jointVel[i];
    	joint->ddq = jointAcc[i];
    	joint->u = jointU[i];
    }
}

void setBodyData(hrp::BodyPtr body,  OpenHRP::DynamicsSimulator_var dynamicsSimulator)
{
    int nJ = body->numJoints();
//    OpenHRP::DblSequence angle;
//    OpenHRP::DblSequence jointVel;
//    OpenHRP::DblSequence jointAcc;
    OpenHRP::DblSequence jointU;
//    angle.length(nJ);
//    jointVel.length(nJ);
//    jointAcc.length(nJ);
    jointU.length(nJ);

    for (int i = 0; i < nJ; ++i) {
    	Link* joint = body->joint(i);
//    	angle[i] = joint->q;
//    	jointVel[i] = joint->dq;
//    	jointAcc[i] = joint->ddq;
    	jointU[i] = joint->u;
    }

//    dynamicsSimulator->setCharacterAllLinkData( body->name().c_str(), DynamicsSimulator::JOINT_VALUE, angle );
//    dynamicsSimulator->setCharacterAllLinkData( body->name().c_str(), DynamicsSimulator::JOINT_VELOCITY, jointVel);
//    dynamicsSimulator->setCharacterAllLinkData( body->name().c_str(), DynamicsSimulator::JOINT_ACCELERATION, jointAcc);
    dynamicsSimulator->setCharacterAllLinkData( body->name().c_str(), DynamicsSimulator::JOINT_TORQUE, jointU);

}

// ロボット描写関連
void drawLink(hrp::Link* link)
{
	if (link == 0) return;

	Link* parent = link->parent;
	if (parent != 0) {
		drawLine(parent->p.data(), link->p.data());
	}

	drawLink(link->child);
	drawLink(link->sibling);
}

void drawLinkPoint(hrp::Link* link)
{
	if (link == 0) return;

	drawSphere(link->p.data(), link->R.data(), 0.02);

	drawLinkPoint(link->child);
	drawLinkPoint(link->sibling);
}

void drawRobot(hrp::BodyPtr body)
{
	Link *rootLink = body->rootLink();

	// ルートリンク
	dsSetColor(1.0, 1.0, 0.0);
	drawSphere(rootLink->p.data(), rootLink->R.data(), 0.03);

	// リンク部
	dsSetColor(0.0, 0.0, 0.0);
	drawLink(rootLink);

	dsSetColor(0.0, 1.0, 0.0);
	drawLinkPoint(rootLink);

	// 足部
	Link* ankle_L = body->link("LLEG_ANKLE_R");
	Link* ankle_R = body->link("RLEG_ANKLE_R");
	Vector3 ankle_p; ankle_p = 0.055, 0.0, -0.05;
	Vector3 ankle_L_p(ankle_L->p + ankle_L->R * ankle_p);
	Vector3 ankle_R_p(ankle_R->p + ankle_R->R * ankle_p);
	const double ankle_sides[3] = {0.25, 0.14, 0.01};
	dsSetColor(0.0, 0.0, 1.0);
	drawBox(ankle_L_p.data(), ankle_L->R.data(), ankle_sides);
	dsSetColor(1.0, 0.0, 0.0);
	drawBox(ankle_R_p.data(), ankle_R->R.data(), ankle_sides);
}

// drawstuff 補助関数

// R[9] -> R[12], double -> float
void R9d_to_R12f(const double R9[9], float R12[12])
{
	R12[0] = (float)R9[0], R12[1] = (float)R9[1], R12[2]  = (float)R9[2], R12[3]  = 0.0f;
	R12[4] = (float)R9[3], R12[5] = (float)R9[4], R12[6]  = (float)R9[5], R12[7]  = 0.0f;
	R12[8] = (float)R9[6], R12[9] = (float)R9[7], R12[10] = (float)R9[8], R12[11] = 0.0f;
}

void drawBox(const double pos[3], const double R[9], const double sides[3])
{
	float posf[3], R12f[12], sidesf[3];
	for (int i = 0; i < 3; i++) posf[i] = (float)pos[i];
	R9d_to_R12f(R, R12f);
	for (int i = 0; i < 3; i++) sidesf[i] = (float)sides[i];
	dsDrawBox(posf, R12f, sidesf);
}

void drawSphere(const double pos[3], const double R[9], const float radius)
{
	float posf[3], R12f[12];
	for (int i = 0; i < 3; i++) posf[i] = (float)pos[i];
	R9d_to_R12f(R, R12f);
	dsDrawSphere(posf, R12f, radius);
}

void drawCylinder(const double pos[3], const double R[9], float length, float radius)
{
	float posf[3], R12f[12];
	for (int i = 0; i < 3; i++) posf[i] = (float)pos[i];
	R9d_to_R12f(R, R12f);
	dsDrawCylinder(posf, R12f, length, radius);
}

void drawCapsule(const double pos[3], const double R[9], float length, float radius)
{
	 float posf[3], R12f[12];
	 for (int i = 0; i < 3; i++) posf[i] = (float)pos[i];
	 R9d_to_R12f(R, R12f);
	 dsDrawCapsule(posf, R12f, length, radius);
}

void drawLine(const double pos1[3], const double pos2[3])
{
	  float pos1f[3], pos2f[12];
	  for (int i = 0; i < 3; i++) pos1f[i] = (float)pos1[i];
	  for (int i = 0; i < 3; i++) pos2f[i] = (float)pos2[i];
	  dsDrawLine(pos1f, pos2f);
}