/*
  Digital bubble level — Robot Maker Toolkit
  Arduino UNO + MPU-6050 + SSD1306 OLED + push button + buzzer

  A spirit level with a bubble that moves the way the real one does, plus
  the number the glass vial can never give you. Press the button to zero
  it against whatever it is sitting on — that is what makes it useful on
  a shelf that is itself out of true.

  Press       — zero to the current surface (REL). The bubble now shows
                deviation from THIS surface, not from gravity.
  Hold 1 s    — back to absolute (ABS).

  The buzzer chirps once each time it crosses into level, so you can watch
  the thing you are levelling instead of the screen.

  NO SENSOR LIBRARY, ON PURPOSE: the Adafruit MPU6050 stack plus the
  SSD1306 frame buffer does not fit an UNO. Built that way this sketch
  compiles to 80% of flash and 710 bytes of globals, and once the display
  library mallocs its 1 KB buffer there are barely 300 bytes of stack left
  — the screen just stays black. Six registers read straight over Wire do
  the same job in a fraction of the space. On an ESP32 or a Pico, use the
  library.

  PIN NOTE: the MPU-6050 sits at I2C address 0x68 and the OLED at 0x3C, so
  both share A4/A5 with no conflict. Do not add a DS3231 RTC to this bus —
  it is 0x68 too, and it will collide with the sensor.

  Libraries: Adafruit SSD1306, Adafruit GFX. Wire is bundled with the IDE.
*/

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

const int BUTTON_PIN = 3;
const int BUZZER_PIN = 5;

const int SCREEN_WIDTH = 128;
const int SCREEN_HEIGHT = 64;
const int OLED_RESET = -1;
const uint8_t OLED_ADDR = 0x3C;   // a few modules are 0x3D
const uint8_t MPU_ADDR  = 0x68;   // 0x69 if AD0 is tied HIGH

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// The bubble vial: full-scale deflection at TILT_FULL_DEG from centre.
const float TILT_FULL_DEG = 10.0;
const float LEVEL_DEG = 0.4;      // inside this, call it level

const int CX = 95, CY = 30, RING_R = 27, BUBBLE_R = 4;

// Accelerometer output is noisy at rest; without smoothing the bubble
// shivers and the last decimal is meaningless.
const float SMOOTH = 0.15;

float pitch = 0, roll = 0;        // smoothed, degrees
float pitchZero = 0, rollZero = 0;
bool relative = false;
bool wasLevel = false;

void mpuWrite(uint8_t reg, uint8_t value) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(reg);
  Wire.write(value);
  Wire.endTransmission();
}

bool mpuBegin() {
  Wire.beginTransmission(MPU_ADDR);
  if (Wire.endTransmission() != 0) return false;
  mpuWrite(0x6B, 0x00);           // PWR_MGMT_1: clear sleep, it boots asleep
  mpuWrite(0x1C, 0x00);           // ACCEL_CONFIG: +/-2 g, the finest range
  mpuWrite(0x1A, 0x04);           // CONFIG: 21 Hz filter — this never moves fast
  return true;
}

// Raw counts, not g. Every angle here is a ratio of two axes, so the
// scale factor cancels and there is nothing to convert.
bool mpuAccel(int16_t &ax, int16_t &ay, int16_t &az) {
  Wire.beginTransmission(MPU_ADDR);
  Wire.write(0x3B);               // ACCEL_XOUT_H
  if (Wire.endTransmission(false) != 0) return false;
  if (Wire.requestFrom((uint8_t)MPU_ADDR, (uint8_t)6) != 6) return false;

  // Read high then low as separate statements: the order of evaluation
  // inside one expression is not guaranteed, and getting it backwards
  // gives angles that look almost right.
  int16_t hi, lo;
  hi = Wire.read(); lo = Wire.read(); ax = (hi << 8) | (lo & 0xFF);
  hi = Wire.read(); lo = Wire.read(); ay = (hi << 8) | (lo & 0xFF);
  hi = Wire.read(); lo = Wire.read(); az = (hi << 8) | (lo & 0xFF);
  return true;
}

void anglesFrom(int16_t ax, int16_t ay, int16_t az, float &p, float &r) {
  float fx = ax, fy = ay, fz = az;
  r = atan2(fy, fz) * RAD_TO_DEG;
  p = atan2(-fx, sqrt(fy * fy + fz * fz)) * RAD_TO_DEG;
}

void setup() {
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  pinMode(BUZZER_PIN, OUTPUT);

  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    // Nothing to draw on — blink the built-in LED so the failure is visible.
    pinMode(LED_BUILTIN, OUTPUT);
    while (true) {
      digitalWrite(LED_BUILTIN, HIGH); delay(200);
      digitalWrite(LED_BUILTIN, LOW);  delay(200);
    }
  }
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.setTextSize(1);

  if (!mpuBegin()) {
    display.setCursor(0, 20);
    display.print(F("No MPU-6050 at 0x68"));
    display.setCursor(0, 34);
    display.print(F("check SDA/SCL + AD0"));
    display.display();
    while (true) delay(1000);
  }

  // Seed the filter so the bubble does not fly in from the corner.
  int16_t ax, ay, az;
  if (mpuAccel(ax, ay, az)) anglesFrom(ax, ay, az, pitch, roll);
}

void readAngles() {
  int16_t ax, ay, az;
  if (!mpuAccel(ax, ay, az)) return;

  float p, r;
  anglesFrom(ax, ay, az, p, r);
  roll  += SMOOTH * (r - roll);
  pitch += SMOOTH * (p - pitch);
}

void drawValue(int y, const __FlashStringHelper *label, float deg) {
  display.setTextSize(1);
  display.setCursor(0, y);
  display.print(label);

  display.setTextSize(2);
  display.setCursor(0, y + 10);
  if (fabs(deg) < 0.05) deg = 0;      // stop -0.0 flickering into view
  display.print(deg, 1);
}

void draw(float p, float r, bool level) {
  display.clearDisplay();

  drawValue(0, F("PITCH"), p);
  drawValue(30, F("ROLL"), r);

  display.setTextSize(1);
  display.setCursor(0, 56);
  display.print(relative ? F("REL") : F("ABS"));
  if (level) {
    display.setCursor(26, 56);
    display.print(F("LEVEL"));
  }

  // The vial: outer ring, the tolerance circle, and a crosshair.
  display.drawCircle(CX, CY, RING_R, SSD1306_WHITE);
  display.drawCircle(CX, CY, 6, SSD1306_WHITE);
  display.drawFastHLine(CX - RING_R, CY, RING_R * 2, SSD1306_WHITE);
  display.drawFastVLine(CX, CY - RING_R, RING_R * 2, SSD1306_WHITE);

  // Bubble. Roll pushes it sideways, pitch pushes it up the screen — the
  // same direction the air pocket in a glass vial goes.
  float travel = RING_R - BUBBLE_R - 1;
  float bx = constrain(r / TILT_FULL_DEG, -1.0, 1.0) * travel;
  float by = constrain(p / TILT_FULL_DEG, -1.0, 1.0) * travel;
  display.fillCircle(CX + (int)bx, CY - (int)by, BUBBLE_R, SSD1306_WHITE);

  display.display();
}

void serviceButton() {
  static bool wasDown = false;
  static unsigned long downAt = 0;
  static bool longFired = false;

  bool down = digitalRead(BUTTON_PIN) == LOW;

  if (down && !wasDown) {
    downAt = millis();
    longFired = false;
    delay(20);                       // ride out the contact bounce
  } else if (down && !longFired && millis() - downAt > 1000) {
    pitchZero = rollZero = 0;        // back to gravity
    relative = false;
    longFired = true;
    tone(BUZZER_PIN, 900, 120);
  } else if (!down && wasDown) {
    if (!longFired) {
      pitchZero = pitch;             // this surface is now "flat"
      rollZero = roll;
      relative = true;
      tone(BUZZER_PIN, 1800, 60);
    }
    delay(20);
  }
  wasDown = down;
}

void loop() {
  serviceButton();
  readAngles();

  float p = pitch - pitchZero;
  float r = roll - rollZero;
  bool level = fabs(p) < LEVEL_DEG && fabs(r) < LEVEL_DEG;

  // Chirp on the crossing, not for as long as it stays there.
  if (level && !wasLevel) tone(BUZZER_PIN, 2400, 80);
  wasLevel = level;

  draw(p, r, level);
  delay(40);
}