//Accelerometer Calibration Matrix A and vector b from matlab

const int N = 3;                //Dimensions of Matrix A and vector b
float A[N][N] = {
                {0.6077, 0.0014, -0.0021},      //A[0][0], A[0][1], A[0][2]
                {0.0, 0.6077, -0.0002   },      //A[1][0], A[1][1], A[1][2]
                {0.0, 0.0, 0.6033       }       //A[2][0], A[2][1], A[2][2]
};

//b[0][0], b[0][1], b[0][2]
float b[N] = {-543.9129, 273.3477, 241.1651};

//Useful Matrix and vectors
float F[N][N] = {
                {0.0000, 0.0000, 0.0000},
                {0.0000, 0.0000, 0.0000},
                {0.0000, 0.0000, 0.0000}
};

float cal[N] = {0.0000, 0.0000, 0.0000}; //calibrated Values
float raw[N] = {0.15210, 0.00232, 1.115302};   //raw values
float dif[N] = {0.0000, 0.0000, 0.0000}; //difference (raw-b) vector

void setup() {
  Serial.begin(115200);
}

void loop() {

//cal = 1.0e-04*A*(raw-b);

//Difference (raw-b)

 for (int j = 0; j < N; j++){
      dif[j] = raw[j] - b[j];
 }


//      Serial.print(dif[0],4);
//      Serial.print(" ");
//      Serial.print(dif[1],4);
//      Serial.print(" ");
//      Serial.println(dif[2],4);



//Moltiplication 1.0e-04 *A*dif


  for (int j = 0; j < N; j++){
    for(int k = 0; k < N; k++)
    {
        F[j][k] = A[j][k] * dif[k];
    }
  }
  for(int j = 0; j < N; j++)
  {
      cal[j] = 0.0001*(F[j][N-3]+F[j][N-2]+F[j][N-1]);
  }

//      Serial.print(cal[0],4);
//      Serial.print(" ");
//      Serial.print(cal[1],4);
//      Serial.print(" ");
//      Serial.println(cal[2],4);

}