#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SH110X.h>  // Biblioteca para o display Grove SH1107

// Define o endereço I2C do display e usa A4 (SDA) e A5 (SCL) para comunicação
Adafruit_SH1107 display = Adafruit_SH1107(64, 128, &Wire);

// Pinos dos fotorresistores
const int laser1Pin = 5;  // Primeiro fotorresistor
const int laser2Pin = 6;  // Segundo fotorresistor

// Variáveis para medir o tempo
unsigned long startTime;
unsigned long endTime;

// Distância entre os lasers (em metros)
const float distanceBetweenLasers = 0.5;

void setup() {
  Serial.begin(9600);

  // Configura as conexões I2C nas portas A4 (SDA) e A5 (SCL)
  Wire.begin();

  // Iniciar o display OLED
  if(!display.begin(0x3C, true)) {  // Endereço I2C padrão: 0x3C
    Serial.println(F("OLED não encontrado!"));
    while(1);
  }

  // Limpar o display
  display.clearDisplay();
  
  // Configurar os pinos dos fotorresistores como entrada
  pinMode(laser1Pin, INPUT);
  pinMode(laser2Pin, INPUT);
}

void loop() {
  int laser1State = digitalRead(laser1Pin);
  int laser2State = digitalRead(laser2Pin);

  // Detecta quando o primeiro laser é interrompido
  if (laser1State == LOW) {
    startTime = millis();  // Armazena o tempo de início
  }

  // Detecta quando o segundo laser é interrompido
  if (laser2State == LOW) {
    endTime = millis();  // Armazena o tempo de fim

    // Calcula o tempo gasto entre os lasers (em segundos)
    float timeTaken = (endTime - startTime) / 1000.0;

    // Calcula a velocidade (m/s)
    float speed = distanceBetweenLasers / timeTaken;

    // Exibe a velocidade no display OLED
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SH110X_WHITE);
    display.setCursor(0, 0);
    display.print("Velocidade: ");
    display.print(speed);
    display.println(" m/s");
    display.display();

    Serial.print("Velocidade: ");
    Serial.print(speed);
    Serial.println(" m/s");
  }
}