//----------------------------------------------------------------------
//  デジタル水準器                                        2018.8.19 naka
//
//  マイコン：Arduino Nano, ATMEGA328Pなど
//  加速度センサ：MPU-6050 (I2C)                       アマゾンで300円弱
//  OLED表示機  ：0.96インチ 128x64ドット有機EL (I2C) アマゾンで500円弱
//
//  ピンアサイン
//    SCL(I2C)             : PC5
//    SDA(I2C)             : PC4
//    キャリブレーションSW : PD2
//    動作モードSW         : PD3
//    圧電ブザー           : PD5
//
//   機能
//    (1) 動作モード（スイッチを押すつど以下を切り替え）
//        ボール表示（傾けた方向に移動）     0.1度単位
//                                             1度単位
//        気泡表示  （傾けた方向と逆に移動） 0.1度単位
//                                             1度単位
//    (2) 水平時のピッ音（スイッチでオンオフ切り替え）
//        水平になったときに音を出す
//
//    (3) 水平設定キャリブレーション
//        スイッチを押し、離してから１秒後に、その時の傾きを水平とする
//        （スイッチを押すことで傾いてしまうので１秒後に判断）
//        ★プログラムを書き込んで最初に電源を入れた際に、
//          キャリブレーション必須
//
//    (4) 設定情報のEEPROMへの記憶
//        次回電源オン時には、以前の設定を読み出して動作
//
// バグがあるかもしれません、無保証です。
// 著作権は留保しますが、改変などご自由にどうぞ。
//----------------------------------------------------------------------
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <EEPROM.h>

#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);

#define MPU6050ADDR 0x68
#define OLEDADDR 0x3C
#define PIN_CALI 2
#define PIN_MODE 3
#define PIN_BUZZER 5
#define PIN_TONE 9
#define TONE_FREQ 880
#define TONE_DURA 100

unsigned char mode, buzzer;
char display_center_x, display_center_y;
char prev_x, prev_y;
float offset_pitch,offset_roll;

void setup() {
  pinMode(PIN_CALI, INPUT);    // キャリブレーション用ピンを入力に設定
  digitalWrite(PIN_CALI , HIGH);  // 内蔵プルアップ
  pinMode(PIN_MODE, INPUT);   // 表示モード切替用ピンを入力に設定
  digitalWrite(PIN_MODE , HIGH);  // 内蔵プルアップ
  pinMode(PIN_BUZZER, INPUT);   // ブザー切替用ピンを入力に設定
  digitalWrite(PIN_BUZZER , HIGH);  // 内蔵プルアップ
  pinMode(PIN_TONE, OUTPUT);    // 圧電スピーカ用ピン

  display.begin(SSD1306_SWITCHCAPVCC, OLEDADDR);  // OLED初期化
  display.clearDisplay();       // OLED画面クリア
  display_center_x = 32;
  display_center_y = 32;
  prev_x = display_center_x;
  prev_y = display_center_y;

  init_mpu6050();   // 加速度センサ初期化
  read_eeprom();    // EEPROM格納データ(キャリブレーション、各設定)
  disp_title();     // タイトル(LEVEL)表示
  disp_mode();      // 表示モードの表示
}

void init_mpu6050() {
  Wire.beginTransmission(MPU6050ADDR);
  Wire.write(0x6B); // PWR_MGMT_1
  Wire.write(0x00);
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050ADDR);
  Wire.write(0x1A); // CONFIG
  Wire.write(0x04); // DLPF_CFG 4 (delay 21ms)
  Wire.endTransmission();
  Wire.beginTransmission(MPU6050ADDR);
  Wire.write(0x1C); // ACCEL_CONFIG
  Wire.write(0x00); // AFS_SEL ±2g
  Wire.endTransmission();
}

void disp_title() {
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(63, 0);
  display.println("LEVEL");
  display.display();
}

void read_eeprom() {
  int i;
  char c_data[4];
  
  for (i=0;i<4;i++) {
    c_data[i] = (char)EEPROM.read(0+i);  // キャリブレーション時のpitch
  }
  offset_pitch = c2f(c_data);
  for (i=0;i<4;i++) {
    c_data[i] = (char)EEPROM.read(4+i);  // キャリブレーション時のroll
  }
  offset_roll = c2f(c_data);

  mode = EEPROM.read(8);
  if (mode<0 || mode>4) {   // EEPROMに設置値が入ってないケースを想定
    mode = 0;
  }
  buzzer = EEPROM.read(9); // ブザー音オン／オフ
  if (buzzer!=0 && buzzer!=1) { // EEPROMに設置値が入ってないケースを想定
    buzzer = 0;
  }
}

float c2f (char cdata[]) {  // char -> float
  int i;
  union {
    char cdata[4];
    float fdata;
  } uni;
  for (i=0;i<4;i++) {
    uni.cdata[i] = cdata[i];
  }
  return (uni.fdata);
}

void f2c (char cdata[],float fdata) {  // float -> char
  int i;
  union {
    char cdata[4];
    float fdata;
  } uni;
  uni.fdata = fdata;
  for (i=0;i<4;i++) {
    cdata[i] = uni.cdata[i];
  }
  return;
}

void loop() {
  int32_t rawX, rawY, rawZ;
  float aveX, aveY, aveZ;
  float pitch, roll, dpitch, droll;
  float ab, ad, bd, bp, ap;
  int dir;
  int nCount = 32;
  int i;
  float tilt;
  int i_tilt;
  char cdata[4];
  boolean calib;
  int radius;

  if (digitalRead(PIN_MODE)==LOW) { // 動作モード
    mode++;
    if (mode>=4) {
      mode = 0;
    }
    EEPROM.write(8, mode);
    disp_mode();
    while(!digitalRead(PIN_MODE)) {};
  }
  
  if (digitalRead(PIN_BUZZER)==LOW) { // ブザー音
    buzzer ^= 0x01;
    EEPROM.write(9, buzzer);
    disp_mode();
    while(!digitalRead(PIN_BUZZER)) {};
  }

  calib = false;
  if (digitalRead(PIN_CALI)==LOW) { // キャリブレーション
    while(!digitalRead(PIN_CALI)) {};
    delay(1000);   // スイッチを開放して１秒後にキャリブレーションを行う
    calib = true;
  }

  // 加速度センサ値を読み取る
  for (i = 0; i < nCount; i++) {
    Wire.beginTransmission(MPU6050ADDR);
    Wire.write(0x3B); // ここから ACCEL_XOUT[16bit],Y[16bit],Z[16bit]
    Wire.endTransmission();
    Wire.requestFrom(MPU6050ADDR, 6);
    while (Wire.available() < 6);
    rawX += Wire.read() << 8 | Wire.read();
    rawY += Wire.read() << 8 | Wire.read();
    rawZ += Wire.read() << 8 | Wire.read();
  }
  aveY = rawX / nCount;  // OLEDの向きと傾きを合わせるためにX/Y入れ替え  
  aveX = rawY / nCount;
  aveZ = rawZ / nCount;

  pitch = atan2(aveX, sqrt(aveY * aveY + aveZ * aveZ));
  roll  = atan2(aveY, aveZ);

  if (calib==true) {
    offset_pitch = pitch;
    f2c(cdata,offset_pitch);
    for (i=0;i<4;i++) {
      EEPROM.write(i, cdata[i]);
    }
    offset_roll  = roll;
    f2c(cdata,offset_roll);
    for (i=0;i<4;i++) {
      EEPROM.write(4+i, cdata[i]);
    }

    display.setTextSize(2);
    display.fillRect(0,22,127,22,BLACK);
    display.setTextColor(WHITE);
    display.setCursor(0, 24);
    display.println("Calibrated");
    display.display();
    delay(500);

    display.fillRect(0,22,127,22,BLACK);
    disp_mode();
  }

  pitch -= offset_pitch;  // キャリブレーション時のpitch
  roll  -= offset_roll;   // キャリブレーション時のroll

  if (pitch>89.9999 / 180 * PI) {  // 90度ピッタリだとエラーになるので。
    pitch -= 0.0001;
  }
  if (roll>89.999 / 180 * PI) {  // 90度ピッタリだとエラーになるので。
    roll -= 0.0001;
  }
  ab = tan(90.0 / 180.0 * PI - abs(pitch));
  ad = tan(90.0 / 180.0 * PI - abs(roll));
  bd = sqrt(ab * ab + ad * ad);
  bp = (ab * ab - ad * ad + bd * bd) / (bd * 2.0);
  ap = sqrt(ab * ab - bp * bp);

  tilt = atan(1.0 / ap) * 180.0 / PI;
  dir       = int(acos(ap / ab) * 180.0 / PI + 0.5);

  if (pitch < 0 && roll >= 0) {
    dir = 180 - dir;
  }
  else if (pitch <0 && roll <0) {
    dir = 180 + dir;
  }
  else if (pitch >= 0 && roll < 0) {
    dir = 360 - dir;
  }
  else if (pitch >= 0 && roll >= 0) {
    dir += 0;
  }

  // 傾きをボール／気泡を表示する位置の半径に変換
  if (mode==0 || mode==2) { 
    if (tilt < 1.4) {
     radius = (int)(tilt * 20.0 + 0.5);
    }
   else {
    radius = 28;
   }
  }
  else {
    i_tilt = (int)(tilt * 2.0 + 0.5);
    if (tilt < 14.0) {
      radius = i_tilt;
    }
   else {
    radius = 28;
   }    
  }

  if (mode==2 || mode==3) { // 気泡
    dir = (dir)%360;    
  }
  else { // ボール
    dir = (dir+180)%360;    
  }
  char x = display_center_x + radius * cos(float(dir) / 180.0 * PI) + 0.5;
  char y = display_center_y + radius * sin(float(dir) / 180.0 * PI) + 0.5;
  char text[10],textf[5];
  
  if (mode==0 || mode==2) { // 精度0.1度
    dtostrf(tilt, 3, 1, textf);
  }
  else {
    sprintf(textf,"%3d",i_tilt);    
    if (i_tilt==0) {
      dir = 360;
    }
  }
  display.fillRect(70,35,57,20,BLACK);
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(70, 35);
  sprintf(text,"Dir:%4d",360-dir);
  display.println(text);
  display.setCursor(70, 45);
  if (tilt>=10.0) {
    tilt = 9.9;
  }
  sprintf(text,"Tilt:%s",textf);
  display.println(text);
  display.setCursor(69, 55);
  display.println("[degree]");
  display.display();

  if ((prev_x!=display_center_x || prev_y!=display_center_x) &&
      x==display_center_x && y==display_center_y && buzzer==1) {
    tone(PIN_TONE,TONE_FREQ,TONE_DURA);
  }
  draw_screen(x,y);
  
}

void draw_screen(int x, int y) {
  display.fillCircle(prev_x, prev_y, 5, BLACK);
  display.drawLine(display_center_x,0,display_center_x,25, WHITE);
  display.drawLine(display_center_x,39,display_center_x,63, WHITE);
  display.drawLine(0, display_center_y,25,display_center_y, WHITE);
  display.drawLine(39, display_center_y,63,display_center_y, WHITE);
  display.drawCircle(display_center_x,display_center_y, 31, WHITE);
  display.drawCircle(display_center_x,display_center_y, 7, WHITE);
  if (mode==0 || mode==1) {
    display.fillCircle(x, y, 5, WHITE);
  }
  else {
    display.drawCircle(x, y, 5, WHITE);    
  }
  prev_x = x;
  prev_y = y;
  display.display();
}

void disp_mode() {
  static const unsigned char PROGMEM icon[] = { // 音符記号アイコン
    B00001000,
    B00001100,
    B00001110,
    B00001010,
    B00001010,
    B00111000,
    B01111000,
    B00110000 };
  display.fillRect(70,18,58,8,BLACK);  
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(70,18);
  if (mode==0) {
    display.println("(Ball-01)");
  }  
  else if (mode==1) {
    display.println("(Ball-1)");
  }
  else if (mode==2) {
    display.println("(Bubb-01)");  
  }
  else {
    display.println("(Bubb-1)");  
  }
  
  if (buzzer==1) {
    display.drawBitmap(120,0,icon,8,8,WHITE);    
  }
  else {
    display.fillRect(120,0,8,8,BLACK);  
  }
}