// IMU PARTIAL

#include "IMUfloat_DCM.h"

#include "../Utils/MathFloat.h"

#include <p33Fxxxx.h>



float	OmegaRot[]={	1	,0		,	0	,
							0		,1	, 	0	,
							0		,0		,1	
						}; 

					
float	R[]=	{	1	,0		,		0	,
							0		,1	, 	0	,
							0		,0		,1	
					}; 
					
float	Rtemp[]=	{	0		,0		,	0	,
							0		,0		, 	0	,
							0		,0		,	0		
					};



float Kpdrift =0.1;
float Kidrift =0.01;

float k_OmegaToAngle= 0.005; // IT time
								
								
float deltaPhiVector[]={0,0,0};

EulerAttitude attitude;


float	estimatorCompensate[3];


void initDCM()
{
Kpdrift =1.0;
Kidrift =0.1;

}	


void gBodyEstimatorCalc(int *prtgBody)
{
	//TO DO
	// a faire plus tard compensation de la force centrifuge
}

void NormalizegBodyEstimator(int *prtgBody)
{

}

void DriftCompMahony(Dof6Accelero accel)
{
float Accelero[3];
float norm;
//float errorVectorField[3];
float errorVector[3];
float KiTerm [3];
static float KiError [3]={0};

// scale accelero 
Accelero[0]=KaccScale*accel.data[0];
Accelero[1]=KaccScale*accel.data[1];
Accelero[2]=KaccScale*accel.data[2];
//normalize acceleration
norm = sqrt(Accelero[0]*Accelero[0] + Accelero[1]*Accelero[1] + Accelero[2]*Accelero[2]);   
norm+=1e-5; //prevent 0 
VectorScale(3,Accelero,Accelero,(1/norm));  

// compute sigma term=innovation compute
// gravity error alignement
VectorCrossProduct( errorVector ,Accelero,&R[6]  ) ; 
//VectorScale( 3 , errorVector , K1 ); 
// North error alignement
//VectorCrossProduct( errorVectorField ,Magneto,&R[0]  ) ;
// VectorScale( 3 , errorVectorField , K2 );
// VectorAdd(3,errorVector,errorVector,errorVectorField);

// integral term compute
VectorScale( 3 , KiTerm ,errorVector, Kidrift );
VectorAdd(3,KiError,KiError,KiTerm);
// Kp gain  term compute
VectorScale( 3 , errorVector ,errorVector, Kpdrift );

// Total error compute
VectorAdd(3,estimatorCompensate,errorVector,KiError);

}	
				
void DCMupdate(float * w)
{
	// estimator compensation : north and gravity error alignment
	VectorAdd(3,w,w,estimatorCompensate);
// objectif: implement DCMnew=DCMold(Imatrix+AngleSkewmatrix)

//step 1	
	// convert w rate to angle 
	VectorScale(3,deltaPhiVector,w,k_OmegaToAngle);
//	VectorSubtract(3,deltaPhiVector,deltaPhiVector,controlOutput);  
	// 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); // 
	// copy Rtemp to R
	int j; 
	for (j=0; j<9;j++) R[j]=Rtemp[j];
			
	normalize();	
	
}






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.
{
	float renorm; 
	float error=0; ;	
	float rbuff[9] ;

	//	compute -1/2 of the dot product between rows 1 and 2
	error =  - VectorDotProduct( 3 , &R[0] , &R[3] )*0.5;
	//	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
	VectorCrossProduct( &rbuff[6] , &rbuff[0] , &rbuff[3] ) ; 

	// normalize DCM
	//taylor approx
	int v;
	for(v=0; v<9; v+=3) 
		{    renorm = 0.5 *(3 - VectorDotProduct(3, &rbuff[v],&rbuff[v]));  // eq.21    
			 VectorScale(3, &R[v], &rbuff[v], renorm); 
	
		}
}



void DCMtoEuler()
{

/*
attitude.phi=ArcTan2 (R[7],R[8]) ; //Tan4Quadrant(y,x)
attitude.teta=-asin(R[6]);  // arcsin
attitude.psi=ArcTan2 (R[3],R[0]);
*/

attitude.phi=atan2f (R[7],R[8]) ; //Tan4Quadrant(y,x)
attitude.teta=-asinf(R[6]);  // arcsin
attitude.psi=atan2f (R[3],R[0]);
}	









