#include "Pos2Joint.h"
#include "kinematicsMatrix.h"
#include "ArmDefine.h"

const int ARM_LINK_R	= 800;
const int ARM_LINK_D	= 135;
const int ARM_LINK_L0	= 115;
const int ARM_LINK_L1	= 501;
const int ARM_LINK_L2	= 400;
const int ARM_LINK_L3	= 100;
const int ARM_LINK_L4	= 326;
const int ARM_LINK_LH	= 110;

bool Pos2JointR( float pos[6] , float joint[6] , int ArmPose )
{
	// ロボット座標からアーム座標へ
	Robo2Arm( pos );

	for(int i=3 ; i<6 ; i++) pos[i] = pos[i]*M_PI/180;  // サーバーの関節角は度なので、計算のためラジアンに変換

	if( !CalcInverseKinematics( pos , joint , ArmPose , RIGHT_ARM ) )
	{
		//_ERR_LOG( "計算失敗" );
		return false;
	}

	if( joint[4] >0 || joint[4]<-120*M_PI/180 )  // ラジアンで考える
	{
		//別な姿勢を試みる
		joint[3] += 180*M_PI/180;
		joint[4] = -joint[4];

		if( joint[4] >0 && joint[4]<-120*M_PI/180 )  // ラジアンで考える
		{
			return false;
		}
	}

	if( joint[4] > -0.05 ) joint[4] = -0.05;
	if( joint[4] < -2 ) joint[4] = -2.0;
	
	return true;
}

bool Pos2JointL( float pos[6] , float joint[6] , int ArmPose )
{
	// ロボット座標からアーム座標へ
	Robo2Arm( pos );

	for(int i=3 ; i<6 ; i++) pos[i] = pos[i]*M_PI/180;  // サーバーの関節角は度なので、計算のためラジアンに変換

	if( !CalcInverseKinematics( pos , joint , ArmPose , LEFT_ARM ) )
	{
		//_ERR_LOG( "計算失敗" );
		return false;
	}

	if( joint[4] >0 || joint[4]<-120*M_PI/180 )  // ラジアンで考える
	{
		//別な姿勢を試みる
		joint[3] += 180*M_PI/180;
		joint[4] = -joint[4];

		if( joint[4] >0 && joint[4]<-120*M_PI/180 )  // ラジアンで考える
		{
			return false;
		}
	}

	if( joint[4] > -0.05 ) joint[4] = -0.05;
	if( joint[4] < -2 ) joint[4] = -2.0; 
	
	return true;
}

bool CalcInverseKinematics( float pos[6] , float joint[6] , int ArmPose , bool rightArm )
{
	double wristPos[6];		// 元の座標系での手首の座標
	double tmpPos[6];		// 計算簡易化のための仮の座標系での手首の位置
	double dsit;

	// 手先から手首の座標を計算する
	wristPos[0] = -ARM_LINK_LH * cos( pos[5] ) * cos( pos[4] ) + pos[0];
	wristPos[1] = -ARM_LINK_LH * sin( pos[5] ) * cos( pos[4] ) + pos[1];
	wristPos[2] =  ARM_LINK_LH * sin( pos[4] ) + pos[2];
	wristPos[3] = pos[3];
	wristPos[4] = pos[4];
	wristPos[5] = pos[5];

	// cosの値の範囲をチェック
	double cosValue = ARM_LINK_L3 / sqrt(wristPos[0]*wristPos[0] + wristPos[1]*wristPos[1]);
	if( cosValue > 0.99 ) cosValue = 0.99;
	else if( cosValue < -0.99 ) cosValue = -0.99;

	// J0
	if( rightArm )
	{
		joint[0] = -acos( cosValue ) - atan2( wristPos[0], wristPos[1]  );
	}
	else
	{
		joint[0] = acos( cosValue ) - atan2( wristPos[0], wristPos[1]  );
	}


	// 仮の座標系へ平行移動
	// 以下、この仮の座標系での計算
	tmpPos[0] = wristPos[0] + ARM_LINK_L3 * sin( joint[0] );
	tmpPos[1] = wristPos[1] - ARM_LINK_L3 * cos( joint[0] );
	for(int i=2 ; i<6 ; i++ ) tmpPos[i] = wristPos[i];

	dsit = sqrt( tmpPos[0]*tmpPos[0] + tmpPos[1]*tmpPos[1] + tmpPos[2]*tmpPos[2] );


	// J1 = α+α'
	double alpha = atan( tmpPos[2] / ( sqrt( tmpPos[0]*tmpPos[0] + tmpPos[1]*tmpPos[1] ) ) );
	double bunbo = dsit*dsit + ARM_LINK_L2*ARM_LINK_L2 - ARM_LINK_L4*ARM_LINK_L4;
	double bunsi = sqrt( 4*ARM_LINK_L2*ARM_LINK_L2*dsit*dsit - bunbo*bunbo );
	if( !rightArm )
	{
		if( ArmPose == ARM_POSE_ELBOW_DOWN ) joint[1] = alpha - atan2( bunsi , bunbo );		//ヒジの位置で場合分け
		else joint[1] = alpha + atan2( bunsi , bunbo );
	}
	else
	{
		if( ArmPose == ARM_POSE_ELBOW_DOWN ) joint[1] = M_PI - (alpha - atan2( bunsi , bunbo ));		//ヒジの位置で場合分け
		else joint[1] = M_PI - (alpha + atan2( bunsi , bunbo ));
	}

	// J2
	bunsi = dsit*dsit - ARM_LINK_L2*ARM_LINK_L2 - ARM_LINK_L4*ARM_LINK_L4;
	bunbo = sqrt( 4.0*ARM_LINK_L2*ARM_LINK_L2*ARM_LINK_L4*ARM_LINK_L4 - bunsi*bunsi );


	if( rightArm )
	{
		if( ArmPose == ARM_POSE_ELBOW_UP )  joint[2] = atan2( bunsi , -bunbo );			//ヒジの位置によって場合分け
		else joint[2] = atan2( bunsi , bunbo );
	}
	else
	{
		if( ArmPose == ARM_POSE_ELBOW_DOWN )  joint[2] = atan2( bunsi , -bunbo );			//ヒジの位置によって場合分け
		else joint[2] = atan2( bunsi , bunbo );
	}


	// 姿勢
	CvMat *Rw_1 = cvmCreateRotMat( 0 , joint[0] );
	CvMat *R1_2 = cvmCreateRotMat( -M_PI/2 , -joint[1] );
	CvMat *R2_3 = cvmCreateRotMat( 0 , M_PI/2-joint[2] );
	CvMat *Rw_3 = cvmMulAll( 3 , Rw_1 , R1_2 , R2_3 );
	CvMat *iRw_3 = cvCreateMat( 3 , 3 , CV_64F );
	cvmInvert( Rw_3 , iRw_3 );
	CvMat *Rw_6 = cvmCreateRotMat( pos[3] , pos[4] , pos[5] );
	CvMat *R3_6 = cvmMulAll( 2 , iRw_3 , Rw_6 );

	joint[3] = atan2( cvmGet(R3_6,2,0) , cvmGet(R3_6,1,0) );
	joint[4] = atan2( cvmGet(R3_6,1,0)/cos(joint[3]) , cvmGet(R3_6,0,0) );
	joint[5] = atan2( cvmGet(R3_6,0,2) , -cvmGet(R3_6,0,1) );


	cvReleaseMat( &Rw_1 );
	cvReleaseMat( &R1_2 );
	cvReleaseMat( &R2_3 );
	cvReleaseMat( &Rw_3 );
	cvReleaseMat( &iRw_3 );
	cvReleaseMat( &Rw_6 );
	cvReleaseMat( &R3_6 );

	for(int i=0 ; i<6 ; i++ )
	{
		if( _isnan( joint[i] ) || !_finite( joint[i] ) )
		{
			return false;
		}
	}

	return true;
}

void CalcKinematics( float J[6] , float P[6] )
{
	CvMat *trans[6];
	CvMat *Tw_6;
	double rx, ry, rz;


	for(int i=0 ; i<6 ; i++ ) trans[i] = cvCreateMat( 4 , 4 , CV_64F );

	cvmSetTransMatrix( trans[0] , 0			, 0			, 0				, J[0]		);
	cvmSetTransMatrix( trans[1] , 0			, -M_PI/2		, ARM_LINK_L3	, -J[1]		);
	cvmSetTransMatrix( trans[2] , ARM_LINK_L2	, 0			, 0				, M_PI/2-J[2] );
	cvmSetTransMatrix( trans[3] , ARM_LINK_L4	, J[3]		, 0				, 0			);
	cvmSetTransMatrix( trans[4] , 0			, 0			, 0				, J[4]		);
	cvmSetTransMatrix( trans[5] , ARM_LINK_LH	, J[5] , 0			, 0	);

	Tw_6 = cvmMulAll( 6 , trans[0] , trans[1] , trans[2] , trans[3] , trans[4] , trans[5] );

	for(int i=0 ; i<3 ; i++ ) P[i] = cvmGet( Tw_6 , i , 3 );

	cvmCalcRPY( Tw_6 , &rx , &ry , &rz );
	P[3] = rx;
	P[4] = ry;
	P[5] = rz;

	for(int i=0 ; i<6 ; i++ ) cvReleaseMat( &trans[i] );
	cvReleaseMat( &Tw_6 );
}