#include <Wire.h>
#include <Servo.h>
#include <LiquidCrystal_I2C.h>
#include <Stepper.h>

LiquidCrystal_I2C lcd(0x27, 16, 2);

const int ledmerah = 2; //conveyor off
const int ledPin = 3; //conveyor standby
const int ledPin2 = 4; //conveyor jalan led hijau nyala
const int leddeteksi = 5; //logam terdeteksi
const int buttonPin = 6; //start
const int buttonstop = 7; //stop

#define trigPin 8  // SENSOR ultrasonik 1 
#define echoPin 9

#define trigPin2 10  // SENSOR proxi 
#define echoPin2 11

#define trigPin3 13  // SENSOR ultrasonik 2
#define echoPin3 14
const int potPin = A1;
const int stepsPerRevolution = 200;
Stepper myStepper(stepsPerRevolution, 15, 16, 17, 18);

long duration,duration2,duration3;
float distance,distance2,distance3;

Servo myservo;
 int pos = 0; 

void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);  
  pinMode(echoPin, INPUT);
  pinMode(trigPin2, OUTPUT);  
  pinMode(echoPin2, INPUT);
  pinMode(trigPin3, OUTPUT);  
  pinMode(echoPin3, INPUT); 
  pinMode(ledPin, OUTPUT); // Menjadikan pin LED sebagai output
  pinMode(ledPin2, OUTPUT);

  pinMode(buttonPin, INPUT_PULLUP); // Menjadikan pin push button sebagai input dengan pull-up resistor internal
  myservo.attach(12);
  myStepper.setSpeed(100);
  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Ketinggian : ");
  lcd.setCursor(0, 1);
  lcd.print("Jarak : ");
}
int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Menerima sinyal pantul dan menghitung jarak
  duration = pulseIn(echoPin, HIGH);
  int distance = duration * 0.034 / 2; // Menghitung jarak dalam centimeter

  return distance;
  }
  int getDistance2() {
  digitalWrite(trigPin2, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin2, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin2, LOW);

  // Menerima sinyal pantul dan menghitung jarak
  duration2 = pulseIn(echoPin2, HIGH);
  int distance2 = duration2 * 0.034 / 2; // Menghitung jarak dalam centimeter

  return distance2;
  }
  int getDistance3() {
  digitalWrite(trigPin3, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin3, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin3, LOW);

  // Menerima sinyal pantul dan menghitung jarak
  duration3 = pulseIn(echoPin3, HIGH);
  int distance3 = duration3 * 0.034 / 2; // Menghitung jarak dalam centimeter

  return distance3 ;
  }
void standby() {
  digitalWrite(ledPin, HIGH); // Mengubah pin LED menjadi HIGH (menyalakan LED)
  digitalWrite(ledmerah, LOW);
}
void sensorUltra1(){
  digitalWrite(ledPin2, HIGH);
  digitalWrite(ledPin, LOW);

  int potValue = analogRead(potPin); // Baca nilai potensiometer
  int speed = map(potValue, 0, 1023, 0, 100);
  myStepper.setSpeed(speed);

  myStepper.step(1);
  delay(10);
}
void sensorUltra2(){
  digitalWrite(ledPin2, LOW);
  digitalWrite(ledPin, HIGH);
}
void merah(){
    digitalWrite(ledmerah, HIGH);
}
void display(int distance, int distance2) {
  lcd.setCursor(13, 0);
  lcd.print("    ");
  lcd.setCursor(13, 0);
  lcd.print(distance);

  lcd.setCursor(8, 1);
  lcd.print("    ");
  lcd.setCursor(8, 1);
  lcd.print(distance2);
}
void Servo() {
    myservo.write(45);
    delay(5000);
    myservo.write(90);
}
void deteksi(){
digitalWrite(leddeteksi, HIGH);
delay(100);
digitalWrite(leddeteksi, LOW);
delay(100);
digitalWrite(leddeteksi, HIGH);
delay(100);
digitalWrite(leddeteksi, LOW);
delay(100);
}
void alloff(){
  digitalWrite(ledPin2, LOW);
  digitalWrite(ledPin, LOW);
  digitalWrite(ledmerah, LOW); 
}

void loop() {
  int buttonState = digitalRead(buttonPin);
  int buttonState2 = digitalRead(buttonstop);
  int distance = getDistance();
  int distance2 = getDistance2();
  int distance3 = getDistance3();
  display(distance, distance2);
  Serial.print("jarak line follower : ");
  Serial.println(distance3);
  delay(100);
  if(distance <10 && digitalRead(ledPin) == HIGH && distance3 < 5){
  sensorUltra1();
  }
  else if(buttonState == LOW){
    standby();
  }
  else if (buttonState == HIGH && digitalRead(ledPin)==LOW && digitalRead(ledPin2)== LOW){
    merah();
  }
  else if(digitalRead(ledPin2)==HIGH && distance2 < 10){
    deteksi();
    Servo();
  }
  else if(distance3 > 5 && digitalRead(ledmerah)==LOW){
   sensorUltra2();
  }
  else if(buttonState2 == HIGH){
    alloff();
  }
}