// Definicje pinów PWM dla silników
const int in1 = 3; // pin sterujący dla Motor1 i Motor2
const int in2 = 5; // pin sterujący dla Motor1 i Motor2 (jazda do tyłu )
const int in3 = 6; // pin sterujący dla Motor3 i Motor4
const int in4 = 9; // pin sterujący dla Motor3 i Motor4 (jazda do tyłu)

// Definicje pinów dla czujnika HC-SR04
const int trigPin = 10;
const int echoPin = 11;

// Ustawienie PWM na 100% mocy (wartość 0-255)
const int speed = 175; // 100% mocy 255, ale w programie używane w większości i tak wartości, nie "speed"

// Licznik przeszkód - używamy żeby za nim nie biegać, po ominięciu ilu przeszkód ma się sam zatrzymać
int obstacleCount = 0;

enum moveStates {STOP, FORWARD, BACK, LEFT, FORWARD2} moveState = FORWARD;
unsigned long timeGas, timeCar;

void gasSensor(){
  if(millis() - timeGas > 200){
    timeGas = millis();
    int wartoscAnalog = analogRead(A0);
    byte wartoscZmapowana = map(wartoscAnalog, 0, 1023, 0, 100);
    char info[96];
    sprintf(info, "MQ-2: %d (%d)", wartoscAnalog, wartoscZmapowana);

    if (wartoscZmapowana > 20) {
      Serial.print(info);
      Serial.println(F(" UWAGA! WYKRYTO GAZ LUB DYM "));
      tone(A1, 262);
    }
    else {
      Serial.println(info);
      noTone(A1);
    }
  }
}

void car(){
  long distance = readUltrasonicDistance();

  //handle states transitions
  if(obstacleCount >= 10) {moveState = STOP;}
  else if(moveState == FORWARD && distance < 100) {moveState = BACK; timeCar = millis();}
  else if(moveState == BACK && millis() - timeCar > 1000){moveState = LEFT; timeCar = millis();}
  else if(moveState == LEFT && millis() - timeCar > 250){moveState = FORWARD2; timeCar = millis();}
  else if(moveState == FORWARD2 && millis() - timeCar > 250){moveState = FORWARD; obstacleCount++;}

  //handle states job
  if(moveState == STOP){carStop();}
  else if(moveState == FORWARD){carMoveForward();}  
  else if(moveState == FORWARD2){carMoveForward();} 
  else if(moveState == BACK){carMoveBack();}
  else if(moveState == LEFT){carMoveLeft();}
}

// Funkcja do pomiaru odległości
long readUltrasonicDistance() {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long duration = pulseIn(echoPin, HIGH);
  long distance = (duration / 2) / 29.1; // Przeliczenie na cm
  return distance;
}

void carStop(){
  // Zatrzymanie wszystkich silników
  analogWrite(in1, 0);
  analogWrite(in2, 0);
  analogWrite(in3, 0);
  analogWrite(in4, 0);
}

void carMoveForward(){
  // Jazda prosto
  analogWrite(in1, 0); // Motor1 i Motor2 tył ( to jest ustawione odwrotnie niż wynika z opisu pinów na początku... UWAGA kabelki na jednym silniku są odwrotnie przylutowane... :D
  analogWrite(in2, 175); // Motor1 i Motor2 przód
  analogWrite(in3, 197); // Motor3 i Motor4 przód - Autko nie jest dobrze wyważone/silniki nie pracuą równo dlatego te wartości musza być różne, żeby jechało w miarę równo do przodu nie znosiło go...
  analogWrite(in4, 0); // Motor3 i Motor4 tył
}

void carMoveBack(){
  // Jazda do tyłu
  analogWrite(in1, 150);  // Motor1 i Motor2 tył
  analogWrite(in2, 0);    // Motor1 i Motor2 przód
  analogWrite(in3, 0);    // Motor3 i Motor4 przód
  analogWrite(in4, 155);
}

void carMoveLeft(){
  // Skręt w lewo (prawe koła przód, lewe koła tył)
  analogWrite(in1, 175);  // Motor1 i Motor2 tył
  analogWrite(in2, 0);    // Motor1 i Motor2 przód
  analogWrite(in3, 175);  // Motor3 i Motor4 przód
  analogWrite(in4, 0);    // Motor3 i Motor4 tył
}

void setup() {
  Serial.begin(9600);
  pinMode(A1, OUTPUT);

  // Ustawienie pinów jako wyjścia
  pinMode(in1, OUTPUT);
  pinMode(in2, OUTPUT);
  pinMode(in3, OUTPUT);
  pinMode(in4, OUTPUT);

  // Ustawienie pinów dla czujnika HC-SR04
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  // Wyłączenie wszystkich silników na początku
  carStop();

  // Początkowe opóźnienie żeby nie jechal od razu jak go podłączymy
  delay(5000);
}

void loop() {
  gasSensor();
  car();
}