// A rebuild from scratch of this project: https://wokwi.com/projects/453013204596561921
// Using the same code.
// The display still needs a update to show the text.
// The "include <math.h>" is not needed, that is already included.
// The MPU-6050 does not work yet.


#include <Wire.h>
// #include <math.h>


#define H 100.00000  // Column height in mm
#define DEG_TO_RAD 0.01745329251994329576
#define RAD_TO_DEG 57.29577951308232087679
#define SAFE_LIMIT 5.00000  // Safe tilt limit in degrees

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

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

float baseZ = 0;
bool calibrated = false;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  

  // Initialize MPU6050
  Wire.beginTransmission(0x68);
  if (Wire.endTransmission() != 0) {
    Serial.println(F("ERROR: MPU6050 not detected"));
    while(1);
  }
  
  Wire.beginTransmission(0x68);
  Wire.write(0x6B); Wire.write(0x00);
  Wire.endTransmission();


  delay(1000);
  
  // Calibration
  float x, y, z;
//  readSensor(&x, &y, &z);
  baseZ = z;
  calibrated = true;
  
  Serial.println(F("System Ready"));

  //OLED code
   // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3c)) { // Address 0x3C for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ; // Don't proceed, loop forever
  }
  display.display();
  delay(500); // Pause for 2 seconds
  display.setTextSize(1);
  display.println(F("Column Plumbness Monitoring"));
  display.setTextColor(WHITE);
  display.setRotation(0);
}

bool readSensor(float* x, float* y, float* z) {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  if (Wire.endTransmission(false) != 0) return false;
  
  if (Wire.requestFrom(0x68, 6) != 6) return false;
  
  int16_t rx = Wire.read() << 8 | Wire.read();
  int16_t ry = Wire.read() << 8 | Wire.read();
  int16_t rz = Wire.read() << 8 | Wire.read();
  
  *x = (float)rx / 16384.0;
  *y = (float)ry / 16384.0;
  *z = (float)rz / 16384.0;
  
  return true;
}

void loop() {

  if (!calibrated) return;
  
  float x, y, z;
//  if (!readSensor(&x, &y, &z)) return;
  
  // Calculate tilt angles
  float tiltX = atan2(x, z) * RAD_TO_DEG;
  float tiltY = atan2(y, z) * RAD_TO_DEG;
  
  // Calculate overall tilt
  float overallTilt = sqrt(tiltX * tiltX + tiltY * tiltY);
  
  // Calculate displacements
  float tiltX_rad = tiltX * DEG_TO_RAD;
  float tiltY_rad = tiltY * DEG_TO_RAD;
  
  float dispX = H * tan(tiltX_rad);
  float dispY = H * tan(tiltY_rad);
  
  // Determine axis deviation
  String axisDeviation = "";
  float absX = fabs(tiltX);
  float absY = fabs(tiltY);
  
  if (overallTilt < 0.1) {
    axisDeviation = "No deviation";
  } else if (absX > absY * 2.0) {
    if (tiltX > 0) axisDeviation = "X-Axis: Right";
    else axisDeviation = "X-Axis: Left";
  } else if (absY > absX * 2.0) {
    if (tiltY > 0) axisDeviation = "Y-Axis: Forward";
    else axisDeviation = "Y-Axis: Backward";
  } else {
    if (tiltX > 0 && tiltY > 0) axisDeviation = "X-Axis: Right, Y-Axis: Forward";
    else if (tiltX > 0 && tiltY < 0) axisDeviation = "X-Axis: Right, Y-Axis: Backward";
    else if (tiltX < 0 && tiltY > 0) axisDeviation = "X-Axis: Left, Y-Axis: Forward";
    else if (tiltX < 0 && tiltY < 0) axisDeviation = "X-Axis: Left, Y-Axis: Backward";
  }
  
  // Display results
  Serial.println(F("\n======================================"));
    Serial.println(F("\n Column Plumbness Monitoring"));
  // Tilt angles
  Serial.print(F("Tilt Angles - X: "));
  Serial.print(tiltX, 5);
  Serial.print(F("°, Y: "));
  Serial.print(tiltY, 5);
  Serial.println(F("°"));
  
  // Overall tilt with axis info
  Serial.print(F("Total Tilt: "));
  Serial.print(overallTilt, 5);
  Serial.print(F("° ["));
  Serial.print(axisDeviation);
  Serial.println(F("]"));
  
  // Displacements
  Serial.print(F("Displacements - X: "));
  Serial.print(dispX, 5);
  Serial.print(F(" mm, Y: "));
  Serial.print(dispY, 5);
  Serial.println(F(" mm"));
  
  // Safety status
  Serial.print(F("Status: "));
  if (overallTilt <= SAFE_LIMIT) {
    Serial.println(F("SAFE (≤5°)"));
  } else {
    Serial.println(F("UNSAFE (>5°)"));
  }
  
  Serial.println(F("======================================"));


  delay(2000);

}