#include <Servo.h>

// กำหนดขา Ultrasonic 
const int trigPin = 5;
const int echoPin = 6;

// กำหนดขา L298N
const int ENA = 9;  // (ต้องเป็นขา PWM)
const int ENB = 10; // (ต้องเป็นขา PWM)

const int IN1 = 13; // 
const int IN2 = 7;  // 
const int IN3 = 11; // 
const int IN4 = 8;  // 

// กำหนดขา Servo
const int servoPin = 3;
Servo myServo;

long duration;
int distance;

int speedLeft = 170;   // ปรับความเร็วล้อซ้าย
int speedRight = 150;   // ปรับความเร็วล้อขวา

void setup() {
  Serial.begin(9600);
  
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  
  // ตั้งค่าขา L298N เป็น OUTPUT ทั้งหมด
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);
  
  myServo.attach(servoPin);
  myServo.write(90);
  delay(2000);
}

void loop() {
  int distanceFront = getDistance();
  
  if (distanceFront > 50) {
    moveForward();
  } else {
    stopMotors(); 
    delay(300);
    moveBackward();
    delay(800);
    stopMotors();
    delay(300);
    
    int distanceRight = lookRight();
    delay(600);
    int distanceLeft = lookLeft();
    delay(600);
    
    if (distanceRight >= distanceLeft) {
      turnRight();
      delay(600); 
    } else {
      turnLeft();
      delay(600); 
    }
    stopMotors();
    delay(200);
  }
}

int getDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2; 
  return distance;
}

int lookRight() {
  myServo.write(10); 
  delay(500);
  int dist = getDistance();
  myServo.write(90); 
  return dist;
}

int lookLeft() {
  myServo.write(170); 
  delay(500);
  int dist = getDistance();
  myServo.write(90); 
  return dist;
}

// --- ฟังก์ชันควบคุมทิศทางและความเร็วมอเตอร์ ---

void moveForward() {
  analogWrite(ENA, speedLeft);
  analogWrite(ENB, speedRight);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void moveBackward() {
  analogWrite(ENA, speedLeft);
  analogWrite(ENB, speedRight);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void turnRight() {
  analogWrite(ENA, speedLeft);
  analogWrite(ENB, speedRight);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, HIGH);
  digitalWrite(IN3, HIGH);
  digitalWrite(IN4, LOW);
}

void turnLeft() {
  analogWrite(ENA, speedLeft);
  analogWrite(ENB, speedRight);
  digitalWrite(IN1, HIGH);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, HIGH);
}

void stopMotors() {
  analogWrite(ENA, 0);
  analogWrite(ENB, 0);
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}