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

#include "ArduinoTrace.h"
#include "Square.h"
#include "Circle.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            4 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Square mySquare;
Circle myCircle;

//======================== SETUP ===========================//

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }

  mySquare = Square();
  myCircle = Circle();
  display.clearDisplay();
  display.display();
}

//======================== LOOP ===========================//

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

//======================== TASK1 ===========================//
void Task1()
{
  static int step=1;

  display.drawRect(mySquare.getX(),mySquare.getY(),20,20,WHITE);

  switch (step)
  {
    case 1:
      mySquare.setX(0);
      mySquare.setY(0);
      step=2;
    break;

    case 2:
      mySquare.mRight(); 
      if(mySquare.getX()>=107)
      {
        step=3;
      }
    break;

    case 3:
      mySquare.mDown(); 
      if(mySquare.getY()>=43)
      {
        step=4;
      }
    break;

    case 4:
      mySquare.mLeft();
      if(mySquare.getX()<=0)
      {
        step=5;
      }
    break;

    case 5:
      mySquare.mUp(); 
      if(mySquare.getY()<=0)
      {
        step=2;
      }
    break;
  }
}

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

//======================== TASK1 ===========================//
void Task2()
{
  static int step=1;

  display.drawCircle(myCircle.getX(),myCircle.getY(),10,WHITE);

  switch (step)
  {
    case 1:
      myCircle.setX(117);
      myCircle.setY(53);
      step=2;
    break;

    case 2:
      myCircle.mUp(); 
      if(myCircle.getY()<=11)
      {
        step=3;
      }
    break;

    case 3:
      myCircle.mLeft(); 
      if(myCircle.getX()<=11)
      {
        step=4;
      }
    break;

    case 4:
      myCircle.mDown();
      if(myCircle.getY()>=53)
      {
        step=5;
      }
    break;

    case 5:
      myCircle.mRight(); 
      if(myCircle.getX()>=117)
      {
        step=2;
      }
    break;
  }
}

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