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

#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)
#define OLED_RESET            -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
bool R,G,Y;
int PR = 8,PY=9,PG = 10;

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

  display.clearDisplay();
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(23,30);
  display.print("G");
  display.setCursor(59,30);
  display.print("Y");
  display.setCursor(97,30);
  display.print("R");
  display.drawCircle(27, 16, 10, WHITE);  //G
  display.drawCircle(64, 16, 10, WHITE);  //Y
  display.drawCircle(101, 16, 10, WHITE); //R         
  // update display with all of the above graphics
  display.display();
}

void loop() {
  // put your main code here, to run repeatedly:
  static unsigned long pm=0;
  if((millis()-pm)>=100)
  {
    pm=millis();
    Task1();
    display.display();
  }
}

//======================== TASK1 ===========================//

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

  //LED Stoplight
  digitalWrite(PR,R);
  digitalWrite(PY,Y);
  digitalWrite(PG,G);
  //OLED Stoplight
  display.fillCircle(27, 16, 7, G);  //G
  display.fillCircle(64, 16, 7, Y); //Y
  display.fillCircle(101, 16, 7, R); //R  

  switch (step)
  {
    case 1:          //INIT
      DUMP(step);
      DUMP(millis());
      G=1;
      counter=50;   //5 seconds
      step=2;
    break;

    case 2:          //G_ON
      counter--;
      if(counter==0)
      {
        DUMP(step);
        DUMP(millis());
        G=0;
        Y=1;
        counter=20;
        step=3;
      }
    break;

    case 3:          //Y_ON
      counter--;
      if(counter==0)
      {
        DUMP(step);
        DUMP(millis());
        Y=0;
        R=1;
        counter=40;
        step=4;
      }
    break;

    case 4:          //R_ON
      counter--;
      if(counter==0)
      {
        DUMP(step);
        DUMP(millis());
        Y=1;
        counter=20;
        step=5;
      }
    break;

    case 5:          //R_ON Y_ON
      counter--;
      if(counter==0)
      {
        DUMP(step);
        DUMP(millis());
        Y=0;
        R=0;
        G=1;
        counter=50;
        step=2;
      }
    break;
  }
}

//========================================================//