#include <ESP32Servo.h>

// Definiowanie obiektów serwomechanizmów
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;


// Definiowanie pinów serwomechanizmów
const int servoPin1 = 2;
const int servoPin2 = 4;
const int servoPin3 = 5;
const int servoPin4 = 15;
// Definiowanie pinów silników
const int motorPin11 = 18;
const int motorPin12 = 23;  // Silnik 1
const int motorPin21 = 19;
const int motorPin22 = 22;  // Silnik 2
const int motorPin31 = 16;
const int motorPin32 = 21;  // Silnik 2
const int motorPin41 = 17;
const int motorPin42 = 0;  // Silnik 2

void setup() {
  // Przyłącz serwa do odpowiednich pinów
  servo1.attach(servoPin1);
  servo2.attach(servoPin2);
  servo3.attach(servoPin3);
  servo4.attach(servoPin4);

  pinMode(motorPin11,OUTPUT);
  pinMode(motorPin12,OUTPUT);
  pinMode(motorPin21,OUTPUT);
  pinMode(motorPin22,OUTPUT);
  pinMode(motorPin31,OUTPUT);
  pinMode(motorPin32,OUTPUT);
  pinMode(motorPin41,OUTPUT);
  pinMode(motorPin42,OUTPUT);

  

  // Inicjalizacja komunikacji szeregowej (dla konsoli)
  Serial.begin(115200);
  Serial.println("Sterowanie serwomechanizmami i silnikami! Wpisz komendy w formacie:");
  Serial.println("'s1 <kat>' - dla serwa na pinie 2");
  Serial.println("'s2 <kat>' - dla serwa na pinie 4");
  Serial.println("'s3 <kat>' - dla serwa na pinie 5");
  Serial.println("'ch <open/close>' - dla silnika na pinie 15");
  Serial.println("'m1 <forward/backward/stop>' - dla silnika na pinie 18");
  Serial.println("'m2 <forward/backward/stop>' - dla silnika na pinie 19");
  Serial.println("'m3 <forward/backward/stop>' - dla silnika na pinie 16");
  Serial.println("'m4 <forward/backward/stop>' - dla silnika na pinie 17");
  Serial.println("Przykład: s1 90, m1 forward");
}

void loop() {
  // Sprawdzenie, czy jest dostępna nowa linia z konsoli
  if (Serial.available() > 0) {
    String input = Serial.readStringUntil('\n');  // Odczyt linii z konsoli
    input.trim();  // Usunięcie białych znaków na początku i końcu

    // Przetwarzanie komendy dla serwomechanizmów
    if (input.startsWith("s1 ")) {
      int angle = input.substring(3).toInt();  // Pobranie kąta dla serwa 1
      if (angle >= 0 && angle <= 180) {
        servo1.write(angle);
        Serial.print("Serwo 1 ustawione na: ");
        Serial.println(angle);
      } else {
        Serial.println("Kąt musi być w zakresie 0-180");
      }
    } else if (input.startsWith("s2 ")) {
      int angle = input.substring(3).toInt();  // Pobranie kąta dla serwa 2
      if (angle >= 0 && angle <= 180) {
        servo2.write(angle);
        Serial.print("Serwo 2 ustawione na: ");
        Serial.println(angle);
      } else {
        Serial.println("Kąt musi być w zakresie 0-180");
      }
    } else if (input.startsWith("s3 ")) {
      int angle = input.substring(3).toInt();  // Pobranie kąta dla serwa 3
      if (angle >= 0 && angle <= 180) {
        servo3.write(angle);
        Serial.print("Serwo 3 ustawione na: ");
        Serial.println(angle);
      } else {
        Serial.println("Kąt musi być w zakresie 0-180");
      }
    }
    // Przetwarzanie komendy dla chwytaka
    else if (input.startsWith("ch ")) {
      String command = input.substring(3);
      if (command == "open") {
        servo4.write(180);  // Otwarcie
        Serial.println("chwytak: otwarty");
      } else if (command == "close") {
        servo4.write(0);  // zamknięcie
        Serial.println("chwytaka: zamknięty");
      } else {
        Serial.println("Nieprawidłowa komenda dla silnika 1. Użyj 'forward', 'backward' lub 'stop'.");
      }
    }
    // Przetwarzanie komendy dla silnika 1
    else if (input.startsWith("m1 ")) {
      String command = input.substring(3);
      if (command == "forward") {
        digitalWrite(motorPin11, HIGH);
        digitalWrite(motorPin12, LOW);  // Uruchomienie silnika do przodu
        Serial.println("Silnik 1: do przodu");
      } else if (command == "backward") {
        digitalWrite(motorPin12, HIGH);
        digitalWrite(motorPin11, LOW);  // Uruchomienie silnika do tyłu
        Serial.println("Silnik 1: do tyłu");
      } else if (command == "stop") {
        digitalWrite(motorPin12, LOW);
        digitalWrite(motorPin11, LOW);  // Zatrzymanie silnika
        Serial.println("Silnik 1: stop");
      } else {
        Serial.println("Nieprawidłowa komenda dla silnika 1. Użyj 'forward', 'backward' lub 'stop'.");
      }
    }
    
    // Przetwarzanie komendy dla silnika 2
    else if (input.startsWith("m2 ")) {
      String command = input.substring(3);
      if (command == "forward") {
        digitalWrite(motorPin21, HIGH);
        digitalWrite(motorPin22, LOW);  // Uruchomienie silnika do przodu
        Serial.println("Silnik 2: do przodu");
      } else if (command == "backward") {
        digitalWrite(motorPin22, HIGH);
        digitalWrite(motorPin21, LOW);  // Uruchomienie silnika do tyłu
        Serial.println("Silnik 2: do tyłu");
      } else if (command == "stop") {
        digitalWrite(motorPin22, LOW);
        digitalWrite(motorPin21, LOW);  // Zatrzymanie silnika
        Serial.println("Silnik 2: stop");
      } else {
        Serial.println("Nieprawidłowa komenda dla silnika 2. Użyj 'forward', 'backward' lub 'stop'.");
      }
    }
    // Przetwarzanie komendy dla silnika 3
    else if (input.startsWith("m3 ")) {
      String command = input.substring(3);
      if (command == "forward") {
        digitalWrite(motorPin31, HIGH);
        digitalWrite(motorPin32, LOW);  // Uruchomienie silnika do przodu
        Serial.println("Silnik 3: do przodu");
      } else if (command == "backward") {
        digitalWrite(motorPin32, HIGH);
        digitalWrite(motorPin31, LOW);  // Uruchomienie silnika do tyłu
        Serial.println("Silnik 3: do tyłu");
      } else if (command == "stop") {
        digitalWrite(motorPin32, LOW);
        digitalWrite(motorPin31, LOW);  // Zatrzymanie silnika
        Serial.println("Silnik 3: stop");
      } else {
        Serial.println("Nieprawidłowa komenda dla silnika 3. Użyj 'forward', 'backward' lub 'stop'.");
      }
    }
    // Przetwarzanie komendy dla silnika 1
    else if (input.startsWith("m4 ")) {
      String command = input.substring(3);
      if (command == "forward") {
        digitalWrite(motorPin41, HIGH);
        digitalWrite(motorPin42, LOW);  // Uruchomienie silnika do przodu
        Serial.println("Silnik 4: do przodu");
      } else if (command == "backward") {
        digitalWrite(motorPin42, HIGH);
        digitalWrite(motorPin41, LOW);  // Uruchomienie silnika do tyłu
        Serial.println("Silnik 4: do tyłu");
      } else if (command == "stop") {
        digitalWrite(motorPin42, LOW);
        digitalWrite(motorPin41, LOW);  // Zatrzymanie silnika
        Serial.println("Silnik 4: stop");
      } else {
        Serial.println("Nieprawidłowa komenda dla silnika 4. Użyj 'forward', 'backward' lub 'stop'.");
      }
    }
    
    // Błędna komenda
    else {
      Serial.println("Nieprawidłowa komenda. Użyj 's1', 's2', 's3', 'm1' lub 'm2' + odpowiednia wartość.");
    }
  }
}