#include <QTRSensors.h>

#define LPWM 3
#define RPWM 11
#define LDIR 12
#define RDIR 13

#define SPEED_SWITCH 2

const uint8_t S1 = A0;
const uint8_t S2 = A1;
const uint8_t S3 = A2;
const uint8_t S4 = A3;
const uint8_t S5 = A4;
const uint8_t S6 = A5;
const uint8_t S7 = 4;
const uint8_t S8 = 5;

QTRSensors qtr;

const uint8_t SensorCount = 8;
uint16_t sensorValues[SensorCount];

float Kp = 0.18;
float Ki = 0.0;
float Kd = 1.8;

int baseSpeed = 250;
int maxSpeed = 200;

int lastError = 0;
long integral = 0;

void setup() {

  Serial.begin(9600);

  qtr.setTypeRC();
  qtr.setSensorPins((const uint8_t[]){
    S1, S2, S3, S4,
    S5, S6, S7, S8
  }, SensorCount);

  pinMode(LPWM, OUTPUT);
  pinMode(RPWM, OUTPUT);
  pinMode(LDIR, OUTPUT);
  pinMode(RDIR, OUTPUT);

  pinMode(SPEED_SWITCH, INPUT_PULLUP);

  Serial.println("Calibrating...");

  for (int i = 0; i < 300; i++) {
    qtr.calibrate();
    delay(5);
  }

  Serial.println("READY");
}

void loop() {

  //==========================
  // SPEED MODE
  //==========================

  if (digitalRead(SPEED_SWITCH) == LOW) {

  Kp = 0.30;
  Ki = 0.0;
  Kd = 3.2;

} else {

  Kp = 0.30;
  Ki = 0.0;
  Kd = 2.0;

}

  uint16_t position = qtr.readLineBlack(sensorValues);

  int error = position - 3500;

  // Deadband
  if (abs(error) < 25)
    error = 0;

  //==========================
  //==========================
// ADAPTIVE SPEED
//==========================

if (digitalRead(SPEED_SWITCH) == LOW) {

  // FAST MODE (Max = 200)
if (abs(error) < 150)
    baseSpeed = 250;
else if (abs(error) < 400)
    baseSpeed = 180;
else if (abs(error) < 800)
    baseSpeed = 140;
else
    baseSpeed = 90;

} else {

   // NORMAL MODE (Max = 100)

  if (abs(error) < 100)
    baseSpeed = 100;

  else if (abs(error) < 300)
    baseSpeed = 90;

  else if (abs(error) < 700)
    baseSpeed = 80;

  else if (abs(error) < 1200)
    baseSpeed = 70;

  else
    baseSpeed = 60;

}

  integral += error;
  integral = constrain(integral, -3000, 3000);

  int derivative = error - lastError;

  int correction =
      (Kp * error) +
      (Ki * integral) +
      (Kd * derivative);

  correction = constrain(correction, -140, 140);

  lastError = error;

  int leftSpeed = baseSpeed + correction;
  int rightSpeed = baseSpeed - correction;

  // Extra slowdown habang lumiliko
if (abs(error) > 800) {
  leftSpeed  -= 40;
  rightSpeed -= 40;
}

  leftSpeed = constrain(leftSpeed, 0, 250);
  rightSpeed = constrain(rightSpeed, 0, 250);

  digitalWrite(LDIR, HIGH);
  digitalWrite(RDIR, HIGH);

  analogWrite(LPWM, leftSpeed);
  analogWrite(RPWM, rightSpeed);

 
}