const int US_TRIG[] = {2, A0, A2};
const int US_ECHO[] = {4, A1, A3};
const int IR_PINS[] = {12, 13};

const int MOTOR_RIGHT[] = {3, 5, 6};
const int MOTOR_LEFT[] = {9, 10, 11};

const int LINE_DETECT = LOW;
const int DISTANCE_THRESHOLD = 30;
const int MAX_DISTANCE = 60;
const int DEFAULT_DISTANCE = 60;

const int MOTOR_SPEED = 255;
const int BACK_DELAY = 200;
const int TURN_DELAY = 200;

enum State {SEARCH, ATTACK, AVOID_LINE};
State currentState = SEARCH;
bool searchDirection = false;


long readUltrasonic(int trigPin, int echoPin) {
  long totalDistance = 0;
  int numReadings = 5;
  for (int i = 0; i < numReadings; i++) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    long duration = pulseIn(echoPin, HIGH, 20000) * 0.034 / 2;
    (duration <= MAX_DISTANCE) ? totalDistance += duration : totalDistance += 70;
  }
  return totalDistance / numReadings;
}

bool isLineDetected() {
  for (int i = 0; i < 2; i++) {
    if (digitalRead(IR_PINS[i]) == LINE_DETECT) return true;
  }
  return false;
}

void detectMusuh(int * distances) {
  for (int i = 0; i < 3; i++) {
    distances[i] = readUltrasonic(US_TRIG[i], US_ECHO[i]);
  }
}

void loopSumoBot() {
  int distances[3] = {0, 0, 0};

  switch (currentState) {
    case AVOID_LINE:
      Serial.println("AVOID LINE MODE");
      moveBackward();
      delay(BACK_DELAY);
      if (digitalRead(IR_PINS[0]) == LINE_DETECT) {
        turnRight();
      } else {
        turnLeft();
      }
      delay(TURN_DELAY);
      currentState = SEARCH;
      break;

    case ATTACK:
      Serial.println("ATTACK MODE");
      detectMusuh(distances);
      if (distances[0] < DISTANCE_THRESHOLD) {
        moveForward();
      } else if (distances[1] < DISTANCE_THRESHOLD) {
        turnLeft();
      } else if (distances[2] < DISTANCE_THRESHOLD) {
        turnRight();
      } else {
        currentState = SEARCH;
      }
      break;

    case SEARCH:
    default:
      Serial.println("SEARCH MODE");
      if (isLineDetected()) {
        currentState = AVOID_LINE;
      } else {
        detectMusuh(distances);
        if (distances[0] < DISTANCE_THRESHOLD || distances[1] < DISTANCE_THRESHOLD || distances[2] < DISTANCE_THRESHOLD) {
          currentState = ATTACK;
        } else {
          if (searchDirection == false) {
            turnLeft();
          } else {
            turnRight();
          }
        }
        if (distances[1] < DISTANCE_THRESHOLD) {
          searchDirection = true; // Cenderung ke kiri
        } else if (distances[2] < DISTANCE_THRESHOLD) {
          searchDirection = false; // Cenderung ke kanan
        }
      }
      break;

  }
}

void setup() {
  for (int i = 0; i < 2; i++) pinMode(IR_PINS[i], INPUT);
  for (int i = 0; i < 3; i++) {
    pinMode(US_TRIG[i], OUTPUT);
    pinMode(US_ECHO[i], INPUT);
    pinMode(MOTOR_LEFT[i], OUTPUT);
    pinMode(MOTOR_RIGHT[i], OUTPUT);
  }
  Serial.begin(9600);

}

void loop() {
  loopSumoBot();
  delay(30);
}




void moveForward() {
  digitalWrite(MOTOR_RIGHT[0], HIGH); digitalWrite(MOTOR_RIGHT[1], LOW); analogWrite(MOTOR_RIGHT[2], MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT[0], HIGH); digitalWrite(MOTOR_LEFT[1], LOW); analogWrite(MOTOR_LEFT[2], MOTOR_SPEED);
}
void moveBackward() {
  digitalWrite(MOTOR_RIGHT[0], LOW); digitalWrite(MOTOR_RIGHT[1], HIGH); analogWrite(MOTOR_RIGHT[2], MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT[0], LOW); digitalWrite(MOTOR_LEFT[1], HIGH); analogWrite(MOTOR_LEFT[2], MOTOR_SPEED);
}
void turnRight() {
  digitalWrite(MOTOR_RIGHT[0], HIGH); digitalWrite(MOTOR_RIGHT[1], LOW); analogWrite(MOTOR_RIGHT[2], MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT[0], LOW); digitalWrite(MOTOR_LEFT[1], HIGH); analogWrite(MOTOR_LEFT[2], MOTOR_SPEED);
}
void turnLeft() {
  digitalWrite(MOTOR_RIGHT[0], LOW); digitalWrite(MOTOR_RIGHT[1], HIGH); analogWrite(MOTOR_RIGHT[2], MOTOR_SPEED);
  digitalWrite(MOTOR_LEFT[0], HIGH); digitalWrite(MOTOR_LEFT[1], LOW); analogWrite(MOTOR_LEFT[2], MOTOR_SPEED);
}