#include  "stdafx.h"

#include  "NiJoints.h"
#include  "NiToolWin.h"



using namespace jbxl;

using namespace jbxwl;







CNiJoints::CNiJoints(void)
{
	mvav_type	= MVAV_Expo;
	mvav_num	= 3;

	noise_min	= 0.02;		// 2cm

	posVect		= NULL;
	rotQuat		= NULL;
	posRing		= NULL;
	rotRing		= NULL;

}




void  CNiJoints::setParameter(CParameterSet param)
{
	confidence	= param.confidence; 

	mvav_type	= param.mvavType;
	mvav_num	= param.mvavNum;
	init_mvav_weight();
}




void  CNiJoints::init_mvav_weight(void)
{
	mvav_num = Min(mvav_num, NI_MVAV_MAX_NUM);
	mvav_num = Max(mvav_num, 1);

	if (mvav_type==MVAV_Simple) {
		for (int i=0; i<mvav_num; i++) {
			mvav_weight[i] = 1.0/mvav_num;
		}
	}
	//
	else if (mvav_type==MVAV_Weight) {
		double sum = (mvav_num+1.0)*mvav_num/2.0;
		for (int i=0; i<mvav_num; i++) {
			mvav_weight[i] = (mvav_num - i)/sum;
		}
	}
	//
	else if (mvav_type==MVAV_Expo) {
		double exp_param = NI_MVAV_EXP_DEC*LN_10/mvav_num;
		double sum = 1.0;
		mvav_weight[0] = 1.0;
		//
		for (int i=1; i<mvav_num; i++) {
			double weight = exp(-exp_param*i);
			mvav_weight[i] = weight;
			sum += weight;
		}
		for (int i=0; i<mvav_num; i++) {
			mvav_weight[i] = mvav_weight[i]/sum;
		}
	}
	//
	else {
		for (int i=0; i<mvav_num; i++) {
			mvav_weight[i] = 0.0;
		}
	}
}





/////////////////////////////////////////////////////////////////////////////////////////////
//
// 平滑化
//

void  CNiJoints::PosMovingAverage(NiSDK_Lib sdk_lib)
{
	if (mvav_num<2) return;

	for (int j=0; j<NI_JOINT_NUM; j++) {
		//
		int joint = Ni2SDKPosJointNum(j, sdk_lib);
		if (joint>=0) {
			//
			Vector<double> vect = posVect[joint]*mvav_weight[0];
			double sum = mvav_weight[0];

			for (int i=1; i<mvav_num; i++) {
				Vector<double>* pv = (Vector<double>*)posRing[joint].get(-i);
				if (pv!=NULL && pv->c>0.0) {
					vect = vect + (*pv)*mvav_weight[i];
					sum += mvav_weight[i];
				}
			}

			posVect[joint] = vect/sum;
		}
	}
}




void  CNiJoints::RotMovingAverage(NiSDK_Lib sdk_lib)
{
	if (mvav_num<2) return;

	for (int j=0; j<NI_JOINT_NUM; j++) {
		//
		int joint = Ni2SDKRotJointNum(j, sdk_lib);
		if (joint>=0) {
			//
			Quaternion quat = rotQuat[joint]*mvav_weight[0];
			double sum = mvav_weight[0];

			for (int i=1; i<mvav_num; i++) {
				Quaternion* pq = (Quaternion*)rotRing[joint].get(-i);
				if (pq!=NULL && pq->c>0.0) {
					quat = quat + (*pq)*mvav_weight[i];
					sum += mvav_weight[i];
				}
			}

			rotQuat[joint] = quat/sum;
		}		
	}

}






/////////////////////////////////////////////////////////////////////////////////////////////
//
// 制約条件
//

void  CNiJoints::PosVibNoiseCanceler(void)
{
	for (int j=0; j<NI_JOINT_NUM; j++) {
		//
		int joint = Ni2SDKPosJointNum(j, sdk_lib);
		if (joint>=0) {
			//
			Vector<double> v[2], d[2];
			memset(&v, 0, sizeof(Vector<double>)*2);

			for (int n=0, i=0; i<NI_MVAV_MAX_NUM; i++) {
				Vector<double>* pv = (Vector<double>*)posRing[joint].get(-i);
				if (pv!=NULL && pv->c>0.0) {
					v[n] = *pv;
					if (n==1) break;
					n++;
				}
			}
	
			if (v[1].c>0.0) {
				d[0] = posVect[joint] - v[0];
				d[1] = v[0] - v[1];
				if (d[0]*d[1]<0.0 && d[0].n<noise_min && d[1].n<noise_min) {
					posVect[joint] = v[0];
				}
			}
		}
	}
}




void  CNiJoints::RotVibNoiseCanceler(void)
{
	for (int j=0; j<NI_JOINT_NUM; j++) {
		//
		int joint = Ni2SDKRotJointNum(j, sdk_lib);
		if (joint>=0) {
			//
			Quaternion qt;
			memset(&qt, 0, sizeof(Quaternion));

			for (int i=0; i<NI_MVAV_MAX_NUM; i++) {
				Quaternion* pq = (Quaternion*)rotRing[joint].get(-i);
				if (pq!=NULL && pq->c>0.0) {
					qt = *pq;
					break;
				}
			}
	
			if (qt.c>0.0) {
				Vector<double> v1 = rotQuat[joint].getVector();
				Vector<double> v2 = qt.getVector();
				double th = rotQuat[joint].getAngle(); 
				if (v1*v2<0.0 && th<0.0873) {	// 5Deg
					rotQuat[joint] = qt;
				}
			}
		}
	}
}




void  CNiJoints::CheckGroundLevel(double ground_level)
{
	// 誤差が大きすぎる
	/*
	if (posVect[NI_L_ANKLE].z<ground_level) {
		posVect[NI_L_FOOT].z += ground_level - posVect[NI_L_ANKLE].z;
		posVect[NI_L_ANKLE].z = ground_level;
	}
	if (posVect[NI_R_ANKLE].z<ground_level) {
		posVect[NI_R_FOOT].z += ground_level - posVect[NI_R_ANKLE].z;
		posVect[NI_R_ANKLE].z = ground_level;
	}
	*/

	return;
}





void  CNiJoints::CheckBoneLength(void)
{
	// HIP - KNEE
	if (posVect[NI_R_KNEE].c>=confidence) {
		double dist_rhk = VectorDist(posVect[NI_R_HIP], posVect[NI_R_KNEE]);
		if (dist_rhk<0.20 || dist_rhk>1.20) {
			posVect[NI_R_KNEE].c  = -1.0;
			posVect[NI_R_ANKLE].c = -1.0;
			posVect[NI_R_FOOT].c  = -1.0;
		}
	}
	if (posVect[NI_L_KNEE].c>=confidence) {
		double dist_lhk = VectorDist(posVect[NI_L_HIP], posVect[NI_L_KNEE]);
		if (dist_lhk<0.20 || dist_lhk>1.20) {
			posVect[NI_L_KNEE].c  = -1.0;
			posVect[NI_L_ANKLE].c = -1.0;
			posVect[NI_L_FOOT].c  = -1.0;
		}
	}

	// KNEE - ANKLE
	if (posVect[NI_R_ANKLE].c>=confidence) {
		double dist_rka = VectorDist(posVect[NI_R_KNEE], posVect[NI_R_ANKLE]);
		if (dist_rka<0.20 || dist_rka>1.20) {
			posVect[NI_R_ANKLE].c = -1.0;
			posVect[NI_R_FOOT].c  = -1.0;
		}
	}
	if (posVect[NI_L_ANKLE].c>=confidence) {
		double dist_lka = VectorDist(posVect[NI_L_KNEE], posVect[NI_L_ANKLE]);
		if (dist_lka<0.20 || dist_lka>1.20) {
			posVect[NI_L_ANKLE].c = -1.0;
			posVect[NI_L_FOOT].c  = -1.0;
		}
	}
}




void  CNiJoints::CheckBoneRotation(void)
{
	// HIP
	Vector<double> vleft = posVect[NI_L_HIP] - posVect[NI_R_HIP];
	Vector<double> vrhip = rotQuat[NI_R_HIP].getVector();
	Vector<double> vlhip = rotQuat[NI_L_HIP].getVector();

	vleft.normalize();
	vrhip.normalize();
	vlhip.normalize();

	double cos_rhip = vleft*vrhip;
	double cos_lhip = vleft*vlhip;

	if (Xabs(cos_rhip)<0.2) {
		if (rotQuat[NI_R_HIP].s<0.8) rotQuat[NI_R_HIP].init();
	}
	if (Xabs(cos_lhip)<0.2) {
		if (rotQuat[NI_L_HIP].s<0.8) rotQuat[NI_L_HIP].init();
	}


	// KNEE
	Vector<double> vrhk = posVect[NI_R_KNEE] - posVect[NI_R_HIP];
	Vector<double> vlhk = posVect[NI_L_KNEE] - posVect[NI_L_HIP];
	Vector<double> vrkn = rotQuat[NI_R_KNEE].getVector();
	Vector<double> vlkn = rotQuat[NI_L_KNEE].getVector();

	vrhk.normalize();
	vlhk.normalize();
	vrkn.normalize();
	vlkn.normalize();

	double cos_rknee = vrhk*vrkn;
	double cos_lknee = vlhk*vlkn;

	if (Xabs(cos_rknee)>0.5) {
		if (rotQuat[NI_R_KNEE].s<0.8) rotQuat[NI_R_KNEE].init();
	}
	if (Xabs(cos_lknee)>0.5) {
		if (rotQuat[NI_L_KNEE].s<0.8) rotQuat[NI_L_KNEE].init();
	}

//	DEBUG_ERR("RIGHT => cos = %f  quat = (%f, %f, %f, %f)", cos_rhip, quat[NI_R_HIP].x, quat[NI_R_HIP].y, quat[NI_R_HIP].z, quat[NI_R_HIP].s); 
//	DEBUG_ERR("LEFT  => cos = %f  quat = (%f, %f, %f, %f)", cos_lhip, quat[NI_L_HIP].x, quat[NI_L_HIP].y, quat[NI_L_HIP].z, quat[NI_L_HIP].s); 
//	DEBUG_ERR(" ");

	return;
}