#define LDR1_PIN 34 // Turn Left 1
#define LDR2_PIN 35 // Turn Left 2
#define LDR3_PIN 32 // Straight
#define LDR4_PIN 33 // Turn Right 1
#define LDR5_PIN 25 // Turn Right 2

#define LED1_PIN 23 // PWM
#define LED2_PIN 22 // PWM

#define IN1 19
#define IN2 18 
#define IN3 5
#define IN4 17



void setup() {
  Serial.begin(115200);
  
  pinMode(LED1_PIN, OUTPUT);
  pinMode(LED2_PIN, OUTPUT);

  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);

  pinMode(LDR1_PIN, INPUT);
  pinMode(LDR2_PIN, INPUT);
  pinMode(LDR3_PIN, INPUT);
  pinMode(LDR4_PIN, INPUT);
  pinMode(LDR5_PIN, INPUT);

}

void loop() {
  int ldr1Value = digitalRead(LDR1_PIN);
  int ldr2Value = digitalRead(LDR2_PIN);
  int ldr3Value = digitalRead(LDR3_PIN);
  int ldr4Value = digitalRead(LDR4_PIN);
  int ldr5Value = digitalRead(LDR5_PIN);

  Serial.print("LDR1: "); Serial.print(ldr1Value); Serial.print("\t");
  Serial.print("LDR2: "); Serial.print(ldr2Value); Serial.print("\t");
  Serial.print("LDR3: "); Serial.print(ldr3Value); Serial.print("\t");
  Serial.print("LDR4: "); Serial.print(ldr4Value); Serial.print("\t");
  Serial.print("LDR5: "); Serial.println(ldr5Value);
  
  if (ldr1Value == HIGH && ldr2Value == LOW && ldr3Value == LOW && ldr4Value == LOW && ldr5Value == LOW) {
    digitalWrite(LED1_PIN, HIGH);
    digitalWrite(LED2_PIN, LOW);
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    Serial.println(" Kiri Tajam ");
    Serial.println(" Kecepatan = 120 ");
  }
  else if (ldr2Value == HIGH && ldr3Value == HIGH && ldr4Value == LOW && ldr5Value == LOW) {
    digitalWrite(LED1_PIN, HIGH);
    digitalWrite(LED2_PIN, LOW);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    Serial.println(" Kiri Sedikit ");
    Serial.println(" Kecepatan = 150 ");
  }
  else if (ldr5Value == HIGH) {
    digitalWrite(LED1_PIN, LOW);
    digitalWrite(LED2_PIN, HIGH);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    Serial.println(" Kanan Tajam ");
    Serial.println(" Kecepatan = 120 ");
  }
  else if (ldr3Value == HIGH && ldr4Value == HIGH) {
    digitalWrite(LED1_PIN, LOW);
    digitalWrite(LED2_PIN, HIGH);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    Serial.println(" Kanan Sedikit ");
    Serial.println(" Kecepatan = 150 ");
  }
  else if (ldr2Value == HIGH && ldr3Value == HIGH && ldr4Value == HIGH) {
    digitalWrite(LED1_PIN, HIGH);
    digitalWrite(LED2_PIN, HIGH);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    Serial.println(" Lurus ");
    Serial.println(" Kecepatan = 180 ");
  }
  else if (ldr3Value == HIGH) {
    digitalWrite(LED1_PIN, HIGH);
    digitalWrite(LED2_PIN, HIGH);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    Serial.println(" Lurus ");
    Serial.println(" Kecepatan = 180 ");
  }
  else if (ldr1Value == HIGH && ldr2Value == HIGH && ldr3Value == HIGH && ldr4Value == HIGH && ldr5Value == HIGH) {
    digitalWrite(LED1_PIN, LOW);
    digitalWrite(LED2_PIN, LOW);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    Serial.println(" Berhenti ");
    Serial.println(" Kecepatan = 0 ");
  }
  else {
    digitalWrite(LED1_PIN, LOW);
    digitalWrite(LED2_PIN, LOW);
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, LOW);
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, LOW);
    Serial.println(" Berhenti ");
    Serial.println(" Kecepatan = 0 ");
  }

  delay(500);
}