#include <ESP32Servo.h>

// Definirea pinilor
#define TRIG_PIN 16
#define ECHO_PIN 4
#define SERVO_PIN 14
#define LED 17
#define BTN_VERDE 26
#define BTN_ROSU 27

// Initializare obiecte

Servo bratRobotic;    // obiect de tip Servo 

void TaskCitireSenzori(void *pvParameters);

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  
  // Setare pini LED
  pinMode(LED, OUTPUT);
  
  // Setare pini Servo
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  
  // Setare pini Butoane
  pinMode(BTN_VERDE, INPUT);
  pinMode(BTN_ROSU, INPUT);

  // Pornire componente
  bratRobotic.attach(SERVO_PIN);
  bratRobotic.write(0); // punem valva la 0 grade 

  xTaskCreate(
    TaskCitireSenzori,   // Funcția task-ului
    "Senzori",     // Nume
    2048,          // Memorie alocată
    NULL,          // Parametri
    1,             // Prioritate
    NULL           // Handle
  );
}

void loop() {
  // GOL
}

void TaskCitireSenzori(void *pvParameters) {
  // Citire butoane
    int stareVerde = digitalRead(BTN_VERDE);
    int stareRosu = digitalRead(BTN_ROSU);

    // Citire senzor ultrasonic (Emitem un puls scurt de sunet)
    digitalWrite(TRIG_PIN, LOW);
    delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH); // Difuzorul țipă timp de 10 microsecunde
    delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);

    // Ascultăm ecoul. Funcția pulseIn ne dă timpul în microsecunde
    long durata = pulseIn(ECHO_PIN, HIGH);
    
    // Viteza sunetului e 0.034 cm/microsecundă. Împărțim la 2 (dus-întors)
    float distanta_cm = durata * 0.034 / 2;

    // Afisare in consola
    Serial.print("Distanta: ");
    Serial.print(distanta_cm);
    Serial.print(" cm | Btn VERDE: ");
    Serial.print(stareVerde);
    Serial.print(" | Btn ROSU: ");
    Serial.println(stareRosu);

    // Task-ul ia o pauză de 200ms
    vTaskDelay(200 / portTICK_PERIOD_MS);
}