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

#define DEFAULT_OLED_ADDR 0x3C

// Define control pins for the multiplexers (one for SDA and the other one for SCL)
int S10 = 3;
int S11 = 4;
int S12 = 5;
int S13 = 6;

int S20 = 8;
int S21 = 9;
int S22 = 10;
int S23 = 11;


#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
#define OLED_RESET     -1

// Create display objects
Adafruit_SSD1306 display1(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
Adafruit_SSD1306 display2(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

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

  pinMode(S10, OUTPUT);
  pinMode(S11, OUTPUT);
  pinMode(S12, OUTPUT);
  pinMode(S13, OUTPUT);
  pinMode(S20, OUTPUT);
  pinMode(S21, OUTPUT);
  pinMode(S22, OUTPUT);
  pinMode(S23, OUTPUT);
  resetPins();

  initDisplay(display1, 1);
  showMessage(display1, 1, "Display 1!");
  initDisplay(display2, 2);
  showMessage(display2, 2, "Display 2!");

}

void loop() {

}

void resetPins(){
  digitalWrite(S10, LOW);
  digitalWrite(S11, LOW);
  digitalWrite(S12, LOW);
  digitalWrite(S13, LOW);
  digitalWrite(S20, LOW);
  digitalWrite(S21, LOW);
  digitalWrite(S22, LOW);
  digitalWrite(S23, LOW);

}


void initDisplay(Adafruit_SSD1306 &display, int id) {

  if(id == 1){
    digitalWrite(S10, HIGH);
    digitalWrite(S20, HIGH);
  }else{
    digitalWrite(S11, HIGH);
    digitalWrite(S21, HIGH);
  }

  delay(2000); 
  
  if(!display.begin(SSD1306_SWITCHCAPVCC, DEFAULT_OLED_ADDR)) {
    Serial.println(F("SSD1306 allocation failed\n"));
    for(;;);
  }
  
  display.clearDisplay();
  display.display();
  Serial.print("initalization successful\n");
  resetPins();
}

void showMessage(Adafruit_SSD1306 &display, int id, const char *message) {
   if(id == 1){
    digitalWrite(S10, HIGH);
    digitalWrite(S20, HIGH);
  }else{
    digitalWrite(S11, HIGH);
    digitalWrite(S21, HIGH);
  }
  delay(2000); 

  display.clearDisplay();

  display.setTextSize(1);      
  display.setTextColor(SSD1306_WHITE);  
  display.setCursor(0, 0);     
  display.print(message);
  display.display();
  Serial.print("Meesage display successful\n");
  resetPins();
}