
#include "IMU.h"

fractional	OmegaRot[]={	0x4000	,0		,	0	,
							0		,0x4000	, 	0	,
							0		,0		,0x4000	
						}; // 1 coded in Q2.14 

					
fractional	R[]=	{	0x4000	,0		,		0	,
							0		,0x4000	, 	0	,
							0		,0		,0x4000	
					}; // 1 coded in Q2.14 

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

EulerAttitude attitude;
				
void DCMupdate(fractional * w)
{

// objectif: implement DCMnew=DCMold(Imatrix+AngleSkewmatrix)
// Q2.14 coding convention integer part sign included
//Q2.14 ie : 1 sign bit + 1 bit integer part +14 fractional part
//   -2.0<  Q2.14 value< (2.0- 1/2^14)	
// LSB= 2^14   	
//step 1	
	// convert w rate to angle
	//k_OmegaToAngle:Q1.15 w:Q2.14
	// deltaPhiVector is Q2.14 since VectorScale use mpy inst (accu scale 2)
	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)
		MatrixMultiply(3,3,3,Rtemp,R,OmegaRot); // result is scale in Q15 
		// since result is Q3.13 and we want Q2.14
		// need to scale x2
		MatrixAdd(3,3,R,Rtemp,Rtemp); //x2scaling	
		
		
	
}	

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] ) ; // note, 1/2 is built into 2.14
	//	scale rows 1 and 2 by the error
	VectorScale( 3 , &rbuff[0] , &R[3] , error ) ;
	VectorScale( 3 , &rbuff[3] , &R[0] , error ) ;
	//	update the first 2 rows to make them closer to orthogonal:
	VectorAdd( 3 , &rbuff[0] , &rbuff[0] , &R[0] ) ;
	VectorAdd( 3 , &rbuff[3] , &rbuff[3] , &R[3] ) ;
	//	use the cross product of the first 2 rows to get the 3rd row
	VectorCross( &rbuff[6] , &rbuff[0] , &rbuff[3] ) ; 
	VectorAdd( 3 , &rbuff[6]  , &rbuff[6] , &rbuff[6]  ); // scale X2

	//	rescale row1
	norm = VectorPower( 3 , &rbuff[0] ) <<1; // <<1 rescale because Scalegain of 0.5
	renorm=_Q15sqrt(norm);
	VectorScale( 3 , &rbuff[0] , &rbuff[0] , renorm ) ;
	VectorAdd( 3 , &R[0] , &rbuff[0] , &rbuff[0] ) ; //rescale
	//	rescale row2
	norm = VectorPower( 3 , &rbuff[3] ) ;
	renorm=_Q15sqrt(norm);
	VectorScale( 3 , &rbuff[3] , &rbuff[3] , renorm ) ;
	VectorAdd( 3 , &R[3] , &rbuff[3] , &rbuff[3] ) ;
	//	rescale row3
	norm = VectorPower( 3 , &rbuff[6] ) ;
	renorm=_Q15sqrt(norm);
	VectorScale( 3 , &rbuff[6] , &rbuff[6] , renorm ) ;
	VectorAdd( 3 , &R[6] , &rbuff[6] , &rbuff[6] ) ;
	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)
fractional scaleR6=(R[6]<<1);	
attitude.teta=- _Q15asinByPI(scaleR6); // scaling because R[6] is Q2.14
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. *dest = *src1X*src2 ;
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);
}