#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Encoder.h>
#include <MPU6050_tockn.h>

// Define components
MPU6050 mpu;
Servo myServo;
Adafruit_SSD1306 display(128, 64, &Wire);
Encoder myEnc(27, 26);  // Rotary encoder pins (CLK and DT)

// Pin definitions
const int buzzerPin = 14;
const int potentiometerPin = 34;
const int buttonPin = 17;

// Variables
long lastEncoderValue = 0;

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

  // Initialize I2C and MPU6050
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed!");
    while (1); // Stop program if connection fails
  }
  Serial.println("MPU6050 connected.");

  // Initialize OLED display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println("SSD1306 OLED initialization failed!");
    while (1);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.println("Enhanced Robotic Arm");
  display.display();
  mpu.begin();
  mpu.calcGyroOffsets(true); // Calibrate MPU6050

  // Attach servo to pin
  myServo.attach(13);

  // Configure pins
  pinMode(buzzerPin, OUTPUT);
  pinMode(buttonPin, INPUT_PULLUP);

  // Display initial message
  delay(2000);  // Allow time to read the startup message
  display.clearDisplay();
  display.display();
}

void loop() {
  // Read MPU6050 data
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // Calculate angle (example using x-axis data)
  int angle = map(ax, -17000, 17000, 0, 180);
  angle = constrain(angle, 0, 180);  // Ensure angle stays within servo range
  mpu.update();
  int angle = map(mpu.getAccX(), -1, 1, 0, 180);
  // Move servo based on MPU6050 angle
  myServo.write(angle);

  // Read potentiometer value and map to servo angle
  int potValue = analogRead(potentiometerPin);
  int potAngle = map(potValue, 0, 4095, 0, 180);

  // Read rotary encoder value
  long encoderValue = myEnc.read();
  if (encoderValue != lastEncoderValue) {
    Serial.print("Encoder: ");
    Serial.println(encoderValue);
    lastEncoderValue = encoderValue;
  }

  // Display data on OLED
  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("MPU Angle: ");
  display.println(angle);
  display.print("Potentiometer: ");
  display.println(potAngle);
  display.print("Encoder: ");
  display.println(encoderValue);
  display.display();

  // Check button state and control buzzer
  if (digitalRead(buttonPin) == LOW) {
    tone(buzzerPin, 1000);  // Buzzer ON
  } else {
    noTone(buzzerPin);      // Buzzer OFF
  }

  // Serial output for debugging
  Serial.print("MPU Angle: ");
  Serial.print(angle);
  Serial.print(" | Potentiometer: ");
  Serial.print(potAngle);
  Serial.print(" | Encoder: ");
  Serial.println(encoderValue);

  delay(100);  // Small delay for stability
}