#include  "NiJointsTool.h"




using namespace jbxl;

using namespace jbxwl;





//////////////////////////////////////////////////////////////////////////
//
// 作業用ジョイント番号
//     SDK に依存
//

// Default or No SDK
int	 NI_PELVIS	= NI_CMN_PELVIS;
int	 NI_TORSO	= NI_CMN_TORSO;
int	 NI_NECK	= NI_CMN_NECK;
int	 NI_HEAD	= NI_CMN_HEAD;
int	 NI_L_EAR	= NI_CMN_L_EAR;
int	 NI_R_EAR	= NI_CMN_R_EAR;
	
int  NI_L_SHLDR	= NI_CMN_L_SHLDR;
int  NI_L_ELBOW	= NI_CMN_L_ELBOW;
int	 NI_L_WRIST	= NI_CMN_L_WRIST;
int	 NI_L_HAND	= NI_CMN_L_HAND;
	
int	 NI_R_SHLDR	= NI_CMN_R_SHLDR;
int	 NI_R_ELBOW	= NI_CMN_R_ELBOW;
int	 NI_R_WRIST	= NI_CMN_R_WRIST;
int	 NI_R_HAND	= NI_CMN_R_HAND;

int	 NI_L_HIP	= NI_CMN_L_HIP;
int  NI_L_KNEE	= NI_CMN_L_KNEE;
int	 NI_L_ANKLE	= NI_CMN_L_ANKLE;
int	 NI_L_FOOT	= NI_CMN_L_FOOT;
	
int	 NI_R_HIP	= NI_CMN_R_HIP;
int	 NI_R_KNEE	= NI_CMN_R_KNEE;
int	 NI_R_ANKLE	= NI_CMN_R_ANKLE;
int	 NI_R_FOOT	= NI_CMN_R_FOOT;




/////////////////////////////////////////////////////////////////////////////
//
// Joints
//

static std::string  _NiJointName[] =		// NI_JOINT_NAME_NUM
{
	"PELVIS", "WAIST", "TORSO", "NECK", "HEAD", "L_EAR", "R_EAR",

	"L_COLLAR", "L_SHLDR", "L_ELBOW", "L_WRIST", "L_HAND", "L_FINGER",
	"R_COLLAR", "R_SHLDR", "R_ELBOW", "R_WRIST", "R_HAND", "R_FINGER",

	"L_HIP", "L_KNEE", "L_ANKLE", "L_FOOT",
	"R_HIP", "R_KNEE", "R_ANKLE", "R_FOOT"
};




////////////////////////////////////////////////////////////////////////////////////////////
//
// Library 固有のジョイント番号から共通のジョイント名を得る
//

static  int  _OpenNI2JointNameNum[] =	// NI_OPENNI_JOINT_NUM
{
	 0,  4,  3,  2,  1,
	13, 14, 15, 16, 17, 18,
	 7,  8,  9, 10, 11, 12,
	23, 24, 25, 26,
	19, 20, 21, 22
};

static  int  _Kinect2JointNameNum[] =	// NI_KINECT_JOINT_NUM
{
	 0,  2,  3,  4, 
	 8,  9, 10, 11,
	14, 15, 16, 17,
	19, 20, 21, 22,
	23, 24, 25, 26 
};

static  int  _Ni2JointNameNum[] =		// NI_JOINT_NUM
{
	 0,  2,  3,  4,  5,  6,
	 8,  9, 10, 11,
	14, 15, 16, 17,
	19, 20, 21, 22,
	23, 24, 25, 26 
};


std::string  jbxwl::NiSDK2JointName(int j, NiSDK_Lib lib)
{
	std::string str = "";

	if (lib==NiSDK_OpenNI) {
		str = _NiJointName[_OpenNI2JointNameNum[j]];
	}
	else if (lib==NiSDK_Kinect) {
		str = _NiJointName[_Kinect2JointNameNum[j]];
	}
	else if (lib==NiSDK_None) {
		str = _NiJointName[_Ni2JointNameNum[j]];
	}

	return str;
}




////////////////////////////////////////////////////////////////////////////////////////////
//
// Library 固有のジョイント番号からミラージョイントの番号を得る
//

static  int  _OpenNI2MirrorJointNum[] =	// NI_OPENNI_JOINT_NUM
{
	 0,  1,  2,  3,  4,
	11, 12, 13, 14, 15, 16,
	 5,  6,  7,  8,  9, 10,
	21, 22, 23, 24,
	17, 18, 19, 20
};

static  int  _Kinect2MirrorJointNum[] =	// NI_KINECT_JOINT_NUM
{
	 0,  1,  2,  3, 
	 8,  9, 10, 11,
	 4,  5,  6,  7,
	16, 17, 18, 19,
	12, 13, 14, 15 
};

static  int  _Ni2MirrorJointNum[] =		// NI_JOINT_NUM
{
	 0,  1,  2,  3,  5,  4,
	10, 11, 12, 13,
	 6,  7,  8,  9,
	18, 19, 20, 21,
	14, 15, 16, 17 
};


int  jbxwl::NiSDK2MirrorJointNum(int j, NiSDK_Lib lib)
{
	int ret = -1;

	if (lib==NiSDK_OpenNI) {
		ret = _OpenNI2MirrorJointNum[j];
	}
	else if (lib==NiSDK_Kinect) {
		ret = _Kinect2MirrorJointNum[j];
	}
	else if (lib==NiSDK_None) {
		ret = _Ni2MirrorJointNum[j];
	}

	return ret;
}





////////////////////////////////////////////////////////////////////////////////////////////
//
// 共通ジョイント番号から，Library固有の座標用ジョイント番号を得る
//

static  int  _Ni2OpenNIPosJointNum[] =		// NI_JOINT_NUM
{
	 0,  3,  2,  1, -1, -1,
	12, 13, -1, 15,
	 6,  7, -1,  9,
	21, 22, -1, 24,
	17, 18, -1, 20
};

static  int  _Ni2KinectPosJointNum[] =		// NI_JOINT_NUM
{
	 0,  1,  2,  3, -1, -1,
	 4,  5,  6,  7,
	 8,  9, 10, 11,
	12, 13, 14, 15,
	16, 17, 18, 19
};


int  jbxwl::Ni2SDKPosJointNum(int j, NiSDK_Lib lib)
{
	int ret = -1;

	if (lib==NiSDK_Kinect) {
		ret = _Ni2KinectPosJointNum[j];
	}
	else if (lib==NiSDK_OpenNI) {
		ret = _Ni2OpenNIPosJointNum[j];
	}
	else if (lib==NiSDK_None) {
		ret = j;
	}

	return ret;
}




////////////////////////////////////////////////////////////////////////////////////////////
//
// 共通ジョイント番号から，Library固有の回転用ジョイント番号を得る
//

static  int  _Ni2OpenNIRotJointNum[] =		// NI_JOINT_NUM
{
	 0,  3,  2, -1, -1, -1,
	12, 13, -1, -1,
	 6,  7, -1, -1,
	21, 22, -1, -1,
	17, 18, -1, -1
};

static  int  _Ni2KinectRotJointNum[] =		// NI_JOINT_NUM
{
	 0,  1,  2, -1, -1, -1,
	 4,  5,  6, -1,
	 8,  9, 10, -1,
	12, 13, 14, -1,
	16, 17, 18, -1
};

static  int  _Ni2RotJointNum[] =			// NI_JOINT_NUM
{
	 0,  1,  2,  3, -1, -1,
	 4,  5,  6, -1,
	 8,  9, 10, -1,
	12, 13, 14, -1,
	16, 17, 18, -1
};


int  jbxwl::Ni2SDKRotJointNum(int j, NiSDK_Lib lib)
{
	int ret = -1;

	if (lib==NiSDK_Kinect) {
		ret = _Ni2KinectRotJointNum[j];
	}
	else if (lib==NiSDK_OpenNI) {
		ret = _Ni2OpenNIRotJointNum[j];
	}
	else if (lib==NiSDK_None) {
		ret = _Ni2RotJointNum[j];
	}

	return ret;
}




////////////////////////////////////////////////////////////////////////////////////////////
//
// 共通のジョイント名から Library 固有のジョイント番号を得る
//

static  int  _JointNameNum2Ni[] =			// NI_JOINT_NAME_NUM
{
	 0, -1,  1,  2,  3,  4,  5,
	-1,  6,  7,  8,  9, -1,
	-1, 10, 11, 12, 13, -1,
	14, 15, 16, 17,
	18, 19, 20, 21 
};


int   jbxwl::JointName2NiSDK(char* jname, NiSDK_Lib lib)
{
	int ret = -1;

	int j;
	for (j=0; j<NI_JOINT_NAME_NUM; j++) {
		if (!strcasecmp(jname, (char*)_NiJointName[j].c_str())) break;
	}

	if (j<NI_JOINT_NAME_NUM) {
		ret = Ni2SDKPosJointNum(_JointNameNum2Ni[j], lib);
	}
	else if (j==NI_JOINT_NAME_NUM) {
		//DEBUG_ERR("JointName2NiSDK: Unknown Joint Name = %s", jname);
	}

	return ret;
}






///////////////////////////////////////////////////////////////////////////
//
//
//

// OpenNI
void  jbxwl::NiSetOpenNIJointNums(void)
{
	NI_PELVIS	=  0;
	NI_TORSO	=  3;	// XN_SKEL_TORSO;
	NI_NECK		=  2;	// XN_SKEL_NECK;
	NI_HEAD		=  1;	// XN_SKEL_HEAD;

	NI_L_EAR	= -1;
	NI_R_EAR	= -1;
	
	NI_L_SHLDR	= 12;	// XN_SKEL_RIGHT_SHOULDER;
	NI_L_ELBOW	= 13;	// XN_SKEL_RIGHT_ELBOW;
	NI_L_WRIST	= -1;
	NI_L_HAND	= 15;	// XN_SKEL_RIGHT_HAND;

	NI_R_SHLDR	=  6;	// XN_SKEL_LEFT_SHOULDER;
	NI_R_ELBOW	=  7;	// XN_SKEL_LEFT_ELBOW;
	NI_R_WRIST	= -1;
	NI_R_HAND	=  9;	// XN_SKEL_LEFT_HAND;
	
	NI_L_HIP	= 21;	// XN_SKEL_RIGHT_HIP;
	NI_L_KNEE	= 22;	// XN_SKEL_RIGHT_KNEE;
	NI_L_ANKLE	= -1;
	NI_L_FOOT	= 24;	// XN_SKEL_RIGHT_FOOT;

	NI_R_HIP	= 17;	// XN_SKEL_LEFT_HIP;
	NI_R_KNEE	= 18;	// XN_SKEL_LEFT_KNEE;
	NI_R_ANKLE	= -1;
	NI_R_FOOT	= 20;	// XN_SKEL_LEFT_FOOT;

	return;
}



// Kinect SDK
void  jbxwl::NiSetKinectJointNums(void)
{
	NI_PELVIS	=  0;	// NUI_SKELETON_POSITION_HIP_CENTER
	NI_TORSO	=  1;	// SPINE は書き換えて使用 
	NI_NECK		=  2;	// NUI_SKELETON_POSITION_SHOULDER_CENTER
	NI_HEAD		=  3;	// NUI_SKELETON_POSITION_HEAD

	NI_L_EAR	= -1;
	NI_R_EAR	= -1;
		
	NI_L_SHLDR	=  4;	// NUI_SKELETON_POSITION_SHOULDER_LEFT
	NI_L_ELBOW	=  5;	// NUI_SKELETON_POSITION_ELBOW_LEFT
	NI_L_WRIST	=  6;	// NUI_SKELETON_POSITION_WRIST_LEFT
	NI_L_HAND	=  7;	// NUI_SKELETON_POSITION_HAND_LEFT
	
	NI_R_SHLDR	=  8;	// NUI_SKELETON_POSITION_SHOULDER_RIGHT
	NI_R_ELBOW	=  9;	// NUI_SKELETON_POSITION_ELBOW_RIGHT
	NI_R_WRIST	= 10;	// NUI_SKELETON_POSITION_WRIST_RIGHT
	NI_R_HAND	= 11;	// NUI_SKELETON_POSITION_HAND_RIGHT

	NI_L_HIP	= 12;	// NUI_SKELETON_POSITION_HIP_LEFT
	NI_L_KNEE	= 13;	// NUI_SKELETON_POSITION_KNEE_LEFT
	NI_L_ANKLE	= 14;	// NUI_SKELETON_POSITION_ANKLE_LEFT
	NI_L_FOOT	= 15;	// NUI_SKELETON_POSITION_FOOT_LEFT
	
	NI_R_HIP	= 16;	// NUI_SKELETON_POSITION_HIP_RIGHT
	NI_R_KNEE	= 17;	// NUI_SKELETON_POSITION_KNEE_RIGHT
	NI_R_ANKLE	= 18;	// NUI_SKELETON_POSITION_ANKLE_RIGHT
	NI_R_FOOT	= 19;	// NUI_SKELETON_POSITION_FOOT_RIGHT

	return;
}