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

#define SCREEN_WIDTH 128 // OLED width,  in pixels
#define SCREEN_HEIGHT 64 // OLED height, in pixels
 
// create an OLED display object connected to I2C
//               name                              I2C Connection  -1 is reset pin
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

int count = 0; // Zähler der Zeit in MS
float MaxSpeed = 2; // Setzen der Maximalen Geschwindigkeit
float speed; // Spätere verwendung für die Geschw. in Kmh
bool mp1 = false; // Messpunkt 1
bool mp2 = false; // Messupunkt 2



void setup() {
  //Initziiren der beiden Messpunkte
  pinMode(23, INPUT); 
  pinMode(19, INPUT);
  //Initziiren des Blitzes und des Auslösers
  pinMode(18, OUTPUT);
  pinMode(5, OUTPUT);
  //Ohne diesen befehl funktioniert Serial.print nicht
  Serial.begin(115200);

}

void loop() {

  if(digitalRead(23) == HIGH){ //Wurde der 1. Messpunkt (MP1) ausgelöst?
    mp1 = true; 
  }
  if(digitalRead(19) == HIGH){ //Wurde der 2. Messpunkt (MP2) ausgelöst?
    mp1 = false; //MP1 Deaktivieren
    mp2 = true; //MP2 Aktivieren
  }

  //Zeitzähler in ms
  if(mp1 == true){
    delay(1);
    Serial.print(count);
    Serial.print('\n');
    count++;
  }

  //Ausrechnen und ausgeben der Geschwindigkeit in Kmh
  if(mp2 == true){
    mp2 = false;
    float TIS = count / 1000.0;
    speed = (1.0 / TIS) * 3.6;
    Serial.print("Es wurden "); Serial.print(speed); Serial.println(" Km/h gemessen.");

    // initialize OLED display       I2C Connection
    oled.begin(SSD1306_SWITCHCAPVCC, 0x3C);
    oled.clearDisplay();
    oled.setTextSize(1);
    oled.setTextColor(WHITE);
    oled.setCursor(0, 0);
    oled.print("Sie sind "); oled.println(speed); oled.println("Km/h gefahren!");
    oled.display();
    
    count = 0;
    
  } 

  //Wurde die Maximale geschwindigkeit überschritten? Wenn ja, Blitz und Foto!
  if(speed > MaxSpeed){
    Serial.println("Geschwindigkeitsüberschreitung gemessen!");
    
    digitalWrite(18, HIGH);
    digitalWrite(5, HIGH);
    delay(100);
    digitalWrite(18, LOW);
    digitalWrite(5, LOW);

    
    //Speed wird auf 0 gesetzt, so wird diese if-Abfrage angehalten 
    //ohne einen weiteren Bool zu benötigen
    speed = 0;
  }
}