#include "stdlib.h"
#include "stdio.h"
#include "math.h"


#define	Accl_Conv	2.81			//Accelerometer Scale factor to produce g's
#define Gyro_Conv	2.81			//Gyroscope Sale factor to produce g's
#define Delta_t		.01				//time between ACCELEROMETER samples in seconds
#define Sq_Delta_t	.0001			//time between ACCELEROMETER samples squared (in seconds)
#define Gyro_Delta	.01				//time between Gyroscope samples in seconds



#define	Fg			9.81			//Force due to gravity m/(s^2)


double VelCoeff[6]={3/8,7/6,23/24,23/24,7/6,3/8};
double PosCoeff[6]={1525/1008,11875/2016,625/504,3125/1008,625/1008,275/2016};





//INS Data Type
struct INS_Data_Type{
	double pos_X;
	double pos_Y;
	double pos_Z;
	
	double vel_X;
	double vel_Y;
	double vel_Z;

	double Roll;
	double Pitch;
	double Yaw;


} INS_Data,Init_Data;


//Buffer Data type
struct Snsr_Buff_Type{
	double AccX[6];
	double AccY[6];
	double AccZ[6];

	double GyrX[6];
	double GyrY[6];
	double GyrZ[6];
} SnsBuff,RotBuff;

//typedef MatPtr




void INS_Round(double **,double **);
void Init_INS(void);
double ** Mat_Mul(double **,double **);
double DotP(double *,double *);
void Calc_Cbn(double **, double ,double, double);
void Mat_Vect_Mul(double **, double *);




void main(void)
{
	double ** Cbn;
	double ** Ctmp;
	int i;
	

	//Setup Cbn Matrix and initialize to Identity
	Cbn= calloc(3,sizeof(double *));
	for(i=0;i<3;i++)
		Cbn[i] = calloc(3,sizeof(double));
	Cbn[0][0]=1;
	Cbn[1][1]=1;
	Cbn[2][2]=1;

	//Setup tmp Matrix
	Ctmp= calloc(3,sizeof(double *));
	for(i=0;i<3;i++)
		Ctmp[i] = calloc(3,sizeof(double));
	




	Init_INS();			//Setup initial conditions of vehicle
						//and initialize other variables/conditions


	INS_Round(Cbn,Ctmp);



}





//Routine initializes the INS from the initial conditions:
//Position		(X,Y,Z)
//Velocity		(X,Y,Z)
//Acceleration	(X,Y,Z)
//Orientation	(Yaw,Pitch,Roll)
//etc. etc.
void Init_INS(void)
{




	return;

}




//Routine that performs one round/cycle of Navigation Update
void INS_Round(double ** Cbn,double ** Ctmp)
{

int i;
double ** Cnn;
double TVect[3];
double VelX,VelY,VelZ,PosX,PosY,PosZ;
double Tmp_Roll, Tmp_Yaw, Tmp_Pitch;



	

	//Rotate Acceleration Values into Navigation frame
	for(i=0;i<6;i++){
		TVect[0]=RotBuff.AccX[i];
		TVect[1]=RotBuff.AccY[i];
		TVect[2]=RotBuff.AccZ[i];

		Mat_Vect_Mul(Cbn,TVect);

		RotBuff.AccX[i]=TVect[0];
		RotBuff.AccY[i]=TVect[1];
		RotBuff.AccZ[i]=TVect[2];
	
		//Subtract of effect of Gravity for each Z component
		RotBuff.AccZ[i]-=Fg;
	
	};

	

	//Once acceleration vectors have been rotated and adjusted (subtract g)
	//calculate velocity and position
	VelX=(Delta_t * DotP(VelCoeff,RotBuff.AccX));
	VelY=(Delta_t * DotP(VelCoeff,RotBuff.AccY));
	VelZ=(Delta_t * DotP(VelCoeff,RotBuff.AccZ));

	PosX=(Sq_Delta_t * DotP(PosCoeff,RotBuff.AccX)) + (5 * INS_Data.vel_X);
	PosY=(Sq_Delta_t * DotP(PosCoeff,RotBuff.AccY)) + (5 * INS_Data.vel_Y);
	PosZ=(Sq_Delta_t * DotP(PosCoeff,RotBuff.AccZ)) + (5 * INS_Data.vel_Z);

	//Update INS_Data (Postion/Velocity)
	INS_Data.vel_X+=VelX;
	INS_Data.vel_Y+=VelY;
	INS_Data.vel_Z+=VelZ;

	INS_Data.pos_X+=PosX;
	INS_Data.pos_Y+=PosY;
	INS_Data.pos_Z+=PosZ;

	

	//Now calculate the amount rotated over the last cycle
	//See Coordinates by Andrew Paul for info on which axis is roll;pitch etc.
	Tmp_Roll=(Gyro_Delta * DotP(VelCoeff,RotBuff.GyrZ));
	Tmp_Yaw=(Gyro_Delta * DotP(VelCoeff,RotBuff.GyrY));
	Tmp_Pitch=(Gyro_Delta * DotP(VelCoeff,RotBuff.GyrX));

	//calculate the values of the intermediate rotation matrix
	Calc_Cbn(Ctmp,Tmp_Roll,Tmp_Pitch,Tmp_Yaw);

	Cnn = Mat_Mul(Cbn,Ctmp);
	
	free(Cbn);
	Cbn=Cnn;



}






//Dot Product: A.B ; uses straigtforward calculation method; method is Optimal
//For speed, this routine does not check if the two vectors are of the same size
//but assumes that they are
double DotP(double vectA[],double vectB[])
{

int i;
double sum;
	
	sum=0;			

	//tight loop that sums the "inner" product
	for(i=0;i<(sizeof(vectA)/sizeof(double));i++)
		sum+=vectA[i]*vectB[i];


	return sum;

}//end DotP


//Calculate the values of the intermediate Cbn; 
//Rotation matrix to convert body to navigation frame
//see LV2 INS Coordinate Document by Andrew Paul (9/16/2001)
void Calc_Cbn(double **Cbn, double theta,double phi,double psi)
{


	Cbn[0][0]=cos(theta)*cos(psi) - sin(theta)*sin(phi)*sin(psi);
	Cbn[0][1]=-sin(theta)*cos(phi);
	Cbn[0][2]=cos(theta)*sin(psi) + sin(theta)*sin(phi)*cos(psi);
	Cbn[1][0]=sin(theta)*cos(psi) + cos(theta)*sin(phi)*sin(psi);
	Cbn[1][1]=cos(theta)*cos(phi);
	Cbn[1][2]=sin(theta)*sin(psi) - cos(theta)*sin(phi)*cos(psi);
	Cbn[2][0]=-sin(psi)*cos(phi);
	Cbn[2][1]=sin(phi);
	Cbn[2][2]=cos(phi)*cos(psi);


	return;

}


//Routine to carry out the Matrix X Vector multiplication
//to rotate a Vector in body frame into Navigation frame
void Mat_Vect_Mul(double **Mat, double *Vect)
{

	double temp[3];

	temp[0]=DotP(Mat[0],Vect);
	temp[1]=DotP(Mat[1],Vect);
	temp[2]=DotP(Mat[2],Vect);

	Vect[0]=temp[0];
	Vect[1]=temp[1];
	Vect[2]=temp[2];

	return;

	


}

//Routine to multiply to 3x3 Matrices. Performs [Mat_A]X[Mat_B]
//Uses straight forward approach, i.e. for A=mxn and B=nxq, there
//will be mnq multiplications, so for our 3x3 sq. matrices, this is
//27 multiplications or n^3.
//*Note: Winograd's and Strassen's methods achieve better results, but
//since the absolute size of our matrices are small, we gain nothing.
//i.e. for multiplications: Ours= n^3 (27), Winograd's = 1/2(n^3)+n^2 (22.5 i.e 23)
//and Strassen's = n^(2.81) (21.9 i.e 22)
//
//Result is a pointer to a 3x3 Matrix
double ** Mat_Mul(double ** Mat_A, double ** Mat_B){

double ** New;
int i,j,k;

	New= calloc(3,sizeof(double *));
	
	for(i=0;i<3;i++)
		New[i] = calloc(3,sizeof(double));


	for (i=0;i<3;i++){
		for(j=0;j<3;j++){
			for(k=0;k<3;k++)
				New[i][j]+=(Mat_A[i][k]*Mat_B[k][j]);
		};//end j loop
	};//end i loop
	


	return(New);
}