// Motor pin definitions
int _output_PD5 = 5;  // rear left motor control
int _output_PD4 = 4;  // front left motor control
int _output_PD7 = 6;  // rear right motor control
int _output_PD6 = 7;  // front right motor control

int _output_PWM10 = 10; // right motor speed
int _output_PWM9 = 9;   // left motor speed

// Sensor pin definitions
int sensors[5] = {A0, A1, A2, A3, A4}; // Analog sensors
int values[5];  // for reading sensor values

// PID variables
float Kp = 10;
float Ki = 0;
float Kd = 5;

int baseSpeed = 20; // Base speed

long weightedSum = 0;     // Weighted sum of sensor values
long position = 0;        // Position (on X-axis) of the line
int error = 0;            // PID error

void setup() {
  // Set pinMode for motor control
  pinMode(_output_PD5, OUTPUT);
  pinMode(_output_PD4, OUTPUT);
  pinMode(_output_PD7, OUTPUT);
  pinMode(_output_PD6, OUTPUT);

  pinMode(_output_PWM10, OUTPUT); // Right motor PWM pin
  pinMode(_output_PWM9, OUTPUT);  // Left motor PWM pin

  // Set pinMode for analog sensors
  for (int i = 0; i < 5; i++) {
    pinMode(sensors[i], INPUT);
  }

  // Start serial communication
  Serial.begin(9600);
}

void loop() {
  // Read values from sensors
  for (int i = 0; i < 5; i++) {
    values[i] = 1023 - analogRead(sensors[i]); // Invert sensor readings
  }

  weightedSum = 0;
  position = 0;

  // Calculate weighted sum for line position
  for (int i = 0; i < 5; i++) {
    weightedSum += values[i];
    position += (long)values[i] * (i * 100); // Weighting the sensors
  }

  error = position / weightedSum - 200; // Calculate line position error

  // Calculate PID correction (simple example)
  int leftSpeed = baseSpeed - Kp * error - Ki * weightedSum - Kd * (error);
  int rightSpeed = baseSpeed + Kp * error + Ki * weightedSum + Kd * (error);

  // Apply correction to motors
  controlMotors(leftSpeed, rightSpeed);

  // Debug: Show values on Serial Monitor
  Serial.print("Error: ");
  Serial.print(error);
  Serial.print(" | Left Speed: ");
  Serial.print(leftSpeed);
  Serial.print(" | Right Speed: ");
  Serial.println(rightSpeed);

  delay(100); // Small delay for control stability
}

void controlMotors(int leftSpeed, int rightSpeed) {
  // Add base speed to calculated speed
  leftSpeed += baseSpeed;
  rightSpeed += baseSpeed;

  // Ensure speed values are in 0–255 range
  leftSpeed = constrain(leftSpeed, 0, 255);
  rightSpeed = constrain(rightSpeed, 0, 255);

  // Control motors with calculated speed
  digitalWrite(_output_PD4, 1);
  digitalWrite(_output_PD5, 0);
  digitalWrite(_output_PD6, 1);
  digitalWrite(_output_PD7, 0);

  analogWrite(_output_PWM9, rightSpeed);  // Left motor uses rightSpeed
  analogWrite(_output_PWM10, leftSpeed);  // Right motor uses leftSpeed
}