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

/*
* Connections
* SSD1306 OLED | Arduino Uno
* ---------------------------
*     VCC      |    +5V       (Vcc/power/+ve)
*     GND      |    GND       (Ground/-ve/0v)
*     SCL      |    A5        (Serial Clock Line)
*     SDA      |    A4        (Serial Data Line)
*/

const int SCREEN_WIDTH = 128; // OLED display width, in pixels
const int SCREEN_HEIGHT = 64; // OLED display height, in pixels


// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);


void displayHelloWorld(int offSet) {
  Wire.begin(10,9);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0 + offSet, 10);
  // Display static text
  display.println("Hello, World!");
  display.display();
  Wire.end();
}


pt ptDisplay;
int displayThread(struct pt* pt) {
  PT_BEGIN(pt);
  int avance =0;
  Serial.println("entra thread ini");
  // Loop forever
  for(;;) {
    Wire.end();
    Wire.begin(10,9);
    PT_SLEEP(pt, 100);
    Serial.println("entra thread 1");
    displayHelloWorld(avance);
    
    PT_SLEEP(pt, 200);
    avance++;
    if (avance>=128)
    {avance=0;
    PT_YIELD(pt);
    }
    
  }

  PT_END(pt);
}



pt ptDisplay2;
int displayThread2(struct pt* pt) {
  PT_BEGIN(pt);
  int avance =0;
  
  Serial.println("entra thread ini");
  // Loop forever
  for(;;) {
    Wire.end();
    Wire.begin(13,14);
    PT_SLEEP(pt, 100);
    Serial.println("entra thread 2");
    displayGoodbye(avance);
    
    PT_SLEEP(pt, 200);
    avance++;
    if (avance>=128)
    {avance=0;
    PT_YIELD(pt);
    }
    
  }

  PT_END(pt);
}


void displayGoodbye(int offSet) {
  Wire.begin(13,14);
  display2.clearDisplay();
  display2.setTextSize(1);
  display2.setTextColor(WHITE);
  display2.setCursor(0 + offSet, 10);
  // Display static text
  display2.println("Goodbye!");
  display2.display();
  Wire.end();
}

void setup() {
  
  Serial.begin(9600);

  Wire.begin(13,14);
  if(!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    while(true);
  }
  Serial.println("display 1 starts ok");
  //displayHelloWorld();
  //Wire.end();


  Wire.begin(10,9);
  if(!display2.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3D for 128x64
    Serial.println(F("SSD1306 allocation failed"));
    while(true);
  }
  Serial.println("display 2 starts ok");
  //displayHelloWorld();
  //Wire.end();

  PT_INIT(&ptDisplay);
  PT_INIT(&ptDisplay2);

  
}

void loop() {
  PT_SCHEDULE(displayThread(&ptDisplay));
  PT_SCHEDULE(displayThread2(&ptDisplay2));
 /*
  display.clearDisplay();

  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 10);
  // Display static text
  display.println("Hello, World!");
  display.display();
  */
}