#include <AFMotor.h>
#include <LiquidCrystal_I2C.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

const int potPin = A8;
int potValue = 0;

AF_DCMotor motors[4] = {
  AF_DCMotor(1, MOTOR12_64KHZ),
  AF_DCMotor(2, MOTOR12_64KHZ),
  AF_DCMotor(3, MOTOR34_64KHZ),
  AF_DCMotor(4, MOTOR34_64KHZ)
};

const int triggerPin = 25;
const int echoPin = 23;

long duration;
int distance_cm;
int target_dist = 0;

float Kp = 2;
float Ki = 0.1;
float Kd = 0.5;

int PWM_offset = 0;
float integral = 0;
float prev_error = 0;

void setup() {
  Serial.begin(9600);
  Serial.println("Motor Control via Potentiometer");

  for (int i = 0; i < 4; i++) {
    motors[i].setSpeed(0);
  }

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

  lcd.init();
  lcd.backlight();
  lcd.noBlink();
}

void loop() {
  potValue = analogRead(potPin);
  target_dist = map(potValue, 0, 1023, 0, 150);

  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance_cm = duration * 0.034 / 2;

  int error = target_dist - distance_cm;

  integral = integral + error;
  float derivative = error - prev_error;
  prev_error = error;

  float PID_output = Kp * error + Ki * integral + Kd * derivative;

  int motorSpeed = PWM_offset + PID_output;
  motorSpeed = constrain(motorSpeed, 0, 255);

  for (int i = 0; i < 4; i++) {
    motors[i].setSpeed(motorSpeed);
  }

  Serial.print("Motor Speed: ");
  Serial.println(motorSpeed);

  lcd.clear();
  lcd.backlight();
  lcd.setCursor(5, 0);
  lcd.print("Mini Drone:");
  lcd.setCursor(0, 3);
  lcd.print("Actual: ");
  lcd.print(distance_cm);
  lcd.print("cm");
  lcd.setCursor(0, 2);
  lcd.print("Target: ");
  lcd.print(target_dist);
  lcd.print("cm");
  lcd.setCursor(20, 4);
  lcd.print("PWM: ");
  lcd.print(motorSpeed);
  lcd.print("");

  lcd.noBlink();

  if (error > 0) {
    for (int i = 0; i < 4; i++) {
      motors[i].run(FORWARD);
    }
  } else {
    for (int i = 0; i < 4; i++) {
      motors[i].run(BACKWARD);
    }
  }

  delay(100);
}