// Include the correct display library
// For a connection via I2C using the Arduino Wire include:
#include <Wire.h>               // Only needed for Arduino 1.6.5 and earlier
#include "SSD1306Wire.h"        // legacy: #include "SSD1306.h"

// Initialize the OLED display using Arduino Wire:
SSD1306Wire display(0x3c, SDA, SCL);  // SCREEN_ADDRESS->0x3C, SDA-> D2(GPIO05), SCL->(GPIO04)

// Basic demo for accelerometer readings from Adafruit MPU6050
#include "MPU6050.h"
MPU6050 mpu;

// 信号処理用フィルター（Kalmanフィルターを使用）
#include "KalmanFilter.h"
//  X 軸と Y 軸のカルマン フィルター設定 (角度、偏差、測定モデルをセット)
KalmanFilter kalmanX(0.001, 0.003, 0.03);
KalmanFilter kalmanY(0.001, 0.003, 0.03);

// 加速度計を使ってピッチとロールの値を計算した値を格納
float accPitch = 0;
float accRoll = 0;

// カルマンフィルターとジャイロスコープを考慮して算出したピッチとロールの値を格納
float kalPitch = 0;
float kalRoll = 0;

//int cnt = 0;

void setup() {
  Serial.begin(115200);
  while (!Serial) {
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  }
  Serial.println();
  Serial.println("Wake Up");

  // Initialising the UI will init the display too.
  display.init();

  // 長手方向が上
  display.flipScreenVertically();

  // Fill the rectangle
  display.fillRect(0, 0, 127, 63);


  Serial.println("Fill the rectangle");

  // Try to initialize!
  if (!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G)) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
      Serial.print(".");
    }
  }

  Serial.println("MPU6050 init");

  // ジャイロスコープの校正
  mpu.calibrateGyro();

  // 感度設定
  mpu.setThreshold(3);

  Serial.println("");
  delay(100);
}

void draw() {
  // Font
  display.setFont(ArialMT_Plain_10);
  // 左詰め
  display.setTextAlignment(TEXT_ALIGN_LEFT);

  // 窓枠の四角形
  display.drawRect(0, 0, 127, 63);

  // 中央の水平線
  display.drawHorizontalLine(0, 32, 86);

  // 中央の垂直線
  display.drawVerticalLine(86, 0, 63);

  // 右枠下段の水平線
  display.drawHorizontalLine(86, 53, 39);

  char m[50];
  // 表示文字(上段)
  display.setFont(ArialMT_Plain_24);
  display.drawString(3, 3, String(dtostrf(kalPitch,3,1,m))+ "°");

  // 表示文字(下段)
  display.setFont(ArialMT_Plain_24);
  display.drawString(3, 33, String(dtostrf(kalRoll,3,1,m))+ "°");

  // 表示文字(右側)
  display.setFont(ArialMT_Plain_10);
  display.drawString(87,  1, "accel");  // Acceleration
  display.drawString(87, 25, "gyro");   // Gyroscope

  //  センサーからベクトルを読み取る
  Vector rawGyro = mpu.readRawGyro();
  Vector normAccel = mpu.readNormalizeAccel();
  // センサーから温度を読み取る
  float temp = mpu.readTemperature();


  /* Print out the values */
  Serial.print("AccelX:");
  Serial.print(rawGyro.XAxis);
  Serial.print(",");
  Serial.print("AccelY:");
  Serial.print(rawGyro.YAxis);
  Serial.print(",");
  Serial.print("AccelZ:");
  Serial.print(rawGyro.ZAxis);
  Serial.print(", ");
  Serial.print("GyroX:");
  Serial.print(normAccel.XAxis);
  Serial.print(",");
  Serial.print("GyroY:");
  Serial.print(normAccel.YAxis);
  Serial.print(",");
  Serial.print("GyroZ:");
  Serial.print(normAccel.ZAxis);
  Serial.print(",");
  Serial.print("temp:");
  Serial.print(temp);
  Serial.println("");

  // 表示文字(右側) ゼロ値
  display.setFont(ArialMT_Plain_10);
  display.setTextAlignment(TEXT_ALIGN_RIGHT);
  display.drawString(125,  7, String(rawGyro.XAxis));	// X
  display.drawString(125, 13, String(rawGyro.YAxis));	// Y
  display.drawString(125, 19, String(rawGyro.ZAxis));	// Z
  display.drawString(125, 31, String(normAccel.XAxis));	// X
  display.drawString(125, 37, String(normAccel.YAxis));	// Y
  display.drawString(125, 43, String(normAccel.ZAxis));	// Z
  display.drawString(125, 52, String(temp));	// Temperature

}


void loop() {
  Serial.print("Round=");  //Serial.println(cnt);

  /*
  //100回に1回だけ描画
  cnt = cnt + 1;
  if(cnt > 1){
    cnt = 0;
    display.clear();
    draw();
    display.display();
  }
  */

  display.clear();
  draw();
  display.display();

  //  センサーからベクトルを読み取る
  Vector acc = mpu.readNormalizeAccel();
  Vector gyr = mpu.readNormalizeGyro();
 
  // 加速度センサーによるピッチ＆ロール計算
  accPitch = -(atan2(acc.XAxis, sqrt(acc.YAxis*acc.YAxis + acc.ZAxis*acc.ZAxis))*180.0)/M_PI;
  accRoll  = (atan2(acc.YAxis, acc.ZAxis)*180.0)/M_PI;
 
  // カルマン - 加速度計とジャイロスコープからのデータ
  kalPitch = kalmanY.update(accPitch, gyr.YAxis) * -1;
  kalRoll = kalmanX.update(accRoll, gyr.XAxis);
 
  Serial.print("Pitch = ");
  Serial.print(accPitch);
  Serial.print(" Roll = ");
  Serial.print(accRoll);
 
  Serial.print(" (K)Pitch = ");
  Serial.print(kalPitch);
  Serial.print(" (K)Roll = ");
  Serial.print(kalRoll);
  Serial.println("");  

  Serial.println("end");  

}