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

// OLED Display Dimensions
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_I2C_ADDRESS 0x3C  // Change to 0x3D if needed

// Create Display and MPU6050 Objects
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Adafruit_MPU6050 mpu;

// Create Servo Objects for Legs
Servo leg1, leg2, leg3, leg4;

// Ultrasonic Sensor Pins and Safe Distance Threshold
const int trigPin = 6;
const int echoPin = 7;
const int safeDistance = 20; // Safe distance in cm

void setup() {
  // Start Serial Communication
  Serial.begin(9600);

  // Initialize OLED Display
  if (!display.begin(SSD1306_SWITCHCAPVCC, OLED_I2C_ADDRESS)) {
    Serial.println("Error: OLED not found!");
    while (1); // Stop program if OLED fails
  }
  display.clearDisplay();
  
  // Display Starting Parameters
  displayMessage("Initializing...");
  delay(2000);  // Show for 2 seconds

  // Initialize MPU6050 Sensor
  if (!mpu.begin()) {
    Serial.println("Error: MPU6050 not found!");
    displayMessage("MPU6050 Error!");
    while (1); // Stop program if MPU6050 fails
  }

  // Display MPU6050 Initialization Success
  displayMessage("MPU6050 Initialized!");
  delay(2000);  // Show for 2 seconds

  // Attach Servo Motors to Their Pins
  leg1.attach(9);
  leg2.attach(10);
  leg3.attach(11);
  leg4.attach(12);

  // Set Ultrasonic Sensor Pin Modes
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Show Ready Status before starting
  displayMessage("Ready to Move!");
  delay(2000);  // Show for 2 seconds
}

void loop() {
  // Measure the distance to the nearest object
  float distance = measureDistance();

  // Get accelerometer and gyroscope data
  sensors_event_t accelEvent, gyroEvent, magEvent;

  // Read accelerometer and gyroscope values
  mpu.getEvent(&accelEvent, &gyroEvent, &magEvent);

  // Update the OLED with the measured distance, accelerometer, and gyroscope data
  display.clearDisplay();
  
  display.setTextSize(1);   // Set text size to 1 for small text
  display.setTextColor(SSD1306_WHITE);  // Set text color
  
  // Display status
  display.setCursor(0, 0);  // Move cursor to top-left
  if (distance < safeDistance) {
    display.println("Obstacle Ahead!");
    avoidObstacle();  // Move backward 5 steps when obstacle detected
  } else {
    display.println("Walking Forward...");
    walkForward();  // Walk forward when no obstacle
  }
  
  // Display distance
  display.setCursor(0, 10);
  display.print("Dist: ");
  display.print(distance);
  display.println(" cm");

  // Display accelerometer data
  display.setCursor(0, 20);
  display.print("Acc X: "); display.print(accelEvent.acceleration.x);
  display.print(" Y: "); display.print(accelEvent.acceleration.y);
  display.print(" Z: "); display.println(accelEvent.acceleration.z);

  // Display gyroscope data
  display.setCursor(0, 40);
  display.print("Gyro X: "); display.print(gyroEvent.gyro.x);
  display.print(" Y: "); display.print(gyroEvent.gyro.y);
  display.print(" Z: "); display.println(gyroEvent.gyro.z);

  display.display();  // Update the OLED display

  delay(1000);  // Add a longer delay before updating again
}

// Measure distance using the ultrasonic sensor
float measureDistance() {
  digitalWrite(trigPin, LOW);   // Clear the trigger pin
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);  // Send a 10us pulse
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  float duration = pulseIn(echoPin, HIGH); // Get echo time
  float distance = duration * 0.034 / 2;   // Convert to cm

  Serial.print("Distance: ");
  Serial.print(distance);
  Serial.println(" cm");

  return distance;
}

// Simulate walking forward by moving servos
void walkForward() {
  // Move legs to simulate walking
  leg1.write(45);
  leg2.write(135);
  leg3.write(135);
  leg4.write(45);
  delay(200); // Wait for movement

  // Reset legs to neutral position
  leg1.write(90);
  leg2.write(90);
  leg3.write(90);
  leg4.write(90);
  delay(200); // Wait for movement
}

// Simulate obstacle avoidance by stepping back 5 times
void avoidObstacle() {
  // Move backward 5 steps
  for (int i = 0; i < 5; i++) {
    leg1.write(135);  // Move leg1 back
    leg2.write(45);   // Move leg2 back
    leg3.write(45);   // Move leg3 back
    leg4.write(135);  // Move leg4 back
    delay(200);       // Wait for movement

    // Reset legs to neutral position
    leg1.write(90);
    leg2.write(90);
    leg3.write(90);
    leg4.write(90);
    delay(200);       // Wait for movement
  }

  // Display a message after moving backward
  displayMessage("Moved Backward!");
}

// Display a message on the OLED screen
void displayMessage(String message) {
  display.clearDisplay();         // Clear previous message
  display.setTextSize(1);          // Set text size
  display.setTextColor(SSD1306_WHITE); // Set text color
  display.setCursor(0, 0);        // Set text cursor position
  display.println(message);       // Print the message
  display.display();              // Update the display

  // Print message to Serial for debugging
  Serial.println(message);
}