int ENA =10;    // Enable pin for motor A (PWM pin)
int ENB =5;   // Enable pin for motor B (PWM pin)
int IN1 =9;    // Input 1 for motor A (controls motor A direction)
int IN2 =8;    // Input 2 for motor A (controls motor A direction)
int IN3 =7;    // Input 3 for motor B (controls motor B direction)
int IN4 =6;    // Input 4 for motor B (controls motor B direction)

int sensorPins[] = {A0, A1, A2, A3, A4};  // Digital pins for the 5 IR sensors
int sensorValues[5];
float weights[] = {-6.5, -4.5, 0, 4.5, 6.5};   // Weights for each sensor: used to calculate error based on position
int baseSpeed = 150;                  // Base speed for the motors
int Kp = 50;                          // Proportional gain constant
int Ki = 0.028;                           // Integral gain constant (not used here, set to 0)
int Kd = 30;                          // Derivative gain constant
int lastError = 0;                    // Stores the last error value (for the derivative term)
int totalError = 0;                   // Cumulative error (for the integral term)

void setup() {
  pinMode(ENA, OUTPUT);  // Set ENA (Enable for Motor A) as output (controls speed of motor A)
  pinMode(ENB, OUTPUT);  // Set ENB (Enable for Motor B) as output (controls speed of motor B)
  pinMode(IN1, OUTPUT);  // Set IN1 as output (for motor A direction)
  pinMode(IN2, OUTPUT);  // Set IN2 as output (for motor A direction)
  pinMode(IN3, OUTPUT);  // Set IN3 as output (for motor B direction)
  pinMode(IN4, OUTPUT);  // Set IN4 as output (for motor B direction)
  
//5 sensor pins are set as input
  pinMode(sensorPins[0], INPUT);  // Set each sensor pin as input
  pinMode(sensorPins[1], INPUT);  // Set each sensor pin as input
  pinMode(sensorPins[2], INPUT);  
  pinMode(sensorPins[3], INPUT);  
  pinMode(sensorPins[4], INPUT);  
  pinMode(sensorPins[5], INPUT);  
Serial.begin(9600);
}

void loop() {
  int position = getPosition();  // Get the position of the line based on sensor readings
  int error = position;          // The error is simply the position value (deviation from center)
  totalError += error;           // Update the cumulative error for the integral term (not used here)
  int dError = error - lastError; // Calculate the derivative term (change in error)

  // Calculate the PID value: Proportional + Integral + Derivative terms
  int SteeringValue = (Kp * error) + (Ki * totalError) + (Kd * dError);
  lastError = error;  // Store the current error for the next loop iteration

  // Control the motor speeds based on the PID output
  int leftSpeed = baseSpeed + SteeringValue;  // Left motor speed decreases when error is positive (turn right)
  int rightSpeed = baseSpeed - SteeringValue; // Right motor speed increases when error is positive (turn right)

  // Constrain motor speeds to the valid range (-250 to 250 for PWM output)
  leftSpeed = constrain(leftSpeed, -250, 250);  // Constrain left motor speed
  rightSpeed = constrain(rightSpeed, -250, 250);  // Constrain right motor speed
  // Call function to set motor speeds
setMotorSpeed(leftSpeed, rightSpeed);
/////////////////

Serial.print (SteeringValue);
Serial.print ("   :   ");
Serial.print (Kp * error);
Serial.print ("===");
Serial.print (Ki * totalError);
Serial.print ("===");
Serial.print (Kd * dError);
Serial.print ("   :   ");
Serial.print (leftSpeed);
Serial.print ("===");
Serial.println (rightSpeed);

}

int getPosition() {
  int weightedSum = 0;      // Sum of weighted sensor readings
  int totalActive = 0;      // Number of active sensors detecting the line

  // Loop through all 5 sensors
   for (int i = 0; i < 5; i++) {
    sensorValues[i] = analogRead(sensorPins[i]);
    if (sensorValues[i] > 500) {   // Threshold value for line detection
      weightedSum += weights[i];
      totalActive++;
    }
  }

  // If no sensors are detecting the line, return the last known error (to avoid losing track)
  if (totalActive == 0) {
    return lastError;
  }

  // Return the weighted average position (normalized by the number of active sensors)
  return weightedSum / totalActive;
}

void setMotorSpeed(int leftSpeed, int rightSpeed) {
  // Constrain motor speeds to the valid range (-250 to -250 for PWM output)
  leftSpeed = constrain(leftSpeed, -250, 250);  // Constrain left motor speed
  rightSpeed = constrain(rightSpeed, -250, 250);  // Constrain right motor speed

  // Set the PWM signal (speed) for each motor
  analogWrite(ENA, leftSpeed);   // Set the speed for motor A (left motor)
  analogWrite(ENB, rightSpeed);  // Set the speed for motor B (right motor)
  
  int position = getPosition();  // Get the position of the line based on sensor readings
  int error = position;          // The error is simply the position value (deviation from center)

 // Set motor directions and speeds for the left motor
  if (leftSpeed >= 0) {
    // Forward direction
    digitalWrite(IN1, HIGH);
    digitalWrite(IN2, LOW);
    analogWrite(ENA, leftSpeed);
  } else {
    // Reverse direction
    digitalWrite(IN1, LOW);
    digitalWrite(IN2, HIGH);
    analogWrite(ENA, -leftSpeed);  // Send positive value to PWM
  }

  // Set motor directions and speeds for the right motor
  if (rightSpeed >= 0) {
    // Forward direction
    digitalWrite(IN3, HIGH);
    digitalWrite(IN4, LOW);
    analogWrite(ENB, rightSpeed);
  } else {
    // Reverse direction
    digitalWrite(IN3, LOW);
    digitalWrite(IN4, HIGH);
    analogWrite(ENB, -rightSpeed);  // Send positive value to PWM
  }
}