#include <SPI.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
//
// OLED Parameter
//
#define OLED_RESET    -1 
#define SCREEN_ADDRESS 0x3C 
#define OLED_WIDTH 128 
#define OLED_HEIGHT 64 
//
// OLED Init
//
Adafruit_SSD1306 oled(OLED_WIDTH, OLED_HEIGHT, &Wire, OLED_RESET);
//
// IO Port Definitionen
//
#define LED_PIN       16 
#define TASTER        35  
#define Trigger_PIN   25 
#define Echo_PIN      26 
//
// Div. Variablen
//
unsigned long Duration;
int Distance;
//
// Setup Programm
//
void setup() {
  //
  // Serieller Port für Debug Ausgaben, falls notwendig
  //
  Serial.begin(9600);
  //
  // OLED Starten
  //
  oled.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS);
  //
  // Taster init
  //
  pinMode(TASTER, INPUT);
  //
  // Rote LED
  //
  pinMode(LED_PIN, OUTPUT);
  //
  // Ultraschall Sensor
  //
  pinMode(Trigger_PIN, OUTPUT);
  pinMode(Echo_PIN, INPUT);
  // 
  // LED ausschlten
  //
  digitalWrite(LED_PIN, LOW);
}

//
// Programmschleife
//
void loop() {
  if(digitalRead(TASTER) == LOW) {
    //
    // LED an
    //
    digitalWrite(LED_PIN, HIGH);     
    //
    // Starte Entfernungs-Messung
    //
    digitalWrite(Trigger_PIN, HIGH);
    delayMicroseconds(20);
    digitalWrite(Trigger_PIN, LOW);
    //
    // Verzögerung einlesen
    //
    Duration = pulseIn(Echo_PIN, HIGH);
    // 
    // Distanz errechnen
    //
    Distance = Duration * 0.03432 / 2;
    //
    // Ergebnis anzeigen
    //
    testdrawtextON(Distance); 
  }
  else {
    testdrawtextOFF(Distance);
    digitalWrite(LED_PIN, LOW);
  }
}

//
// Ausgabe, wenn Taster gedrückt ist
//
void testdrawtextON(int distance) {
  char s[40], sl[40];  

  sprintf(s,"  %d cm",distance);
  oled.clearDisplay();
  oled.setTextSize(2);  
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0,0);   
  oled.println(F(" Messung.."));
  oled.println("");

  sprintf(sl,"%d",distance);
  if (strlen(sl) == 1)
    sprintf(s,"  %d cm",distance);
  if (strlen(sl) == 2)
    sprintf(s," %d cm",distance);
  if (strlen(sl) == 3)
    sprintf(s,"%d cm",distance);
  oled.setTextSize(3); 
  oled.println(F(s));
  oled.display();
}

//
// Ausgabe, wenn Taster nicht gedrückt ist
//
void testdrawtextOFF(int distance) { 
  char s[40], sl[40];

  oled.clearDisplay();
  oled.setTextSize(2);  
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0,0);   
  oled.println(F("Entfernung"));
  oled.println("");

  sprintf(sl,"%d",distance);
  if (strlen(sl) == 1)
    sprintf(s,"  %d cm",distance);
  if (strlen(sl) == 2)
    sprintf(s," %d cm",distance);
  if (strlen(sl) == 3)
    sprintf(s,"%d cm",distance);
  oled.setTextSize(3); 
  oled.println(F(s));
  oled.display();
}