// IMU PARTIAL

#include "IMUpartial2.h"

fractional	OmegaRot[]={	0	,0		,	0	,
							0		,0	, 	0	,
							0		,0		,0	
						}; // 0 coded in Q1.15

fractional	IdentityMat[]={	0x7FFF	,0		,	0	,
							0		,0x7FFF	, 	0	,
							0		,0		,0x7FFF	
						}; // 1 coded in Q1.15
					
fractional	R[]=	{	0x7FFF	,0		,		0	,
							0		,0x7FFF	, 	0	,
							0		,0		,0x7FFF	
					}; // 1 coded in Q1.15

					
fractional	Rtemp[]=	{	0		,0		,	0	,
							0		,0		, 	0	,
							0		,0		,	0		
					};


typedef struct
   { 
	   int rawData[3];
	   int data[3];
	   int offset[3];
   } Dof6Accelero;



fractional k_OmegaToAngle=2288 ;// coeeficient permettant d'obtenir angle pour un deltaT=2ms => deltaPhi=4 deg=0.06991 
								//= 1144 en 2.14 = 2288 en Q1.15 
								
fractional deltaPhiVector[]={0,0,0};

EulerAttitude attitude;
				
void DCMupdate(fractional * w)
{

// objectif: implement DCMnew=DCMold(Imatrix+AngleSkewmatrix)

//step 1	
	// convert w rate to angle (w Q1.15)
	//k_OmegaToAngle:Q1.15 deltaPhiVector=Q1.15
	VectorScale(3,deltaPhiVector,w,k_OmegaToAngle);  
	// skew construct
		// first row
	OmegaRot[1]=-deltaPhiVector[2]; //-phi_z
	OmegaRot[2]=+deltaPhiVector[1]; //+phi_y
		// second row
	OmegaRot[3]=deltaPhiVector[2]; //+phi_z
	OmegaRot[5]=-deltaPhiVector[0]; //-phi_x
		// third row
	OmegaRot[6]=-deltaPhiVector[1]; //-phi_y
	OmegaRot[7]=+deltaPhiVector[0]; //phi_x

// step two: Rnew=R*(I+Angleskwew)=R+R*Angleskwew
// PS : multiplication is splitted
		MatrixMultiply(3,3,3,Rtemp,R,OmegaRot); // result is in Q15 

		MatrixAdd(3,3,R,R,Rtemp); 		
	
}	

void normalize(void)
//	This is the routine that maintains the orthogonality of the
//	direction cosine matrix, which is expressed by the identity
//	relationship that the cosine matrix multiplied by its
//	transpose should equal the identity matrix.
//	Small adjustments are made at each time step to assure orthogonality.
{
	fractional norm ; // actual magnitude
	fractional renorm ;	// renormalization factor
	fractional error ;	// renormalization factor
	fractional rbuff[9] ;
	//	compute -1/2 of the dot product between rows 1 and 2
	error =  - VectorDotProduct( 3 , &R[0] , &R[3] ) ; 
	error=error>>2; // to obtain error/2
	//	scale rows 1 and 2 by the error
	VectorScale( 3 , &rbuff[0] , &R[3] , error ) ; 	//-(error/2)*X
	VectorScale( 3 , &rbuff[3] , &R[0] , error ) ;	//-(error/2)*Y
	//	update the first 2 rows to make them closer to orthogonal:
	VectorAdd( 3 , &rbuff[0] , &rbuff[0] , &R[0] ) ; 	// Xorthogonal=X-(error/2)*X
	VectorAdd( 3 , &rbuff[3] , &rbuff[3] , &R[3] ) ;	// Yorthogonal=Y-(error/2)*Y
	//	use the cross product of the first 2 rows to get the 3rd row
	VectorCross( &rbuff[6] , &rbuff[0] , &rbuff[3] ) ; 


	//	rescale row1
	norm = VectorPower( 3 , &rbuff[0] ); 
	renorm=_Q15sqrt(norm);
	VectorScale( 3 , &rbuff[0] , &rbuff[0] , renorm ) ;
	//	rescale row2
	norm = VectorPower( 3 , &rbuff[3] ) ;
	renorm=_Q15sqrt(norm);
	VectorScale( 3 , &rbuff[3] , &rbuff[3] , renorm ) ;

	//	rescale row3
	norm = VectorPower( 3 , &rbuff[6] ) ;
	renorm=_Q15sqrt(norm);
	VectorScale( 3 , &rbuff[6] , &rbuff[6] , renorm ) ;

	return ;
}




void DCMtoEuler()
{
// all angles are coded in Q1.15
// -1.0 -> -PI  ; +0.9999 ->+PI

// no need to scale because tan=y/x
attitude.phi=Tan4Quadrant (R[7],R[8]) ; //Tan4Quadrant(y,x)
attitude.teta=- _Q15asinByPI(R[6]); 
attitude.psi=Tan4Quadrant (R[3],R[0]);
}	




/////////////////////////////////
//// MATH
fractional Tan4Quadrant(fractional y,fractional x)
{	// careful dspic function _Q15atanYByX(x,y)  with Y=sin(e) X=cos(e)	
	fractional result=0;
	if (x>0)
		if(y>0) // quadrant 1
			if (x>=y)
				{
				result=_Q15atanYByXByPI(x,y); //0->PI/4
				return (result);
				}
			else
				{
				result=16383 - _Q15atanYByXByPI(y,x); //PI/2-(angle>0)
				return (result);
				}
		else  // quadrant 4
			if (x>=_Q15abs(y))
				{
				result=_Q15atanYByXByPI(x,y); //0->-PI/4
				return (result);
				}
			else
				{
				result=-16384 - _Q15atanYByXByPI(y,x); //-PI/2-(angle<0)
				return (result);
				}
	else
		if(y>0) // quadrant 2
			if (_Q15abs(x)>=y)
				{
				result=32767+_Q15atanYByXByPI(x,y); // +PI+(angle<0)  ;
				return (result);
				}
			else
				{
				result=16383 - _Q15atanYByXByPI(y,x); //PI/2-(angle<0)
				return (result);
				}
		else
			if (x>=y)// quadrant 3 careful x <0 et y<0 , x>y => abs(x)<abs(y)
				{
				result=-16384 - _Q15atanYByXByPI(x,y); //-PI/2-(angle>0)
				return (result);
				}
			else
				{
				result=-32768 + _Q15atanYByXByPI(x,y); //-PI +(angle>0)
				return (result);
				}
				
	if ((x>0)&&(y==0)) return(0); //0
	if ((x==0)&&(y>0)) return(16383); //+PI/2
	if ((x==0)&&(y<0)) return(-16384); //-PI/2
	if ((x<0)&&(y==0)) return(32768); //-PI
	if ((x==0)&&(y==0)) return(0); //impossible	 return 0
}


void EulerEstimatorAccelero(Dof6Accelero *ptrAccelero,EulerAttitude *ptrEuler)
{
ptrEuler->phiAccelero=Tan4Quadrant(ptrAccelero->data(1],ptrAccelero->data(0]);



}

//	Implement the cross product in Q15 result
void VectorCross( fractional * dest , fractional * src1 , fractional * src2 )
{
	fractional skewMatsrc1[9];
	skewMatsrc1[0]=0;
	skewMatsrc1[1]=-src1[2];
	skewMatsrc1[2]=src1[1];
	skewMatsrc1[3]=src1[2];
	skewMatsrc1[4]=0;
	skewMatsrc1[5]=-src1[0];
	skewMatsrc1[6]=-src1[1];
	skewMatsrc1[7]=src1[0];
	MatrixMultiply(3,3,1,dest,skewMatsrc1,src2);
}
