#include <Arduino.h>
#include <Wire.h>
#include <MPU6050.h>

MPU6050 mpu;

long cm, duration;
const int echoPin = 10;
const int trigPin = 9;


int16_t gyroX, gyroY, gyroZ;
float angleX, angleY, angleZ;

// Define the motor control pins
const int motor1En = 11;   // PWM pin for motor 1 (clockwise)
const int motor1Input1 = 2; // Motor 1 input 1
const int motor1Input2 = 3; // Motor 1 input 2

const int motor2En = 12;  // PWM pin for motor 2 (counterclockwise)
const int motor2Input1 = 4; // Motor 2 input 1
const int motor2Input2 = 5; // Motor 2 input 2

void setup() {
  // Set motor control pins as OUTPUT
  //pinMode(motor1En, OUTPUT);
  pinMode(motor1Input1, OUTPUT);
  pinMode(motor1Input2, OUTPUT);
  //pinMode(motor2En, OUTPUT);
  pinMode(motor2Input1, OUTPUT);
  pinMode(motor2Input2, OUTPUT);

  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  

  // Initialize the serial communication
  Serial.begin(9600);

    // Initialize MPU-6050
  Wire.begin();
  mpu.initialize();

  // Calibrate the sensor
  mpu.CalibrateGyro();

  // Set the gyro range to +/- 250 degrees per second
 #//mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);  // Use 0x00 as an alternative
}

void loop() {
  wall_avoid();
}

void rot_left() {
  int targetSpeed = 255; // Target speed in RPM (adjust as needed)

  // Set motor directions
  digitalWrite(motor1Input1, HIGH);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, LOW);
  digitalWrite(motor2Input2, LOW);
  delay(1500);

  // Set motor speeds using PWM
  //analogWrite(motor1En, targetSpeed);
  //analogWrite(motor2En, targetSpeed);

  // Print motor speed to serial monitor
  Serial.print("Motor Speed (RPM): ");
  Serial.println(targetSpeed);

  delay(6500); // Delay for 6.5 second
}

void stop() {
  int targetSpeed = 0; // Target speed in RPM

  // Set motor directions
  digitalWrite(motor1Input1, LOW);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, LOW);
  digitalWrite(motor2Input2, LOW);

  // Set motor speeds using PWM
  //analogWrite(motor1En, targetSpeed);
  //analogWrite(motor2En, targetSpeed);

  // Print motor speed to serial monitor
  Serial.print("Motor Speed (RPM): ");
  Serial.println(targetSpeed);

  delay(1000); // Delay for 1 second
}

void forward() {
  int targetSpeed = 255; // Target speed in RPM

  // Set motor directions
  digitalWrite(motor1Input1, HIGH);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, HIGH);
  digitalWrite(motor2Input2, LOW);

  // Set motor speeds using PWM
  //analogWrite(motor1En, targetSpeed);
  //analogWrite(motor2En, targetSpeed);

  // Print motor speed to serial monitor
  Serial.print("Motor Speed (RPM): ");
  Serial.println(targetSpeed);

  delay(500); // Delay for 1 second
}
void rot() {
  int targetSpeed = 255; // Target speed in RPM (adjust as needed)

  // Set motor directions
  digitalWrite(motor1Input1, HIGH);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, LOW);
  digitalWrite(motor2Input2, HIGH);
  //delay(1500);

  // Set motor speeds using PWM
  //analogWrite(motor1En, targetSpeed);
  //analogWrite(motor2En, targetSpeed);

  // Print motor speed to serial monitor
  //Serial.print("Motor Speed (RPM): ");
  //Serial.println(targetSpeed);
 // Delay for 6.5 second
}

void mpu_sense(){
    // Read raw gyro data
  mpu.getRotation(&gyroX, &gyroY, &gyroZ);

  // Convert raw gyro data to degrees per second
  float gyroX_degPerSec = gyroX / 131.0;
  float gyroY_degPerSec = gyroY / 131.0;
  float gyroZ_degPerSec = gyroZ / 131.0;

  // Integrate gyro data to get angle change
  angleX += gyroX_degPerSec * 0.01;  // 0.01 is the time interval in seconds
  angleY += gyroY_degPerSec * 0.01;
  angleZ += gyroZ_degPerSec * 0.01;

  // Print the angles
  Serial.print("X-axis: ");
  Serial.print(angleX);
  Serial.print(" Y-axis: ");
  Serial.print(angleY);
  Serial.print(" Z-axis: ");
  Serial.println(angleZ);

  rot();
  // Check for 180-degree rotation
  if (angleZ >= 3) {
    Serial.print("180-degree rotation detected!");
    
    stop();
    forward();
    // Do something when 180-degree rotation is detected
  }

  delay(10);  // Adjust the delay based on your application's requirements
}


void wall_avoid(){
    // the distance ahead using an ultrasonic sensor
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);

// converting the time into a distance in Centimetre
  
   cm = duration*0.034/2;
 
  if(cm < 7)
  {
    rotateClockwise();
    delay(500);
    mpu_sense();
  }
  
  else
  {
    forward();
  }
  
// For Serial Monitor
Serial.print("Distance:CM ");  
Serial.println(cm); 
  
}


void rotateClockwise() {
  // Set motor direction for clockwise rotation (one side forward, other side backward)
  digitalWrite(motor1Input1, HIGH);
  digitalWrite(motor1Input2, HIGH);
  digitalWrite(motor2Input1, HIGH);
  digitalWrite(motor2Input2, HIGH);
  delay(1000);
  digitalWrite(motor1Input1, LOW);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, LOW);
  digitalWrite(motor2Input2, LOW);
  delay(1000);
  digitalWrite(motor1Input1, HIGH);
  digitalWrite(motor1Input2, HIGH);
  digitalWrite(motor2Input1, HIGH);
  digitalWrite(motor2Input2, HIGH);
  delay(1000);
  digitalWrite(motor1Input1, LOW);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, LOW);
  digitalWrite(motor2Input2, LOW);
  delay(1000);
  digitalWrite(motor1Input1, HIGH);
  digitalWrite(motor1Input2, HIGH);
  digitalWrite(motor2Input1, HIGH);
  digitalWrite(motor2Input2, HIGH);
  delay(1000);
  digitalWrite(motor1Input1, LOW);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, LOW);
  digitalWrite(motor2Input2, LOW);
  delay(2000);
}

void rotateCounterclockwise() {
  // Set motor direction for counterclockwise rotation (one side backward, other side forward)
  digitalWrite(motor1Input1, LOW);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, LOW);
  digitalWrite(motor2Input2, LOW);
  delay(1000);
  digitalWrite(motor1Input1, HIGH);
  digitalWrite(motor1Input2, HIGH);
  digitalWrite(motor2Input1, HIGH);
  digitalWrite(motor2Input2, HIGH);
  delay(1000);
  digitalWrite(motor1Input1, LOW);
  digitalWrite(motor1Input2, LOW);
  digitalWrite(motor2Input1, LOW);
  digitalWrite(motor2Input2, LOW);
  delay(1000);
  digitalWrite(motor1Input1, HIGH);
  digitalWrite(motor1Input2, HIGH);
  digitalWrite(motor2Input1, HIGH);
  digitalWrite(motor2Input2, HIGH);
  delay(1000);
}