/*
 * Niveau à bulle numérique — Kaihatsu Lab
 * MPU6050 + écran OLED SSD1306, tous deux sur le même bus I2C (SDA 21, SCL 22).
 * La bulle va du côté le plus haut, comme sur un vrai niveau. Bibliothèques : Adafruit GFX et SSD1306.
 *
 * Monte le MPU6050 à plat avec ses axes dans le même sens que l'écran : X vers la droite, Y vers le haut.
 * Chiffres affichés : P = pitch (autour de Y), R = roll (autour de X).
 */
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#define SCREEN_W  128
#define SCREEN_H  64
#define OLED_ADDR 0x3C
const uint8_t MPU_ADDR = 0x68;

Adafruit_SSD1306 display(SCREEN_W, SCREEN_H, &Wire, -1);

const int   CX = 40, CY = 32, R_OUT = 28;   // cadran à gauche, chiffres à droite
const float PIXELS_PER_DEG = 2.0;           // sensibilité de la bulle
float roll = 0, pitch = 0;                  // degrés, lissés

int16_t read16() {
  uint8_t hi = Wire.read();
  uint8_t lo = Wire.read();
  return (int16_t)((hi << 8) | lo);
}

bool readAngles(float &r, float &p) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom(MPU_ADDR, (uint8_t)6) != 6) return false;
  float ax = read16() / 16384.0;
  float ay = read16() / 16384.0;
  float az = read16() / 16384.0;
  r = atan2(ay, az) * RAD_TO_DEG;
  p = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;
  return true;
}

void setup() {
  Wire.begin(21, 22);
  Wire.setClock(400000);
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
}

void loop() {
  float r, p;
  if (readAngles(r, p)) {
    roll  += 0.2 * (r - roll);              // lissage exponentiel : la bulle ne tremble pas
    pitch += 0.2 * (p - pitch);
  }

  float bx = CX - pitch * PIXELS_PER_DEG;   // tangage positif : le côté droit descend, la bulle va à gauche
  float by = CY - roll * PIXELS_PER_DEG;    // roulis positif : le haut monte, la bulle monte
  float dx = bx - CX, dy = by - CY;
  float dist = sqrt(dx * dx + dy * dy), maxD = R_OUT - 5;
  if (dist > maxD) { bx = CX + dx * maxD / dist; by = CY + dy * maxD / dist; }

  display.clearDisplay();
  display.drawCircle(CX, CY, R_OUT, SSD1306_WHITE);       // cadran
  display.drawCircle(CX, CY, 7, SSD1306_WHITE);           // cible centrale
  display.drawFastHLine(CX - R_OUT, CY, 6, SSD1306_WHITE);
  display.drawFastHLine(CX + R_OUT - 5, CY, 6, SSD1306_WHITE);
  display.fillCircle((int)bx, (int)by, 5, SSD1306_WHITE); // bulle

  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(82, 14);
  display.print(F("P "));
  display.print(pitch, 1);
  display.setCursor(82, 34);
  display.print(F("R "));
  display.print(roll, 1);
  display.display();
  delay(20);
}