// IMU PARTIAL

#include "IMUpartial.h"
#include <p33Fxxxx.h>
#define Q14_OnePlus 0x4000  //01.xxxxxxx	+1  in Q2.14
#define Q14_OneMinus 0xC000  //11.xxxxxxx   -2+1=-1 in Q2.14

fractional	OmegaRot[]={	0x7FFF	,0		,	0	,
							0		,0x7FFF	, 	0	,
							0		,0		,0x7FFF	
						}; // 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;


// coeeficient permettant d'obtenir angle pour un deltaT=2ms => deltaPhimax=4 deg=0.06991=2288 en Q15 
// k*wmax=deltaPhiMax avec wmax=2000deg/s et 1deg/s=14.375LSB=> wmax=28750
// k=2288/28750=0.07957=2607 en Q15
fractional k_OmegaToAngle=2607 ;
								
								
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:Q15 deltaPhiVector=Q15
	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*OmegaRot (identity is included in OmegaRot)

	MatrixMultiply(3,3,3,Rtemp,R,OmegaRot); // result is in Q15
	int j; 
	for (j=0; j<9;j++) R[j]=Rtemp[j];	

			
	
}	

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; 
	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>>1; // to obtain error/2
	//	scale rows 1 and 2 by the error
	VectorScale( 3 , &rbuff[0] , &R[3] , error ) ; 	//-(error/2)*X term
	VectorScale( 3 , &rbuff[3] , &R[0] , error ) ;	//-(error/2)*Y term
	//	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] ) ; 


	// VectorPower cannot be use
	// because Q15(ie Q1.15) saturation  when addition of power terms
	norm = MyVectorPower( 3 , &rbuff[0] ); // norm is in Q2.14
	renorm=inverseSqrtNormApproximation(norm); // result is Q2.14
	VectorScale( 3 , &rbuff[0] , &rbuff[0] , renorm ) ; // Q1.15*Q2.14= Q3.29
	VectorAdd( 3 , &R[0] , &rbuff[0] ,  &rbuff[0] ) ;// x2 scaling to obtain Q15
	//	rescale row2
	norm = MyVectorPower( 3 , &rbuff[3] ); // norm is Q2.14
	renorm=inverseSqrtNormApproximation(norm);
	VectorScale( 3 , &rbuff[3] , &rbuff[3] , renorm ) ;
	VectorAdd( 3 , &R[3] , &rbuff[3] ,  &rbuff[3] ) ;// x2 scaling
	//	rescale row3
	norm = MyVectorPower( 3 , &rbuff[6] ); // norm is Q2.14
	renorm=inverseSqrtNormApproximation(norm);
	VectorScale( 3 , &rbuff[6] , &rbuff[6] , renorm ) ;
	VectorAdd( 3 , &R[6] , &rbuff[6] ,  &rbuff[6] ) ;// x2 scaling

	return ;
}


// Taylor estimation of 1/sqrt(x1^2+x2^2+x3^2) 
//INPUT in Q2.14 format
// return as Q2.14
fractional inverseSqrtNormApproximation(fractional vectnorm)
{	// Taylor estimation of 1/sqrt(x1^2+x2^2+x3^2)
	// since norm of row column are close to 1 we have  1/sqrt(1+x)
	// with x= (x1^2+x2^2+x3^2) -1
	// Taylor f=(1+x)^-0.5   f'=-(1/2).(1+x)^(3/2)    f''=(3/4)(1+x)^(5/2)
	// lim f(x->a->0) = f(a)+f'(a)(x-a) +f''(a)/2 (x-a)^2 + f'''(a)/3! (x-a)^3
	// so  1/sqrt(1+x) =1 -(1/2)x +(3/8)x^2  with x= (x1^2+x2^2+x3^2) -1
	
	long int result=0; 
	long int result2,tempo;
	fractional x=vectnorm+Q14_OneMinus; // (x1^2+x2^2+x3^2) -1
	
	// compute =-(1/2)x +(3/8)x^2
	result=__builtin_mulss(x,0x0000E000) ;// -.5= -2 +1 +0.5 =0xE000
	result2=__builtin_mulss(x,x); // x^2 term
	result2=result2>>14; // since result2 is Q2.24 scaling to Q214
	result+=__builtin_muluu(result2,0x0001800);//3/8= 0.375 *2^14= 6144
	x=result>>14; // scale Q4.24 in Q2.14
	x+=Q14_OnePlus; // add 1: 1 + [-(1/2)x +(3/8)x^2]
	
 	return (x); // return as Q2.14
}

void DCMtoEuler()
{
// all angles are coded in Q1.15
// -1.0 -> -PI  ; +0.9999 ->+PI
// careful because _Q15atanYByXByPI error become signifiant with angle>25deg

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
}


//	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];
	skewMatsrc1[8]=0;
	MatrixMultiply(3,3,1,dest,skewMatsrc1,src2);
}


fractional MyVectorPower( int lenght, fractional *ptr )
{
long int result=0;
int i;
fractional valQ;
 
for (i=0; i<lenght;i++)
{
	valQ=ptr[i]>>1;	// scale input to Q2.14 format
	result+=__builtin_mulss(valQ,valQ) ;//
}
result=result>>14;
return (result);
}	