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

#define PIN_IN 7
#define PIN_2 2
#define PIN_5 3

#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
// The pins for I2C are defined by the Wire-library. 
// On an arduino UNO:       A4(SDA), A5(SCL)
// On an arduino MEGA 2560: 20(SDA), 21(SCL)
// On an arduino LEONARDO:   2(SDA),  3(SCL), ...
#define OLED_RESET     4 // Reset pin # (or -1 if sharing Arduino reset pin)
#define SCREEN_ADDRESS 0x3C ///< See datasheet for Address; 0x3D for 128x64, 0x3C for 128x32
Adafruit_SSD1306 oled(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

float time;
unsigned long t = 0, delta_t;
byte read;


void setup()
{
  pinMode(PIN_IN, INPUT);
  pinMode(PIN_2, OUTPUT);
  pinMode(PIN_5, OUTPUT);

  Serial.begin(9600);
  oled.begin(SSD1306_EXTERNALVCC, SCREEN_ADDRESS);
  oled.clearDisplay();
}


void loop() {
  read = digitalRead(PIN_IN);

  if (read)
  {
    digitalWrite(PIN_2, LOW);
    digitalWrite(PIN_5, LOW);
    if (!t)
    {
      t = millis();
    }
  } else
  {
    if (t)
    {
      delta_t = millis() - t;
      t = 0;

      if (delta_t > 1000 && delta_t < 3000) digitalWrite(PIN_2, HIGH);
      if (delta_t > 3500 && delta_t < 5500) digitalWrite(PIN_5, HIGH);
    }
  }

  oled.clearDisplay();
  oled.setTextSize(2); // Draw 2X-scale text
  oled.setTextColor(SSD1306_WHITE);
  oled.setCursor(0, 0);
  oled.print("Time:");
  oled.setCursor(0, 20);
  if (!t)
  {
    oled.print("....");
  } else 
  {
    oled.print(millis()-t);
  }
  oled.setCursor(0,40);
  oled.print(read);
  oled.display();

  Serial.print(digitalRead(PIN_IN));

  delay(50);
}