#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define sensor PD5
Adafruit_SSD1306 display(128 ,64, &Wire);

int previous_state = 0;
unsigned long time1, time2, calc;
float to_disp, to_disp_2;


void setup() {
  pinMode(sensor, INPUT);
  
  Serial.begin(9600);
  Serial.println("Dzialam...");
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  Serial.println("Init OLED");
  display.display();
  delay(2000);
   display.clearDisplay ();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0); 
  display.println(F("Laser"));
  display.setCursor(12,10); 
  display.println(F("Shutter"));
  display.setCursor(24,20); 
  display.println(F("Tester"));
  display.setCursor(90,50); 
  display.println(F("v. 1.0a"));
  display.display();
  Serial.println("Wyswietlilem powitanie");
  delay(3000);
  display.clearDisplay();
  display.setTextSize(3);
  display.setTextColor(WHITE);
  display.setCursor(20,20); 
  display.println(F("Ready..."));
  display.display();
  Serial.println("Wyswietlilem status");
}

void wyswietl(float wartosc)
{
  to_disp = float(wartosc)/1000;
  display.clearDisplay();
  if (to_disp <1.0 )
    {
      to_disp_2 = 1.0/to_disp;
      display.setTextSize(2);             // Normal 1:1 pixel scale
      display.setCursor(20,14);
      display.println(F("1"));
      display.drawLine(5,32,42,32, WHITE);
      display.setCursor(25,34);
      display.println(to_disp_2,DEC);
      display.drawLine(50,0,50,64, WHITE);
      display.setCursor(60,24);
      display.println(to_disp,DEC);
      display.setCursor(110,23);
      display.println(F("[s]"));
      Serial.println("Wyswietlilem obliczenia < 1");
    }
  else 
    {
    display.setCursor(15,20);
    display.println(to_disp,DEC);
    display.setCursor(100,20);
    display.println(F("s"));
    Serial.println("Nie mam co wyswietlic");
    }
  display.display();
}


void loop() {
  int sensorState = digitalRead(sensor);
  
  if (previous_state == 0 && sensorState == HIGH)
    {
      time1 = millis();
      previous_state = 1;
      Serial.println("zmiana statusu");
    }
  if (previous_state == 1 && sensorState == LOW)
    {
      calc = millis() - time1;
      wyswietl(calc);
      Serial.println("Wyslalem do wyswietlacza");
      previous_state = 0;
    }
Serial.println(sensorState);
Serial.print("---");
delay(10);
}