//----- Oled display --------------------------------------------------------------//
#include <SPI.h>
#include <Wire.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

// 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);

//----- Include for debugging ----------------//
#include "ArduinoTrace.h"

//---- Task Scheduler ------------------------//
#include <TaskScheduler.h>
Scheduler ts;

//---- Tasks definitions----------------------//
void task1CB();
Task tTask1(100,TASK_FOREVER,&task1CB,&ts);

void task2CB();
Task tTask2(100,TASK_FOREVER,&task2CB,&ts);

//===== Tasks CallBacks ======================//
void task1CB()          //period 100 ms
{
  Task1();         
}

void task2CB()          //period 100 ms
{
  Task2();        
}

//---- Control Signals ------------------------//

bool Q0,Q1,Q2;

//================= SETUP ====================//
void setup() {
  // put your setup code here, to run once:

  //---- Begin oled display ------------------------//
  Serial.begin(115200);
  // initialize and clear display
  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;);
  }

  //--- Display initial setting and clear ---//
  display.setTextSize (2);
  display.setTextColor (WHITE);
  display.clearDisplay();
  display.display();
  
  //--- start message ---------//
  Serial.println("INICIO: ");
  
  //--- Enabling Tasks ------//
  tTask1.enable();
  tTask2.enable();
  tTask2.delay(50);
}

//================= LOOP ====================//
void loop() {
  // put your main code here, to run repeatedly:
  ts.execute();
}

//======================= TASKS =====================

//---------------- TASK1 ----------------------------
void Task1()     //period=100ms ; initial delay=0ms
{
  //--- display handle ----//
  oledDisplay(); 
}

//---------------- TASK2 ----------------------------
void Task2()     //period=100ms ; initial delay=50ms
{
  //--- sequential functions ----//
  sf1();
  sf2();
  sf3();
}

//====== OLED DISPLAY =================//
void oledDisplay()
{
  display.clearDisplay();
  led1Display();
  led2Display();
  led3Display();
  display.display(); 
}

//--------- LED1 DISPLAY ---------------//
void led1Display()
{
  display.drawCircle(15,15,15,WHITE);
  if(Q0==1)
  {
    display.fillCircle(15,15,10,WHITE);
  }
  display.setCursor(6,40);
  display.print("L1");
}

//--------- LED2 DISPLAY ---------------//
void led2Display()
{
  display.drawCircle(63,15,15,WHITE);
  if(Q1==1)
  {
    display.fillCircle(63,15,10,WHITE);
  }
  display.setCursor(53,40);
  display.print("L2");
}

//--------- LED3 DISPLAY ---------------//
void led3Display()
{
  display.drawCircle(110,15,15,WHITE);
  if(Q2==1)
  {
    display.fillCircle(110,15,10,WHITE);
  }
  display.setCursor(100,40);
  display.print("L3");
}

//========= SEQUENTIAL FUNCTIONS =======//

//---------- SF1 ---------------//
void sf1()
{
  //control signal Q0: Period=(30+70)*100ms = 1s; Ton=30*100ms = 3s
  static byte step=1;
  static byte timer=5;
  switch(step)
  {
    case 1:
      if(timer==0)
      {
        Q0=1;
        step=2;
        timer=20;
      }
    break;
    case 2:      
      if(timer==0)
      {
        Q0=0;
        step=1;
        timer=80;
      }
    break;
  }
  if(timer>0) timer--;
}

//---------- SF2 ---------------//
void sf2()
{
  //control signal Q1: Period=(50+50)*100ms = 1s; Ton=50*100ms = 5s
  //ToDo:
  static byte astep=1;
  static byte atimer=5;
  switch(astep)
  {
    case 1:
      if(atimer==0)
      {
        Q1=1;
        astep=2;
        atimer=30;
      }
    break;
    case 2:      
      if(atimer==0)
      {
        Q1=0;
        astep=1;
        atimer=70;
      }
    break;
  }
  if(atimer>0) atimer--;
}

//---------- SF3 ---------------//
void sf3()
{
  //control signal Q2: Period=(80+20)*100ms = 1s; Ton=80*100ms = 8s
  //ToDo:
  static byte bstep=1;
  static byte btimer=5;
  switch(bstep)
  {
    case 1:
      if(btimer==0)
      {
        Q2=0;
        bstep=2;
        btimer=30;
      }
    break;
    case 2:      
      if(btimer==0)
      {
        Q2=1;
        bstep=1;
        btimer=70;
      }
    break;
  }
  if(btimer>0) btimer--;
}

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