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

#define sensor  14
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);


int previous_state = 0;
bool finished, read_ready = false;
unsigned long time1, time2, calc;
float to_disp, to_disp_2;


void setup() {
 
  Serial.begin(9600);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C))  // Address 0x3C for 128x64
    Serial.println("SSD1306 allocation failed");
    for(;;); // Don't proceed, loop forever  
  pinMode(sensor, INPUT);
  display.clearDisplay ();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0,0); 
  display.println("Laser");
  display.setCursor(12,10); 
  display.println("Shutter");
  display.setCursor(24,20); 
  display.println("Tester");
  display.setCursor(90,50); 
  display.println("v. 1.0a");
  display.display();
  delay(3000);
  display.clearDisplay();
  display.setTextSize(3);
  display.setTextColor(WHITE);
  display.setCursor(20,20); 
  display.println("Ready...");
  display.display();
}

void loop() {

  int sensorState = digitalRead(sensor);
  if (previous_state == 0 && sensorState == HIGH){
    time1 = millis();
    previous_state = 1;
  }
  if (previous_state == 1 && sensorState == LOW){
    time2 = millis();
    previous_state = 0;
    read_ready = true;
  }
  if(read_ready == true){
    calc = time2-time1;
    to_disp = float(calc)/1000;
    read_ready = false;
    finished = true;
  }
  if(finished == true){
    if(to_disp<1.0){
      to_disp_2 = 1/to_disp;
      display.clearDisplay();
      display.setTextSize(2);             // Normal 1:1 pixel scale
      display.setCursor(20,14); 
      display.println("1");
      display.drawLine(5,32,42,32, WHITE);
      display.setCursor(25,34);
      display.println(to_disp_2,0);
      display.drawLine(50,0,50,64, WHITE);
      display.setCursor(60,24);
      display.println(to_disp,3);
      display.setCursor(110,23); 
      display.println("[s]");
      display.display();
    } else {
    display.clearDisplay();
    display.setCursor(15,20); 
    display.println(to_disp,3);
    display.setCursor(100,20); 
    display.println("s");
    display.display();
    }
    finished = false;
  }
  
  delay(1);
}