#define Rpwm1 6
#define RDir1 7
#define RDir2 8
#define Lpwm1 11
#define LDir1 10
#define LDir2 9
#define ObjectSensFront 4
#define ObjectSensBack 2
#define LQ1 A7
#define LQ2 A6
#define LQ3 A5
#define LQ4 A4
#define LQ5 A3
#define LQ6 A2
#define LQ7 A1
#define LQ8 A0

#define SW1 12
#define SW2 13

#define Led 13

float veri = 0;
float LeftBaseSpeed  = 220 ;
float RightBaseSpeed = 220 ;
float Kp = 0.07;
float Kd = 2;
float Ki = 0.0001 ;

int Q[8] = {LQ8, LQ7, LQ6, LQ5, LQ4, LQ3, LQ2, LQ1};
int SensMin[8];
int SensMax[8];
int SensVal[8];
int SensValX[8];
int FinalError = 0;
int Error = 0;
int ExtraSpeed = 0;
int RightSpeed = 0;
int LeftSpeed = 0;
int Integral = 0;
int Val = 0;
unsigned int Position = 3500;

byte LineOk = 0;
byte WhiteLine = 1;

void Blink() {
  for (int i = 0; i < 10; i++) {
    digitalWrite(Led, LOW);
    delay(180);
    digitalWrite(Led, HIGH);
    delay(180);
  }
}

void MotorsSpeed(int RightSpeed, int LeftSpeed) {
  if (RightSpeed <= 0) {
    RightSpeed = abs(RightSpeed);
    analogWrite(Rpwm1, RightSpeed);
    digitalWrite(RDir1, HIGH);
    digitalWrite(RDir2, LOW);
  }
  else {
    analogWrite(Rpwm1, RightSpeed);
    digitalWrite(RDir1, LOW);
    digitalWrite(RDir2, HIGH);
  }
  if (LeftSpeed <= 0) {
    LeftSpeed = abs(LeftSpeed);
    analogWrite(Lpwm1, LeftSpeed);
    digitalWrite(LDir1, HIGH);
    digitalWrite(LDir2, LOW);
  }
  else {
    analogWrite(Lpwm1, LeftSpeed);
    digitalWrite(LDir1, LOW);
    digitalWrite(LDir2, HIGH);
  }
}

int ReadSensor() {
  unsigned long Middle = 0;
  unsigned int Total = 0;
  LineOk = 0;
  for (int i = 0; i < 8; i++) {
    int Difference = SensMax[i] - SensMin[i];
    long int Calculation = analogRead(Q[i]) - SensMin[i];
    Calculation = Calculation * 1000;
    Calculation = Calculation / Difference;
    SensVal[i] = constrain(Calculation, 0, 1000);
    SensValX[i] = SensVal[i];
  }
  for (int i = 0; i < 8; i++) {
    if (WhiteLine == 1)  SensValX[i] = 1000 - SensVal[i];
    if (SensValX[i] > 250)  LineOk = 1;
    if (SensValX[i] > 50) {
      Middle += SensValX[i] * i ;
      Total += SensValX[i];
    }
  }
  if (LineOk == 0) {
    if (Val < 3000)  return 0;
    if (Val > 5000)  return 7000;
    if (Val > 3000 && Val < 5000) return 3500;
  }
  Middle = Middle * 1000;
  Val = Middle / Total;
  return Val;
}

void setup() {
  pinMode(Rpwm1, OUTPUT);
  pinMode(Lpwm1, OUTPUT);
  pinMode(RDir1, OUTPUT);
  pinMode(RDir2, OUTPUT);
  pinMode(LDir1, OUTPUT);
  pinMode(LDir2, OUTPUT);
  pinMode(LQ1, INPUT);
  pinMode(LQ2, INPUT);
  pinMode(LQ3, INPUT);
  pinMode(LQ4, INPUT);
  pinMode(LQ5, INPUT);
  pinMode(LQ6, INPUT);
  pinMode(LQ7, INPUT);
  pinMode(LQ8, INPUT);

  pinMode(ObjectSensFront, INPUT);
  pinMode(ObjectSensBack, INPUT);
  pinMode(Led, OUTPUT);

  pinMode(SW1, INPUT);
  pinMode(SW2, INPUT);
  Serial.begin(9600);
  MotorsSpeed(0, 0);
  delay(2000);
  for (int i = 0; i < 8; i++) {
    SensMin[i] = 1024;
    SensMax[i] = 0;
  }
  for (int i = 0; i < 150; i++)
  {
    for (int i = 0; i < 8; i++) {
      if (SensMin[i] > analogRead(Q[i])) SensMin[i] = analogRead(Q[i]);
      if (SensMax[i] < analogRead(Q[i])) SensMax[i] = analogRead(Q[i]);
      delay(1);
    }
  }
  delay(700);
  Blink();
}

void solla(){
    // Sol şeride çıkış
    MotorsSpeed(-30, 60); 
    delay(300);  // Sol dönüş için yeterli süre

    // Düz gitme ve sürekli sensör okuma
    unsigned long startTime = millis();
    while (millis() - startTime < 610) {  // 1 saniye boyunca
        Position = ReadSensor();
        Error = Position - 3500;
        Integral += Error;
        int Verify = Kp * Error + Kd * (Error - FinalError) + Ki * Integral;
        FinalError = Error;
        RightSpeed = RightBaseSpeed + Verify + ExtraSpeed;
        LeftSpeed = LeftBaseSpeed - Verify + ExtraSpeed;
        RightSpeed = constrain(RightSpeed, -100, 100);
        LeftSpeed = constrain(LeftSpeed, -100, 100);
        MotorsSpeed(LeftSpeed, RightSpeed);
    }

    // Sağa dönüş ile ana şeride geri dönüş
    MotorsSpeed(60, -30); 
    delay(300);  // sağdan sonra sola dön
    MotorsSpeed(-30,70); 
    delay(220);
}

void parkEt(){
  MotorsSpeed(100, 100);
  delay(250);
  MotorsSpeed(0,0);
  delay(10000);
}

bool engelVar = false;

void loop() {

  bool hepsiBeyaz = true;
  bool hepsiSiyah = true;
  int beyazEsik = 900;  // Beyaz için üst eşik değeri
  int siyahEsik = 200;  // Siyah için alt eşik değeri

  for (int i = 0; i < 8; i++) {
      int sensorValue = analogRead(Q[i]);

      if (sensorValue < beyazEsik) {
          hepsiBeyaz = false; // Eğer bir sensör değeri beyaz eşikten düşükse, hepsi beyaz değildir.
      }
      if (sensorValue > siyahEsik) {
          hepsiSiyah = false; // Eğer bir sensör değeri siyah eşikten yüksekse, hepsi siyah değildir.
      }

      // Eğer her iki koşul da yanlışsa, döngüden çıkabiliriz. Çünkü bir sensör bile diğer rengi gösteriyorsa, hepsi aynı renkte değildir.
      if (!hepsiBeyaz && !hepsiSiyah) {
          break;
      }
  }
  
  if(hepsiBeyaz){
    Serial.println("Park etmek istiyorum");
    //parkEt();
  }

  if (digitalRead(ObjectSensFront) == LOW || digitalRead(ObjectSensBack) == LOW) {
    delay(7); // Önceki okumadan kısa bir gecikme sağla
    unsigned long startTime = millis(); // Döngüye başlarken zamanı kaydet
    while ((digitalRead(ObjectSensFront) == LOW || digitalRead(ObjectSensBack) == LOW)) {
      MotorsSpeed(0, 0); // Motorları durdur
      engelVar = true;
      unsigned long currentTime = millis(); // Geçerli zamanı al
      if (currentTime - startTime >= 3500) { // 3 saniye kontrolü
          engelVar = false;
          solla();
        break; // Eğer 3 saniye geçtiyse döngüden çık
      }
    }
  }
  if(engelVar) {
    engelVar = false;
    delay(650);
  }
  Position = ReadSensor();
  Error = Position - 3500;
  Integral += Error;

  int Verify = Kp * Error + Kd * (Error - FinalError) + Ki * Integral;
  FinalError = Error;

  RightSpeed = RightBaseSpeed + Verify + ExtraSpeed ;
  LeftSpeed = LeftBaseSpeed - Verify +  ExtraSpeed ;

  RightSpeed = constrain(RightSpeed, -150, 150);
  LeftSpeed = constrain(LeftSpeed, -150, 150);
  MotorsSpeed(LeftSpeed, RightSpeed);
}