class Ideone
{
	main{
		double t = time diffrence; //this is the delta t between each imput
		double[4] net_rotation = convert(0,0,0); //this is the total rotation quaternion between the start and present
		while(true){

			double x_g = imput x rotation rate; // gyro x axis
			double y_g = imput y rotation rate; // gyro y axis
			double z_g = imput z rotation rate; // gyro z axis

			net_rotation = multi( convert( x_g * t , y_g * t , z_g * t ) , net_rotation );

			double[3] acc_data  = { acc_x imput , acc_y imput , acc_z imput } //acceleraterometer imput
			multi( acc_data , revert(net_rotation); //this converts the acc data to inertial frame

			double[3] mag_data  = { mag_x imput , mag_y imput , mag_z imput } //magnetometer imput
			multi( mag_data , revert(net_rotation); //this converts the mag data to inertial frame
		}


	}




    public static double[4] convert(double x, double y, double z)
    {
        double x;
        double y;
        double z;

        double rot[][] = {
            { Math.cos(y) * Math.cos(z) , Math.cos(z) * Math.sin(x) * Math.sin(y) - Math.cos(x) * Math.sin(z) , Math.cos(x) * Math.cos(z) * Math.sin(y) + Math.sin(x) * Math.sin(z) },
            { Math.cos(y) * Math.sin(z) , Math.cos(x) * Math.cos(z) + Math.sin(x) * Math.sin(y) * Math.sin(z) , Math.cos(x) * Math.sin(y) * Math.sin(z) - Math.cos(z) * Math.sin(x) },
            { -Math.sin(z) , Math.cos(y) * Math.sin(x) , Math.cos(x) * Math.cos(y) }
        };

        double T = rot[0][0]+rot[1][1]+rot[2][2];

        double q[] = new q[4];

        if ( T >= T && T >= rot[0][0] && T>= rot[1][1] && T >= rot[2][2]){
            q[3] = Math.sqrt((1+T)/4);
            q[0] = ( rot[1][2] - rot[2][1] ) / (4 * q[3]);
            q[1] = ( rot[2][0] - rot[0][2] ) / (4 * q[3]);
            q[2] = ( rot[0][1] - rot[1][0] ) / (4 * q[3]);
        }
        else{
	        if ( rot[0][0] >= T && rot[0][0] >= rot[0][0] && rot[0][0] >= rot[1][1] && rot[0][0] >= rot[2][2]){
				q[0] = Math.sqrt((2+rot[0][0]-T)/4);
				q[1] = ( rot[0][1] + rot[1][0] ) / (4 * q[0]);
				q[2] = ( rot[0][2] + rot[2][0] ) / (4 * q[0]);
				q[3] = ( rot[1][2] - rot[2][1] ) / (4 * q[0]);
			}
			else{
				if ( rot[1][1] >= T && rot[1][1] >= rot[0][0] && rot[1][1] >= rot[1][1] && rot[1][1] >= rot[2][2]){
					q[1] = Math.sqrt((2 + rot[1][1] - T)/4);
					q[0] = ( rot[0][1] + rot[1][0] ) / (4 * q[1]);
					q[3] = ( rot[2][0] - rot[0][2] ) / (4 * q[1]);
					q[2] = ( rot[1][2] + rot[2][1] ) / (4 * q[1]);
				}
				else{
					if ( rot[2][2] >= T && rot[2][2] >= rot[0][0] && rot[2][2] >= rot[1][1] && rot[2][2] >= rot[2][2]){
						q[2] = Math.sqrt((2+rot[2][2]-T)/4);
						q[3] = ( rot[0][1] - rot[1][0] ) / (4 * q[2]);
						q[0] = ( rot[0][2] + rot[2][0] ) / (4 * q[2]);
						q[1] = ( rot[1][2] - rot[2][1] ) / (4 * q[2]);
					}
				}
			}
		}
       return q;
     }
    public static double[4] multi(double[4] q1, double[4] q2){
        //given the one from A-> B and the one from B-> C, the one from A-> C is given by C*B:
        //double q1[] = new double [4];//A->B
        //double q2[] = new double [4];//B->C
        double result[] = new double [4];//final

        result[0]= q2[3] * q1[0] + q2[2] * q1[1] - q2[1] * q1[2] + q2[0] * q1[3];
        result[1]=-q2[2] * q1[0] + q2[3] * q1[1] + q2[0] * q1[2] + q2[1] * q1[3];
        result[2]= q2[1] * q1[0] - q2[0] * q1[1] + q2[3] * q1[2] + q2[2] * q1[3];
        result[3]=-q2[0] * q1[0] - q2[1] * q1[1] - q2[2] * q1[2] + q2[3] * q1[3];
        return result;
    }

    public static double[3][3] revert(double[4] q){
        double[3][3] yeah = {
            { 2 * q[3] * q[3] + 2 * q[0] * q[0] - 1 , 2 * q[0] * q[1] + 2 * q[3] * q[2] , 2 * q[0] * q[2] - 2 * q[3] * q[1] },
            { 2 * q[1] * q[0] - 2 * q[3] * q[2] , 2 * q[3] * q[3] + 2 * q[1] * q[1] - 1 , 2 * q[1] * q[2] + 2 * q[3] * q[0] },
            { 2 * q[0] * q[2] + 2 * q[3] * q[1] , 2 * q[1] * q[2] - 2 * q[3] * q[0] , 2 * q[3] * q[3] + 2 * q[2] * q[2] - 1 }
        };
        return yeah;
    }

	public static void multi(double[3] acc, double[3][3] rot){
		acc[0]= acc[0] * rot [0][0] + acc[1] * rot[1][0] + acc[2] * rot[2][0];
		acc[1]= acc[0] * rot [0][1] + acc[1] * rot[1][1] + acc[2] * rot[2][1];
		acc[2]= acc[0] * rot [0][2] + acc[1] * rot[1][2] + acc[2] * rot[2][2];
	}

}