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

#define SCREEN_WIDTH 128 
#define SCREEN_HEIGHT 64 
#define OLED_RESET -1

int posX = 0;

bool R,G,Y;

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(;;);
  }

  // update display with all of the above graphics
  display.clearDisplay();
  display.display();
}

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

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

void Task1()
{
  static int step=1;

  switch (step)
  {
    case 1:  
      DUMP("PASAO");
      display.drawRect(posX, 22, 20, 20, WHITE);
      step=2;
    break;

    case 2:
      posX = (posX + 3);
      display.drawRect(posX, 22, 20, 20, WHITE);
      if(posX > 128)
      {  
        display.drawRect(posX, 22, 20, 20, WHITE);
        step=3;
      }
    break;

    case 3:
      posX = (posX - 3);
      display.drawRect(posX, 22, 20, 20, WHITE);
      if(posX < 0)
      {
        display.drawRect(posX, 22, 20, 20, WHITE);
        step=1;
      }
    break;
  }

  DUMP(step);
  DUMP(posX);
}

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