#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <Bounce2.h>
#include <Ticker.h>
#include <EEPROM.h>
#include <Servo.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define BTN 13

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

char dataIn;
float circumfrance = 1.2767;
int prevTime;
int timeout;
float time;
float v;
int cnt;
bool messageEnd = true;
Bounce btn = Bounce();
Bounce safe = Bounce();
static float safedData[60];
int sDLength = sizeof(safedData)/sizeof(safedData[0]);
int speicherPos = 0;
bool nullen = false;
bool voll = false;
float strecke;

void anzeigen ()
{
  display.clearDisplay();
  display.setCursor(0,10);
  display.print("km/h:");
  display.setCursor(0,30);
  display.print("Strecke:");
  display.setCursor(30,10);
  display.print(v);
  display.setCursor(50,30);
  display.print(strecke);
  display.display();
}

void vMessen ()
{
  nullen = false;
  if (prevTime > 0)
  {
    timeout = millis();
    time = (float)(millis()-prevTime)/1000;
    v = circumfrance/time*3.6;
    strecke += circumfrance;
    //Serial.println(speicherPos);
    //werteSpeichern(v);
  }
  prevTime = millis();
}

void werteSpeichern (float v)
{
  safedData[speicherPos] = v;
  speicherPos++;
  if (speicherPos >= 60)
  {
    voll = true;
    speicherPos = 0;
  }
  
}

void vRuecksetzen()
{
  if (millis()-prevTime > 6000 && !nullen)
  {
    v = 0;
    werteSpeichern(v);
    nullen = true;
  }
}

Ticker t1(anzeigen,400);


void setup() 
{
  Serial.begin(9600);
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  btn.attach( BTN ,  INPUT_PULLUP ); 
  btn.interval(5);
  safe.attach( 12 ,  INPUT_PULLUP ); 
  safe.interval(10);
  pinMode(11, OUTPUT);
  myServo.attach(11);
  t1.start();
}

void loop() 
{
  btn.update();
  safe.update();
  t1.update();
  if (btn.fell())
  {
    vMessen();
  }
  if (voll)
  {
    for (int j = 0; j < 60; j++)
    {
      EEPROM.write(j, safedData[j]);
    }
    Serial.println("voll");
    voll = false;
  }
  if (safe.fell())
  {
    /*for (int i = 0; i < 60; i++)
    {
      Serial.println(EEPROM.read(i));
    }
    */
    strecke = 0;
  }
  float winkel = map(v,0,50,0,180);
  myServo.write(winkel);
  
}