#include "Wire.h"
#include "I2Cdev.h"
#include "MPU9150.h"

#define N_SAMPLE 256
#define WAIT_TIME 50
#define N_DIM 4
#define EPS 1e-8

// ライブラリ内で定義されたセンサーの値を取得するクラスのインスタンス
MPU9150 sensor;

// 磁気センサの値を保存する変数
float mx, my, mz;

// 連立方程式
float A[4][4];
float B[4];

void setup() {
  Serial.begin(9600);

  // センサーの初期化
  Wire.begin();
  Serial.println("Initializing I2C devices...");
  sensor.initialize();
  Serial.println("Testing device connections...");
  Serial.println(sensor.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
}

void loop() {
  // 動かさないと値が取れないので動かして！！！って表示する
  Serial.println("calculating mag offset...");
  Serial.println("move sensor...");
  // ちょっと待ってあげる
  delay(500);
  
  init_data();    // 初期化して
  calc_offset();  // オフセットを計算・表示する

  delay(2000);
}

void calc_offset() {
  float a, b, c, r;
  for (int i = 0 ; i < N_SAMPLE ; i++ ) {
    // センサーの値を読み取る
    get_mag();

    // 左辺(係数行列)
    A[0][0] += 2.0 * mx * mx;
    A[0][1] += 2.0 * mx * my;
    A[0][2] += 2.0 * mx * mz;
    A[0][3] += 2.0 * mx;

    A[1][0] += 2.0 * my * mx;
    A[1][1] += 2.0 * my * my;
    A[1][2] += 2.0 * my * mz;
    A[1][3] += 2.0 * my;

    A[2][0] += 2.0 * mz * mx;
    A[2][1] += 2.0 * mz * my;
    A[2][2] += 2.0 * mz * mz;
    A[2][3] += 2.0 * mz;

    A[3][0] += 2.0 * mx;
    A[3][1] += 2.0 * my;
    A[3][2] += 2.0 * mz;
    A[3][3] += 2.0 * 1.0;

    // 右辺
    B[0] += mx * (mx * mx + my * my + mz * mz);
    B[1] += my * (mx * mx + my * my + mz * mz);
    B[2] += mz * (mx * mx + my * my + mz * mz);
    B[3] += mx * mx + my * my + mz * mz;

    Serial.print(mx);Serial.print("\t");
    Serial.print(my);Serial.print("\t");
    Serial.print(mz);Serial.print("\n");

    delay(WAIT_TIME);
  }
  solve(A, B);  // 連立方程式を計算

  // a,b,c,dのパラメータがB[0],B[1],B[2],B[3]に代入されているので
  // a,b,c,rを代入して関数を終わる
  a = B[0];
  b = B[1];
  c = B[2];
  r = sqrt(B[0] * B[0] + B[1] * B[1] + B[2] * B[2] + 2 * B[3]);

  Serial.print("(x,y,z,r) = ");
  Serial.print("(");
  Serial.print(a); Serial.print("\t");
  Serial.print(b); Serial.print("\t");
  Serial.print(c); Serial.print("\t");
  Serial.print(r); Serial.print("\t");
  Serial.println(")");
}

void init_data() {
  // A,Bを初期化する関数
  A[0][0] = 0.0; A[0][1] = 0.0; A[0][2] = 0.0; A[0][3] = 0.0;
  A[1][0] = 0.0; A[1][1] = 0.0; A[1][2] = 0.0; A[1][3] = 0.0;
  A[2][0] = 0.0; A[2][1] = 0.0; A[2][2] = 0.0; A[2][3] = 0.0;
  A[3][0] = 0.0; A[3][1] = 0.0; A[3][2] = 0.0; A[3][3] = 0.0;
  B[0] = B[1] = B[2] = B[3] = 0.0;
}

void get_mag() {
  // センサーの値を一時的に格納する変数
  int16_t raw_ax, raw_ay, raw_az;
  int16_t raw_gx, raw_gy, raw_gz;
  int16_t raw_mx, raw_my, raw_mz;

  // センサーの値を読み取る
  sensor.getMotion9(
    &raw_ax, &raw_ay, &raw_az,
    &raw_gx, &raw_gy, &raw_gz,
    &raw_mx, &raw_my, &raw_mz);

  // スケールを調整
  mx = ((float)raw_mx) / 4096.0;
  my = ((float)raw_my) / 4096.0;
  mz = ((float)raw_mz) / 4096.0;
}

// 連立方程式を求める関数
void solve(float A[N_DIM][N_DIM], float B[N_DIM]) {
  float temp;

  for (int i = 0 ; i < N_DIM ; i++ ) {
    // ピボット選択的みたいな処理
    for (int j = i + 1 ; j < N_DIM ; j++ ) {
      if ( is_zero(A[i][j]) == false )
        break;

      for (int k = 0 ; k < N_DIM ; k++ )
        A[i][k] += A[j][k];
      B[i] += B[j];
    }

    // 対角成分を1に
    temp = A[i][i];
    for (int j = i ; j < N_DIM ; j++ )
      A[i][j] /= temp;
    B[i] /= temp;

    // 前進消去
    for (int j = i + 1 ; j < N_DIM ; j++ ) {
      temp = A[j][i];

      for (int k = i ; k < N_DIM ; k++ )
        A[j][k] -= temp * A[i][k];
      B[j] -= temp * B[i];
    }
  }

  // 後進消去
  for (int i = N_DIM - 1 ; i >= 0 ; i-- )
    for (int j = i - 1 ; j >= 0 ; j-- )
      B[j] -= A[j][i] * B[i];
}

// ほぼ0ならtrueを返す関数
bool is_zero(float val){
  return -EPS < val && val < EPS;
}