// speedSpeedometerStepperServo.pde
// -*- mode: C++ -*-
//
// Use a Potentiometer to control the speed of several motors:
// a stepper motor with drivers with speed proportional to pot
// a stepper motor with drivers with position proportional to pot
// a servo motor porportional to pot
// a led/DC motor proportional to pot
// a led/DC motor proportional and opposite to pot
// leds/reversibleDC motor proportional to bidirectioal pot 
//

// Sim: https://wokwi.com/projects/330649945363186260
// Discussion: https://discord.com/channels/787627282663211009/787630013658824707/957769150556667954
//         and https://github.com/wokwi/wokwi-features/issues/191
// Docs:  https://docs.wokwi.com/parts/wokwi-stepper-motor
//   
// See also: https://wokwi.com/projects/327381547863769683 W/o drivers
//           https://wokwi.com/projects/327613078439985746 MultiStepper
// 

#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <MPU6050.h>
#include <AccelStepper.h>
#include <Servo.h>
#include <Ultrasonic.h>

// LCD
LiquidCrystal_I2C lcd(0x27, 20, 4);

// MPU6050
MPU6050 mpu;

// Pins
const byte ledUpPin = 2;
const byte ledDownPin = 6;
const byte buzzerPin = 4;

const byte dirPin = 11;
const byte stepPin = 12;
const byte dirPin1 = 5;
const byte stepPin1 = 8;

const byte servoPin = 3;
Servo myservo;

// Ultrasonic Sensor
const byte trigPin = 10;
const byte echoPin = 9;
Ultrasonic ultrasonic(trigPin, echoPin);

// Stepper motors
AccelStepper stepper(AccelStepper::DRIVER, stepPin, dirPin);
AccelStepper stepper2(AccelStepper::DRIVER, stepPin1, dirPin1);

// Motor RPM
float motorSpeed = 0;
float motorRPM = 0;
float maxRPM = 50;

// Pothole threshold
const int potholeThreshold = -15000;

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

  lcd.init();
  lcd.backlight();
  lcd.clear();

  mpu.initialize();

  stepper.setMaxSpeed(2000);
  stepper.setSpeed(0);
  stepper2.setMaxSpeed(2000);
  stepper2.setSpeed(0);

  myservo.attach(servoPin);

  pinMode(ledUpPin, OUTPUT);
  pinMode(ledDownPin, OUTPUT);
  pinMode(buzzerPin, OUTPUT);
}

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

  // Potentiometer for RPM
  int potState = analogRead(A0);
  motorSpeed = map(potState, 0, 1023, 0, 255);
  motorRPM = (motorSpeed / 255.0) * maxRPM;

  // Ultrasonic Distance
  long distance = ultrasonic.read(CM);

  // LCD Display
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Motor RPM: ");
  lcd.print(motorRPM);

  lcd.setCursor(0, 1);
  lcd.print("Acc: ");
  lcd.print(ax); lcd.print(", "); lcd.print(ay); lcd.print(", "); lcd.print(az);

  lcd.setCursor(0, 2);
  lcd.print("Gyro: ");
  lcd.print(gx); lcd.print(", "); lcd.print(gy); lcd.print(", "); lcd.print(gz);

  lcd.setCursor(0, 3);
  lcd.print("Distance: ");
  lcd.print(distance); lcd.print(" cm");

  // --- Pothole & Obstacle Detection Logic ---
  bool potholeDetected = az < potholeThreshold;
  bool obstacleClose = distance < 50;

  if (obstacleClose) {
    Serial.println("⚠️ Alert: Pothole or Obstacle Detected!");
    digitalWrite(ledUpPin, HIGH);
    digitalWrite(ledDownPin, LOW);
    digitalWrite(buzzerPin, HIGH);
  }
  else if(potholeDetected ) {
    digitalWrite(ledUpPin, HIGH);
    digitalWrite(ledDownPin, LOW);
    digitalWrite(buzzerPin, LOW);
  }
  else {
    digitalWrite(ledUpPin, LOW);
    digitalWrite(ledDownPin, HIGH);
    digitalWrite(buzzerPin, LOW);
  }

  delay(500);
}