#define SERVO 5
#define RESET 12
#define TACHO 13
#define PRO_UMDREHUNG 1.2
#define WARTEZEIT_VOR_RESET 5000
#define TACHO_UPDATE_TIME 1000
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Servo.h>
#include <TickTwo.h>
#include "debounce.h"

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Servo servo;


float speed = 0;
float distance = 0;
char is_spinning = 0;
char bufferspeed[5];
char bufferdistance[6];
unsigned long formerspeedtime, currentspeedtime;


void setup() 
{
  servo.attach(5); 
  Serial.begin(115200);

  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 10);
  // Display static text
  display.println("Hello, world!");
  display.display();

  pinMode(SERVO, OUTPUT);
  pinMode(RESET, INPUT_PULLUP);
  pinMode(TACHO, INPUT_PULLUP);
  formerspeedtime = millis();
}

void anzeige ()
{
  display.clearDisplay();
  display.setTextSize(2);
  display.setCursor(0, 10);
  display.println(speed);
  display.setCursor(50, 10);
  display.print("km/h");
  display.setTextSize(1);
  display.setCursor(0, 40);
  display.println(distance);
  display.setCursor(30, 40);
  display.print("m");
  display.setCursor(0, 55);
  display.println(currentspeedtime-formerspeedtime);
  display.display();
}

void loop() 
{
  currentspeedtime = millis();
  if (debounce(TACHO))
  {
    if (is_spinning)
    {
      speed = 3.6 * (PRO_UMDREHUNG) / ((currentspeedtime - formerspeedtime)/1000);
      distance += PRO_UMDREHUNG;
      
    }
    else
    {
      is_spinning = 1;
    }
    formerspeedtime = currentspeedtime;
  }
  anzeige();
  if (!digitalRead(RESET))
  {
    distance = 0.0;
    anzeige();
  }
  if ((millis()-formerspeedtime)>WARTEZEIT_VOR_RESET)
  {
    speed = 0.0;
    if (is_spinning)
    {
      anzeige();
    }
    is_spinning = 0;
  }
}