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

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

constexpr uint8_t SENSOR1_PIN = 25;
constexpr uint8_t SENSOR2_PIN = 26;

constexpr uint8_t SENSOR3_PIN = 13;
constexpr uint8_t SENSOR4_PIN = 14;

unsigned long last_run = 0;

constexpr float DISTANCE_METERS = 0.05f; // 5 cm

int counter = 20;
int H;
int L;

Adafruit_SSD1306 display(
    SCREEN_WIDTH,
    SCREEN_HEIGHT,
    &Wire,
    -1
);

unsigned long startTime = 0;
bool waitingForSecond = false;

bool lastSensor1 = HIGH;
bool lastSensor2 = HIGH;

void showVelocity(float velocity)
{
    display.clearDisplay();
    display.setCursor(0, 20);
    display.print("Speed:");
    display.setCursor(0, 40);
    display.print(velocity, 2);
    display.print(" m/s");
    display.display();
}

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

    pinMode(SENSOR1_PIN, INPUT);
    pinMode(SENSOR2_PIN, INPUT);
    pinMode(SENSOR3_PIN, INPUT);
    pinMode(SENSOR4_PIN, INPUT);

    Wire.begin();

    if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))
    {
        Serial.println("OLED not found!");
        while (true);
    }

    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);

    display.setCursor(0, 20);
    display.println("System Ready");
    display.display();
}

 
void loop()
{
 

   //Gewicht einstellung
    //-------------------------------------------------------

     if (digitalRead(H) == HIGH) {
      counter++;
      if (counter > 40) counter = 40;
    }
    if (digitalRead(L) == HIGH) {
      counter--;
      if (counter < 20) counter = 20;
    }
    

    //-------------------------------------------------------

   int H = digitalRead(SENSOR3_PIN);
    int L = digitalRead(SENSOR4_PIN);

    bool sensor1 = !digitalRead(SENSOR1_PIN);
    bool sensor2 = !digitalRead(SENSOR2_PIN);


    Serial.print(counter);

    if(SENSOR3_PIN >= 15){
       Serial.print("Taster1");
    }

    if(SENSOR4_PIN >= 20){
       Serial.print("Taster2");
    }


    //Messung
    //-------------------------------------------------------

    // Flankenerkennung Sensor 1
    if (!waitingForSecond && sensor1 && !lastSensor1)
    {
        startTime = micros();
        waitingForSecond = true;

        Serial.println("Sensor 1 triggered");
    }

    // Flankenerkennung Sensor 2
    if (waitingForSecond && sensor2 && !lastSensor2)
    {
        unsigned long endTime = micros();

        float timeSec = (endTime - startTime) / 1000000.0f;

        if (timeSec > 0.0f)
        {
            float velocity = DISTANCE_METERS / timeSec;

            Serial.print("Velocity: ");
            Serial.print(velocity, 2);
            Serial.println(" m/s");

            showVelocity(velocity);
        }

        waitingForSecond = false;
    }

    lastSensor1 = sensor1;
    lastSensor2 = sensor2;
}