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

#define SCREEN_WIDTH        128 // OLED display width, in pixels
#define SCREEN_HEIGHT        64 // OLED display height, in pixels
bool R,G,Y;

// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET            -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

void setup() {
  Serial.begin(9600);
  // initialize and clear display
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;);
  }

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(62,34);
  //display GREEN LED:
  display.print("Y");
  display.drawCircle(64, 16, 10, WHITE);

  display.setCursor(25,34);
  //display GREEN LED:
  display.print("G");
  display.drawCircle(27, 16, 10, WHITE);

  display.setTextColor(WHITE);
  display.setCursor(99,34);
  //display GREEN LED:
  display.print("R");
  display.drawCircle(101, 16, 10, WHITE);   
  // update display with all of the above graphics
  display.display();
  //BREAK();
}

void loop() {
  static unsigned long pm=0;

  if((millis()-pm)>=100)
  {
    pm=millis();
    Task1();
  }
}

void Task1()
{
  static int step=1;
  static int counter=0;

  switch (step)
  {
    case 1:          //INIT
      G=1;
      display.fillCircle(64, 16, 7, BLACK);
      display.fillCircle(27, 16, 7, WHITE);
      display.display();
      
      counter=50;
      step=2;
    break;

    case 2:          //G_ON
      counter--;
      if(counter==0)
      {
        G=0;
        Y=1;
        display.fillCircle(27, 16, 7, BLACK);
        display.fillCircle(64, 16, 7, WHITE);
        display.display();

        counter=20;
        step=3;
      }
    break;

    case 3:          //Y_ON
      counter--;
      if(counter==0)
      {
        Y=0;
        R=1;
        display.fillCircle(64, 16, 7, BLACK);
        display.fillCircle(101, 16, 7, WHITE);
        display.display();

        counter=40;
        step=4;
      }
    break;

    case 4:          //R_ON
      counter--;
      if(counter==0)
      {
        Y=1;
        display.fillCircle(64, 16, 7, WHITE);
        display.display();

        counter=20;
        step=5;
      }
    break;

    case 5:          //R_ON Y_ON
      counter--;
      if(counter==0)
      {
        Y=0;
        R=0;
        G=1;
        display.fillCircle(64, 16, 7, BLACK);
        display.fillCircle(101, 16, 7, BLACK);
        display.fillCircle(27, 16, 7, WHITE);
        display.display();

        counter=50;
        step=2;
      }
    break;
  }
}