/*************************************************************
 
  DigiLevel, Version 3.0
  A digital level for personal use only. No warranty is made
  as to accuracy of any displayed values.
  Keith C. Roberts

*************************************************************/

#include <Arduino.h>

// Graphics support for OLED display (SSD1306)
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// Accelerometer support (MPU6050)
#include <MPU6050.h>
#include <Wire.h>

// Software version number
#define VERSION "3.0"

// OLED display setup
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// MPU6050 setup
MPU6050 mpu;

//
// Set these variables with initial values as appropriate
//
int xCalibration = 0;        // calibration amount for leveling out the x-axis
int yCalibration = 0;        // calibration amount for leveling out the y-axis
long irvCalibration = 1457;  // calibration amount for internal reference voltage

//
// Global variables - do not change
//
int16_t ax, ay, az;  // Accelerometer readings
int xAngle;
int yAngle;
int offset = 37;
int xOffset = 0;
int yOffset = 0;
int counter = 300;
long voltage = 0;
bool lastLevelStatus = false; // To store the last level status

// Initialization routine
void setup(void) {
  Serial.begin(9600);  // Start serial for debugging

  // Initialize MPU6050
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("Failed to connect to MPU6050");
    while (1) {
      // Halt execution
    }
  } else {
    Serial.println("MPU6050 connection successful");
  }

  // Initialize SSD1306 OLED Display (128 x 64)
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }
  display.display();
  delay(2000);  // Length of time the credits are displayed
  display.clearDisplay();

  showCredits();
  delay(2000);
  display.clearDisplay();

  beep(1);  // One beep when device starts
  delay(500);
}

// Main processing loop
void loop(void) {
  // Read the MPU6050 to get the angles and the current temperature
  mpu.getMotion6(&ax, &ay, &az, NULL, NULL, NULL);

  xAngle = (ax / 16384.0 + xCalibration) * 90;
  yAngle = (ay / 16384.0 + yCalibration) * 90;

  // Debugging output
  Serial.print("xAngle: "); Serial.print(xAngle);
  Serial.print(" yAngle: "); Serial.println(yAngle);

  // Check if the device is level
  bool level = isLevel(xAngle, yAngle);

  // Update the display
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("DigiLevel");
  display.setCursor(0, 12);
  display.print("H:" + String(abs(xAngle)) + char(176));
  display.setCursor(0, 24);
  display.print("V:" + String(abs(yAngle)) + char(176));
  display.setCursor(0, 56);
  display.setTextSize(2);
  drawLegend();
  drawPoint(xAngle, yAngle);
  counter = setBatteryLevel(counter);
  display.display();
  delay(100);

  // Beep logic based on level status changes
  if (level != lastLevelStatus) {
    if (level) {
      beep(3); // Three beeps for level
    } else {
      beep(2); // Two beeps for tilt
    }
    lastLevelStatus = level; // Update last level status
  }
}

// Show credits at startup
void showCredits(void) {
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 12);
  display.print("DigiLevel");
  display.setCursor(0, 24);
  display.print("Version " + String(VERSION));
  display.display();
}

// Draw the chart for displaying relative angles
void drawLegend(void) {
  // Draw horizontal line
  display.drawLine(offset, 31, offset + 90, 31, SSD1306_WHITE);
  display.drawLine(offset, 27, offset, 35, SSD1306_WHITE);
  display.drawLine(offset + 90, 27, offset + 90, 35, SSD1306_WHITE);
  // Draw vertical line
  display.drawLine(offset + 45, 0, offset + 45, 63, SSD1306_WHITE);
  display.drawLine(offset + 41, 0, offset + 49, 0, SSD1306_WHITE);
  display.drawLine(offset + 41, 63, offset + 49, 63, SSD1306_WHITE);
}

// Draw the level's position (xAngle, yAngle) on the graph
void drawPoint(int x, int y) {
  xOffset = offset + (x / 2) + 45;
  yOffset = 31 - (y / 2);
  if (yOffset < 0) {
    yOffset = 0;
  } else if (yOffset > 63) {
    yOffset = 63;
  }
  display.fillCircle(xOffset, yOffset, 3, SSD1306_WHITE);
}

// Creates one or more beeps if the optional Piezo speaker is attached to pin D7
void beep(int cnt) {
  for (int idx = 1; idx <= cnt; idx++) {
    tone(7, 500, 100);  // values are: digital pin, hertz, duration (ms)
    delay(200);
  }
}

// Checks to see if the DigiLevel is level (within 0-2 degrees)
bool isLevel(int x, int y) {
  return x < 2 && y < 2;
}

// Determines battery level and displays icon in the upper right of the display
int setBatteryLevel(int cnt) {
  if (cnt == 300) {
    voltage = (readVcc() / 100) - 37;
    if (voltage < 0) {
      voltage = 0;
    }
    cnt = 0;
  } else {
    cnt = cnt + 1;
  }
  display.drawRect(116, 0, 11, 5, SSD1306_WHITE);
  display.fillRect(117, 0, 2 * voltage, 5, SSD1306_WHITE);
  return cnt;
}

// Reads the internal voltage
long readVcc() {
  long result;
  // Read 1.1V reference against AVcc
  ADMUX = _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1);
  delay(2);  // Wait for Vref to settle
  ADCSRA |= _BV(ADSC);  // Convert
  while (bit_is_set(ADCSRA, ADSC));
  result = ADCL;
  result |= ADCH << 8;
  result = irvCalibration * 1024 / result;  // Back-calculate AVcc in mV
  return result;
}