// Maze solving robot with tuned parameters

const int motorAForward = 7;   // Right Motor
const int motorABackward = 8;
const int enA = 9;
const int motorBForward = 3;    // Left Motor
const int motorBBackward = 4;
const int enB = 5;

// Ultrasonic pins
const int trigPinFront = 10;
const int echoPinFront = 11;
const int trigPinLeft = 12;
const int echoPinLeft = 13;
const int trigPinRight = A0;
const int echoPinRight = A1;

// Adjusted Distance thresholds (cm)
const int WALL_DISTANCE = 20;  // Increased from 10
const int WALL_TOO_CLOSE = 8;  // Slightly increased from 5
const int CORRECTION_DISTANCE = 15; // Increased from 12

// Adjusted Motor speeds (0-255)
const int BASE_SPEED = 250;        
const int SLOW_SPEED = 200;        
const int TURN_SPEED = 200;        

// Timing constants
const int TURN_DELAY = 600;  
const int CORRECTION_DELAY = 150;
const int BACKUP_DELAY = 250;

void setup() {
  // Initialize motor and sensor pins
  pinMode(motorAForward, OUTPUT);
  pinMode(motorABackward, OUTPUT);
  pinMode(enA, OUTPUT);
  pinMode(motorBForward, OUTPUT);
  pinMode(motorBBackward, OUTPUT);
  pinMode(enB, OUTPUT);

  pinMode(trigPinFront, OUTPUT);
  pinMode(echoPinFront, INPUT);
  pinMode(trigPinLeft, OUTPUT);
  pinMode(echoPinLeft, INPUT);
  pinMode(trigPinRight, OUTPUT);
  pinMode(echoPinRight, INPUT);

  // Set higher PWM frequency for smoother operation
  TCCR0B = (TCCR0B & 0b11111000) | 0x01;
  TCCR1B = (TCCR1B & 0b11111000) | 0x01;

  delay(1000);
}

void loop() {
  int frontDist = getDistance(trigPinFront, echoPinFront);
  int leftDist = getDistance(trigPinLeft, echoPinLeft);
  int rightDist = getDistance(trigPinRight, echoPinRight);

  // Emergency behavior
  if (frontDist <= WALL_TOO_CLOSE || leftDist <= WALL_TOO_CLOSE || rightDist <= WALL_TOO_CLOSE) {
    emergencyStop();
    adjustPosition(leftDist, rightDist);
  }

  // Navigation logic
  if (frontDist <= WALL_DISTANCE) {
    if (leftDist > WALL_DISTANCE) {
      turnLeft();
      delay(TURN_DELAY);
    } else if (rightDist > WALL_DISTANCE) {
      turnRight();
      delay(TURN_DELAY);
    } else {
      turnAround();
      delay(TURN_DELAY * 2);
    }
  } else {
    if (leftDist > WALL_DISTANCE) {
      turnLeft();
      delay(TURN_DELAY);
    } else {
      if (leftDist <= CORRECTION_DISTANCE) {
        slightRight();
        delay(CORRECTION_DELAY);
      } 
      else if (rightDist <= CORRECTION_DISTANCE) {
        slightLeft();
        delay(CORRECTION_DELAY);
      } else {
        moveForward();
      }
    }
  }
}

void adjustPosition(int leftDist, int rightDist) {
  // Back up if too close to front wall
  if (getDistance(trigPinFront, echoPinFront) <= WALL_TOO_CLOSE) {
    moveBackward();
    delay(BACKUP_DELAY);
    stopMotors();
  }
  
  // Adjust position based on which side is closer
  if (leftDist < rightDist) {
    slightRight();
    delay(CORRECTION_DELAY);
  } else {
    slightLeft();
    delay(CORRECTION_DELAY);
  }
  stopMotors();
}

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

// Movement
void moveForward() {
  digitalWrite(motorAForward, HIGH);
  digitalWrite(motorABackward, LOW);
  analogWrite(enA, BASE_SPEED);
  
  digitalWrite(motorBForward, HIGH);
  digitalWrite(motorBBackward, LOW);
  analogWrite(enB, BASE_SPEED);
}

void moveBackward() {
  digitalWrite(motorAForward, LOW);
  digitalWrite(motorABackward, HIGH);
  analogWrite(enA, SLOW_SPEED);
  
  digitalWrite(motorBForward, LOW);
  digitalWrite(motorBBackward, HIGH);
  analogWrite(enB, SLOW_SPEED);
}

void turnLeft() {
  // Right motor forward
  digitalWrite(motorAForward, HIGH);
  digitalWrite(motorABackward, LOW);
  analogWrite(enA, TURN_SPEED);
  
  // Left motor backward for sharper turn
  digitalWrite(motorBForward, LOW);
  digitalWrite(motorBBackward, HIGH);
  analogWrite(enB, TURN_SPEED/2);
}

void turnRight() {
  // Right motor backward
  digitalWrite(motorAForward, LOW);
  digitalWrite(motorABackward, HIGH);
  analogWrite(enA, TURN_SPEED/2);
  
  // Left motor forward
  digitalWrite(motorBForward, HIGH);
  digitalWrite(motorBBackward, LOW);
  analogWrite(enB, TURN_SPEED);
}

void slightLeft() {
  digitalWrite(motorAForward, HIGH);
  digitalWrite(motorABackward, LOW);
  analogWrite(enA, BASE_SPEED);
  
  digitalWrite(motorBForward, HIGH);
  digitalWrite(motorBBackward, LOW);
  analogWrite(enB, SLOW_SPEED);
}

void slightRight() {
  digitalWrite(motorAForward, HIGH);
  digitalWrite(motorABackward, LOW);
  analogWrite(enA, SLOW_SPEED);
  
  digitalWrite(motorBForward, HIGH);
  digitalWrite(motorBBackward, LOW);
  analogWrite(enB, BASE_SPEED);
}

void turnAround() {
  // Right motor backward
  digitalWrite(motorAForward, LOW);
  digitalWrite(motorABackward, HIGH);
  analogWrite(enA, TURN_SPEED);
  
  // Left motor forward
  digitalWrite(motorBForward, HIGH);
  digitalWrite(motorBBackward, LOW);
  analogWrite(enB, TURN_SPEED);
}

void emergencyStop() {
  digitalWrite(motorAForward, LOW);
  digitalWrite(motorABackward, LOW);
  analogWrite(enA, 0);
  
  digitalWrite(motorBForward, LOW);
  digitalWrite(motorBBackward, LOW);
  analogWrite(enB, 0);
  delay(100);
}

void stopMotors() {
  emergencyStop(); // same as stop
}