#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ESP32Servo.h>

Servo myservo;
int pos = 0;

// pin del sensore HC-SR04
#define PIN_TRIG 4
#define PIN_ECHO 2

// dimensioni del display OLED
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// indirizzo I2C del display OLED
#define OLED_RESET -1
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// dichiarazioni variabili misura
long duration, distCm, distInch;

// variabili per il timeout di inattività
unsigned long lastMovementTime = 0;  // Tempo dell'ultimo movimento

void setup() {
  Serial.begin(115200);
  myservo.attach(13, 500, 2400); // configurazione pin del servo motore

  // configurazione dei pin del sensore
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);

  // inizializzazione del display OLED
  if (!display.begin(SSD1306_BLACK, 0x3C)) {
    Serial.println(F("errore nell'avvio del display OLED"));
    for (;;);
  }
  display.clearDisplay();
}

void loop() {
  // trigger del sensore
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);

  // lettura dell'eco
  duration = pulseIn(PIN_ECHO, HIGH);
  distCm = duration / 58;
  distInch = duration / 148;

  // stampa della distanza sul serial monitor
  Serial.println("Misuratore della Distanza:");
  Serial.print(distCm);
  Serial.println(" CM");
  Serial.print(distInch);
  Serial.println(" INCH");

  // se la distanza è inferiore o uguale a 300 cm, muovi il servo motore a 90°
  if (distCm <= 300) {
    Serial.println("Apertura del servo motore");
    myservo.write(90);  // Imposta il servo motore a 90°
    lastMovementTime = millis();  // Resetta il timer dell'inattività
  } else {
    // Se sono passati più di 5 secondi senza movimento, torna alla posizione iniziale
    if (millis() - lastMovementTime >= 5000) {
      Serial.println("Timeout inattività - Servo ritorna a 0°");
      myservo.write(0);  // Torna alla posizione iniziale
    }
  }
  
  // visualizzazione sul Monitor OLED
  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(0, 0);
  display.print("Distanza: ");
  display.print(distCm);
  display.println(" CM");
  display.print(distInch);
  display.println(" INCH");
  display.display();

  delay(500);  // Pausa di mezzo secondo
}