#pragma once
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#include <vector>

using namespace std;

#define ARM_JOINT_NUM 8 // アームの関節の数
// 各関節の半径
#define ARM0_R 0.11
#define ARM1_R 0.029
#define ARM2_R 0.029
#define ARM3_R 0.029
#define ARM4_R 0.029
#define ARM5_R 0.029
#define ARM6_R 0.0079 // 指
// 各関節の長さ
#define ARM0_L 0.4
#define ARM1_L 0.41
#define ARM2_L 0.175
#define ARM3_L 0.135
#define ARM4_L 0.04
#define ARM5_L 0.06
#define ARM6_L 0.08 // 指
// 各関節の質量
#define ARM0_M 1.0
#define ARM1_M 0.5
#define ARM2_M 0.5
#define ARM3_M 0.5
#define ARM4_M 0.4
#define ARM5_M 0.3
#define ARM6_M 0.01 // 指

#define BASE_R 0.45 // 土台の半径
#define BASE_L 0.72 // 土台の高さ
#define BASE_M 60

// オブジェクトを掴んでいるか
#define GRASP_NOTHING 0
#define GRASP_LEFT    1
#define GRASP_RIGHT   2

// ODE上で単精度と倍精度の両方を使えるようにする
#ifdef dDOUBLE
#define dsDrawBox      dsDrawBoxD
#define dsDrawSphere   dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCapsule  dsDrawCapsuleD
#endif

typedef struct
{
	dBodyID bodyID;
	dGeomID geomID;
	dReal x,y,z,l,r,m;
	dReal Rotation;
	dMatrix3 R;
	dJointID Joint;
	dReal Pos[3];
} Object;

class ArmSim
{
public:
	ArmSim();
	~ArmSim();

	bool StartSimulation();
	static bool   m_EndSim;
	static double m_WorldStep;
	static bool   m_UseGUI;
	static bool   m_UseManual;
	static bool   m_Inited;
	static bool   m_isBalanced;
	static bool   m_isCollision;
	static int    m_GraspHand; // どっちのアームで掴んでいるか
	static Object m_Object;

	static vector<Object> m_Obstacle;

	static dReal m_CollisionPos[ARM_JOINT_NUM];

	static dReal m_JointAngleL[ARM_JOINT_NUM];
	static dReal m_JointAngleR[ARM_JOINT_NUM];

	static dReal m_VelosityL[ARM_JOINT_NUM];
	static dReal m_VelosityR[ARM_JOINT_NUM];

	static dReal m_InitAngleL[ARM_JOINT_NUM];
	static dReal m_InitAngleR[ARM_JOINT_NUM];

	static dReal m_TargetAngleL[ARM_JOINT_NUM];
	static dReal m_TargetAngleR[ARM_JOINT_NUM];

private:
	static void start();
	static void simLoop(int pause);
	static void command(int cmd);
	static void stop();
	dsFunctions m_fn;

	static dWorldID m_WorldID;
	static dSpaceID m_SpaceID;
	static dGeomID m_ground;

	static dJointGroupID m_contactgroup;

	static void nearCallback(void *data, dGeomID o1, dGeomID o2);

	void MakeArm();
	static void DrawArm();

	static void ArmControl();

	static void MakeObstacle();
	static void DrawObstacle();

	static Object m_Face[4];
	static Object m_base;
	static Object m_ArmLeft[ARM_JOINT_NUM]; // 0〜5が関節6,7が指
	static Object m_ArmRight[ARM_JOINT_NUM];
	static dJointID m_Base2Ground;
	static dJointID m_ArmLeftJoint[ARM_JOINT_NUM];
	static dJointID m_ArmRightJoint[ARM_JOINT_NUM];
	static dJointID m_Object2Arm;

	static void GetArmLeftPos (dReal Px[ARM_JOINT_NUM], dReal Py[ARM_JOINT_NUM], dReal Pz[ARM_JOINT_NUM],
		                       dReal Ax[ARM_JOINT_NUM], dReal Ay[ARM_JOINT_NUM], dReal Az[ARM_JOINT_NUM]);
	static void GetArmRightPos(dReal Px[ARM_JOINT_NUM], dReal Py[ARM_JOINT_NUM], dReal Pz[ARM_JOINT_NUM],
		                       dReal Ax[ARM_JOINT_NUM], dReal Ay[ARM_JOINT_NUM], dReal Az[ARM_JOINT_NUM]);

	// PosSourceを各軸を中心にRadian[rad]回転させたときの座標をResultに格納する
	static void Get3DRotationX(dReal Result[3][3], const dReal PosSource[3][3], dReal Radian);
	static void Get3DRotationY(dReal Result[3][3], const dReal PosSource[3][3], dReal Radian);
	static void Get3DRotationZ(dReal Result[3][3], const dReal PosSource[3][3], dReal Radian);
	static void Get3DUnitMatrix(dReal Result[3][3]);
	static void GetMatrix33x13(dReal Result[3], const dReal Source[3][3]); // 3x3行列と1x3行列の掛け算
	static void GetdMatrix3(dMatrix3 Result, const dReal m[3][3]);

	static dReal AdjustScope(dReal a); // ラジアンを-pi〜piの範囲に収める
};

