#include "kinematicsMatrix.h"
#include <stdarg.h>

CvMat *cvmCreateRotMat( double alpha , double theta )
{
	CvMat *R = cvCreateMat( 3 , 3 , CV_64F );
	cvmSetRotationMatrix( R , alpha , theta );
	return R;
}

CvMat *cvmCreateRotMat( double rx , double ry , double rz )
{
	CvMat *R = cvCreateMat( 3 , 3 , CV_64F );
	cvmSetRotationMatrix( R , rx , ry , rz );
	return R;
}

CvMat *cvmCreateTransMat( double a , double alpha , double d , double theta  )
{
	CvMat *T = cvCreateMat( 4 , 4 , CV_64F );
	cvmSetTransMatrix( T , a , alpha , d , theta );
	return T;
}

CvMat *cvmCreateTransMat( double a , double P[6]  )
{
	CvMat *T = cvCreateMat( 4 , 4 , CV_64F );
	cvmSetRotationMatrix( T , P[5] , P[4] , P[3] );
	for(int i=0 ; i<3 ; i++) cvmSet( T , i , 3 , P[i] );
	return T;
}

void cvmSetTransMatrix( CvMat *trans , double a , double alpha , double d , double theta )
{
	cvmSet( trans , 0 , 0 , cos(theta) );
	cvmSet( trans , 0 , 1 , -sin(theta) );
	cvmSet( trans , 0 , 2 , 0 );
	cvmSet( trans , 0 , 3 , a );

	cvmSet( trans , 1 , 0 , cos(alpha)*sin(theta) );
	cvmSet( trans , 1 , 1 , cos(alpha)*cos(theta) );
	cvmSet( trans , 1 , 2 , -sin(alpha) );
	cvmSet( trans , 1 , 3 , -d*sin(alpha) );

	cvmSet( trans , 2 , 0 , sin(alpha)*sin(theta) );
	cvmSet( trans , 2 , 1 , sin(alpha)*cos(theta) );
	cvmSet( trans , 2 , 2 , cos(alpha) );
	cvmSet( trans , 2 , 3 , d*cos(alpha) );
	
	cvmSet( trans , 3 , 0 , 0 );
	cvmSet( trans , 3 , 1 , 0 );
	cvmSet( trans , 3 , 2 , 0 );
	cvmSet( trans , 3 , 3 , 1 );
}
	

void cvmSetRotationMatrix( CvMat *rot , double alpha , double theta )
{
	cvmSet( rot , 0 , 0 , cos(theta) );
	cvmSet( rot , 0 , 1 , -sin(theta) );
	cvmSet( rot , 0 , 2 , 0 );

	cvmSet( rot , 1 , 0 , cos(alpha)*sin(theta) );
	cvmSet( rot , 1 , 1 , cos(alpha)*cos(theta) );
	cvmSet( rot , 1 , 2 , -sin(alpha) );

	cvmSet( rot , 2 , 0 , sin(alpha)*sin(theta) );
	cvmSet( rot , 2 , 1 , sin(alpha)*cos(theta) );
	cvmSet( rot , 2 , 2 , cos(alpha) );
}


void cvmSetRotationMatrix( CvMat *rot , double rx , double ry , double rz)
{
	CvMat *Rr = cvCreateMat( 3 , 3 , CV_64F );
	CvMat *Rp = cvCreateMat( 3 , 3 , CV_64F );
	CvMat *Ry = cvCreateMat( 3 , 3 , CV_64F );

	cvmSet(Rr,0,0, cos(rz) );
	cvmSet(Rr,0,1, -sin(rz) );
	cvmSet(Rr,0,2, 0);
	cvmSet(Rr,1,0, sin(rz) );
	cvmSet(Rr,1,1, cos(rz) );
	cvmSet(Rr,1,2, 0);
	cvmSet(Rr,2,0, 0);
	cvmSet(Rr,2,1, 0);
	cvmSet(Rr,2,2, 1);

	cvmSet(Rp,0,0, cos(ry) );
	cvmSet(Rp,0,1, 0 );
	cvmSet(Rp,0,2, sin(ry) );
	cvmSet(Rp,1,0, 0 );
	cvmSet(Rp,1,1, 1 );
	cvmSet(Rp,1,2, 0);
	cvmSet(Rp,2,0, -sin(ry) );
	cvmSet(Rp,2,1, 0 );
	cvmSet(Rp,2,2, cos(ry));

	cvmSet(Ry,0,0, 1 );
	cvmSet(Ry,0,1, 0 );
	cvmSet(Ry,0,2, 0 );
	cvmSet(Ry,1,0, 0 );
	cvmSet(Ry,1,1, cos(rx) );
	cvmSet(Ry,1,2, -sin(rx));
	cvmSet(Ry,2,0, 0 );
	cvmSet(Ry,2,1, sin(rx) );
	cvmSet(Ry,2,2, cos(rx) );

	cvmMul( Rr , Rp , rot );
	cvmMul( rot , Ry , rot );

	cvReleaseMat( &Rr );
	cvReleaseMat( &Rp );
	cvReleaseMat( &Ry );
}

void cvmCalcRPY( CvMat *r , double *rx , double *ry , double *rz )
{
	CvMat *tmpRot = cvCreateMat( 3 , 3 , CV_64F );


	//
	//	パターン1
	//
	*rz = atan2( cvmGet(r,1,0) , cvmGet(r,0,0) );
	*ry = atan2( -cvmGet(r,2,0) , sqrt( cvmGet(r,0,0)*cvmGet(r,0,0) + cvmGet(r,1,0)*cvmGet(r,1,0) ) );
	*rx = atan2( cvmGet(r,2,1) , cvmGet(r,2,2) );

	//検算．正しければリターン
	cvmSetRotationMatrix( tmpRot , *rx , *ry , *rz );
	if( cvmCompareRotMat( r , tmpRot ) ) return;


	//
	//	パターン2
	//
	*rz = atan2( -cvmGet(r,1,0) , -cvmGet(r,0,0) );
	*ry = atan2( cvmGet(r,2,0) , -sqrt( cvmGet(r,0,0)*cvmGet(r,0,0) + cvmGet(r,1,0)*cvmGet(r,1,0) ) );
	*rx = atan2( -cvmGet(r,2,1) , -cvmGet(r,2,2) );
	
	//検算．正しければリターン
	cvmSetRotationMatrix( tmpRot , *rx , *ry , *rz );
	if( cvmCompareRotMat( r , tmpRot ) ) return;


	//
	//	パターン3
	//
	//ry=±M_PI/2の時特異点となり上の方法では解が出ないので
	//さらに解となりうるパターンを探す
	*rz = -atan2( cvmGet(r,0,1) , cvmGet(r,0,2) );
	*ry = M_PI/2;
	*rx = 0;

	//検算．正しければリターン
	cvmSetRotationMatrix( tmpRot , *rx , *ry , *rz );
	if( cvmCompareRotMat( r , tmpRot ) ) return;


	//
	//	パターン4
	//
	*rz = -atan2( cvmGet(r,0,1) , -cvmGet(r,0,2) );
	*ry = -M_PI/2;
	*rx = 0;

	//検算．正しければリターン
	cvmSetRotationMatrix( tmpRot , *rx , *ry , *rz );
	if( cvmCompareRotMat( r , tmpRot ) ) return;

	cvReleaseMat( &tmpRot );

	return;
}

bool cvmCompareRotMat( CvMat *m1 , CvMat *m2 , double error )
{

	for(int r=0 ; r<3 ; r++ )
	{
		for(int c=0 ; c<3 ; c++ )
		{
			if( abs( cvmGet(m1,r,c) - cvmGet(m2,r,c) ) > error ) return false; 
		}
	}

	return true;
}


CvMat* cvmMulAll( int num , ... )
{
	va_list list;
	va_start( list, num );

	CvMat *mat1 = va_arg(list, CvMat*);
	CvMat *mat2 = va_arg(list, CvMat*);
	CvMat *result = cvCloneMat( mat1 );

	cvmMul( mat1 , mat2 , result );

	for(int i=2 ; i<num ; i++ )
	{
		cvmMul( result , va_arg(list, CvMat*) , result );
	}

	va_end( list );


	return result;
}

void cvmPrint( CvMat *mat )
{
	for(int i=0; i<mat->rows ; i++ )
	{
		for(int j=0 ; j<mat->cols ; j++ )
		{
			//printf( "%.3lf	|" , cvmGet( mat , i , j ) );
		}

		//printf( "\n" );
	}
	//printf( "\n" );
}