#include <Adafruit_SSD1306.h>
#include <U8g2_for_Adafruit_GFX.h>

#define S_WIDTH 128
#define S_HEIGHT 64

#define RED_PIN 6
#define GREEN_PIN 5
#define YELLOW_PIN 4

#define BUTTON_PIN 7

#define DEBOUNCE_DELAY 50

#define OLED_RESET 4
Adafruit_SSD1306 display(S_WIDTH, S_HEIGHT, &Wire, OLED_RESET);
U8G2_FOR_ADAFRUIT_GFX u8g2_for_adafruit_gfx;

String colors[] = {"zółta ON","żółta OFF","zielona ON","zielona OFF","czerwona ON", "czerwona OFF"};
int pins[] = {4, 4, 5, 5, 6, 6};
int actual_state = 0;
int max_state = 5;
bool button_state;
bool last_button_state = true;
unsigned int last_button_click_time = 0;


void switchLight (String state, int pin, bool action) {
  display.clearDisplay();
  u8g2_for_adafruit_gfx.setCursor(0,25);
  u8g2_for_adafruit_gfx.println("Dioda");
  u8g2_for_adafruit_gfx.println(state);
  display.display();
  if (action)
    digitalWrite(pin, HIGH);
  else
    digitalWrite(pin, LOW);
}

bool readButtonStateChange () {
  int reading = digitalRead(BUTTON_PIN);
  if (reading != last_button_state) {
    last_button_click_time = millis();
  }

  if ((millis() - last_button_click_time) > DEBOUNCE_DELAY) {
    if (reading != button_state) {
      button_state = reading;
      if (button_state == LOW)
        return true;
    }
  }
  last_button_state = reading;
  return false;
}


void setup() {
  Serial.begin(9600);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3c)) {
    Serial.println(F("SSD1306 allocation failed"));
    exit(1);
  }
  u8g2_for_adafruit_gfx.begin(display);
  u8g2_for_adafruit_gfx.setForegroundColor(WHITE);
  u8g2_for_adafruit_gfx.setFont(u8g2_font_7x13_te);
  pinMode(RED_PIN, OUTPUT);
  pinMode(GREEN_PIN, OUTPUT);
  pinMode(YELLOW_PIN, OUTPUT);
  pinMode(BUTTON_PIN, INPUT_PULLUP);
  switchLight(colors[actual_state], pins[actual_state], actual_state%2==0);
}

void loop() {
  if (readButtonStateChange()) {
    if (actual_state < max_state) {
      actual_state++;
    } else {
      actual_state = 0;
    }
    switchLight(colors[actual_state], pins[actual_state], actual_state%2==0);
  }
}