#include <NewPing.h>

// Pin definitions
#define TRIG_PIN_FRONT 2
#define ECHO_PIN_FRONT 3
#define LINE_SENSOR_LEFT 6
#define LINE_SENSOR_RIGHT 7
#define MOTOR_LEFT_ENABLE 8
#define MOTOR_LEFT_FORWARD 9
#define MOTOR_LEFT_BACKWARD 10
#define MOTOR_RIGHT_ENABLE 11
#define MOTOR_RIGHT_FORWARD 12
#define MOTOR_RIGHT_BACKWARD 13

// Constants
#define MAX_DISTANCE 200  // Maximum distance for ultrasonic sensor
#define ATTACK_DISTANCE 20 // Distance to initiate attack
#define ACCEL_STEP 2 // Step size for speed increment
#define ACCEL_DELAY 10 // Delay in milliseconds between speed increments

// Objects
NewPing sonarFront(TRIG_PIN_FRONT, ECHO_PIN_FRONT, MAX_DISTANCE);

// Function to move forward with variable speed
void forward(int maxSpeed, int duration) {
  for (int s = 0; s <= maxSpeed; s += ACCEL_STEP) {
    // for right motor
    analogWrite(MOTOR_RIGHT_ENABLE, s);
    digitalWrite(MOTOR_RIGHT_FORWARD, HIGH);
    digitalWrite(MOTOR_RIGHT_BACKWARD, LOW);
    // for left motor
    analogWrite(MOTOR_LEFT_ENABLE, s);
    digitalWrite(MOTOR_LEFT_FORWARD, HIGH);
    digitalWrite(MOTOR_LEFT_BACKWARD, LOW);
    delay(ACCEL_DELAY);
  }
  delay(duration);
  stop();
}

// Function to move backward with variable speed
void moveBackward(int maxSpeed, int duration) {
  for (int s = 0; s <= maxSpeed; s += ACCEL_STEP) {
    // for right motor
    analogWrite(MOTOR_RIGHT_ENABLE, s);
    digitalWrite(MOTOR_RIGHT_FORWARD, LOW);
    digitalWrite(MOTOR_RIGHT_BACKWARD, HIGH);
    // for left motor
    analogWrite(MOTOR_LEFT_ENABLE, s);
    digitalWrite(MOTOR_LEFT_FORWARD, LOW);
    digitalWrite(MOTOR_LEFT_BACKWARD, HIGH);
    delay(ACCEL_DELAY);
  }
  delay(duration);
  stop();
}

// Function to rotate left with variable speed
void rotateLeft(int maxSpeed, int duration) {
  for (int s = 0; s <= maxSpeed; s += ACCEL_STEP) {
    // for right motor
    analogWrite(MOTOR_RIGHT_ENABLE, s);
    digitalWrite(MOTOR_RIGHT_FORWARD, HIGH);
    digitalWrite(MOTOR_RIGHT_BACKWARD, LOW);
    // for left motor
    analogWrite(MOTOR_LEFT_ENABLE, s);
    digitalWrite(MOTOR_LEFT_FORWARD, LOW);
    digitalWrite(MOTOR_LEFT_BACKWARD, HIGH);
    delay(ACCEL_DELAY);
  }
  delay(duration);
  stop();
}

// Function to rotate right with variable speed
void rotateRight(int maxSpeed, int duration) {
  for (int s = 0; s <= maxSpeed; s += ACCEL_STEP) {
    // for right motor
    analogWrite(MOTOR_RIGHT_ENABLE, s);
    digitalWrite(MOTOR_RIGHT_FORWARD, LOW);
    digitalWrite(MOTOR_RIGHT_BACKWARD, HIGH);
    // for left motor
    analogWrite(MOTOR_LEFT_ENABLE, s);
    digitalWrite(MOTOR_LEFT_FORWARD, HIGH);
    digitalWrite(MOTOR_LEFT_BACKWARD, LOW);
    delay(ACCEL_DELAY);
  }
  delay(duration);
  stop();
}

// Function to stop
void stop() {
  analogWrite(MOTOR_LEFT_ENABLE, 0);
  digitalWrite(MOTOR_LEFT_FORWARD, LOW);
  digitalWrite(MOTOR_LEFT_BACKWARD, LOW);
  analogWrite(MOTOR_RIGHT_ENABLE, 0);
  digitalWrite(MOTOR_RIGHT_FORWARD, LOW);
  digitalWrite(MOTOR_RIGHT_BACKWARD, LOW);
}

// Function to read line sensors
bool isOnWhiteLine() {
  return digitalRead(LINE_SENSOR_LEFT) == HIGH || digitalRead(LINE_SENSOR_RIGHT) == HIGH;
}

// Function to scan for opponent
bool scanForOpponent() {
  int distanceFront = sonarFront.ping_cm();
  return distanceFront > 0 && distanceFront < MAX_DISTANCE;
}

void setup() {
  // Initialize pins
  pinMode(MOTOR_LEFT_ENABLE, OUTPUT);
  pinMode(MOTOR_LEFT_FORWARD, OUTPUT);
  pinMode(MOTOR_LEFT_BACKWARD, OUTPUT);
  pinMode(MOTOR_RIGHT_ENABLE, OUTPUT);
  pinMode(MOTOR_RIGHT_FORWARD, OUTPUT);
  pinMode(MOTOR_RIGHT_BACKWARD, OUTPUT);

  pinMode(LINE_SENSOR_LEFT, INPUT);
  pinMode(LINE_SENSOR_RIGHT, INPUT);

  Serial.begin(9600); // For debugging purposes

  // Add a 5-second delay before the robot starts
  Serial.println("Starting in 5 seconds...");
  delay(5000);
  Serial.println("Go!");
}

void loop() {
  // Check if the robot is on the white line
  if (isOnWhiteLine()) {
    moveBackward(255, 500); // Move backward with max speed for 0.5 seconds to avoid going out of the ring
    rotateLeft(255, 500); // Rotate left with max speed for a short period to change direction
    return; // Exit loop to avoid further actions
  }

  // Scan for opponent with ultrasonic sensor
  if (scanForOpponent()) {
    stop(); // Stop the robot before attacking
    Serial.println("Opponent detected, attacking...");
    forward(255, 1000); // Move forward with max speed for 1 second to simulate attack
    return; // Exit loop to continue attacking
  }

  // If no opponent is detected, rotate to search
  rotateLeft(255, 500); // Rotate left with max speed for a short period to scan the surroundings
  Serial.println("Searching for opponent...");
}