// Reading_2_CD4051_Multiplexer_V1
// https://forum.arduino.cc/t/problems-with-reading-2-cd4051-multiplexer/1329466

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

// PCA9548A multiplexer address
#define PCA9548A_ADDRESS 0x70

// PCA9548A channels
#define CH1 0x01
#define CH2 0x02
#define CH3 0x04
#define CH4 0x08

// OLED display settings
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define OLED_ADDRESS 0x3D

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

const int pin0 = 0, pin1 = 1, pin2 = 2, pin3 = 3, pin4 = 4, pin5 = 5;
const int analogPinA0 = A0, analogPinA1 = A1;

int sortedIndexes[16] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15};

// Function to switch PCA9548A channel
void switchChannel(uint8_t channel) {
  Wire.beginTransmission(PCA9548A_ADDRESS);
  Wire.write(channel);
  Wire.endTransmission();
  delay(10);
}

// Function to initialize the display
void initializeDisplay(uint8_t channel) {
  switchChannel(channel);
  if (!display.begin(OLED_ADDRESS)) {
    Serial.print(F("Failed to initialize display on channel: "));
    Serial.println(channel, HEX);
    return;
  }
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.display();
}

// Function to display values on an OLED
void displayValues(uint8_t channel, float values[], int startIndex) {
  switchChannel(channel);
  display.clearDisplay();

  for (int i = 0; i < 4; i++) {
    int sortedIndex = sortedIndexes[startIndex + i];
    display.setCursor(0, i * 15);
    display.print(F("Ch"));
    display.print(sortedIndex + 1);
    display.print(F(": "));
    display.print(values[sortedIndex], 2);
    display.print(F("A"));

    // Log to Serial Monitor
    Serial.print(F("Display CH"));
    Serial.print(sortedIndex + 1);
    Serial.print(F(": "));
    Serial.print(values[sortedIndex], 2);
    Serial.println(F("A"));
  }

  display.display();
}

// Function to read and assign analog values
void readAnalogValues(float values[]) {
  Serial.println(F("\n--- Reading Group 1 (A0) ---"));
  // Read Group 1 values (A0)
  for (int i = 0; i < 8; i++) {
    digitalWrite(pin2, (i & 0b001) ? HIGH : LOW);
    digitalWrite(pin1, (i & 0b010) ? HIGH : LOW);
    digitalWrite(pin0, (i & 0b100) ? HIGH : LOW);
    delay(50);
    values[i] = analogRead(analogPinA0);
    Serial.print(F("Ch"));
    Serial.print(i + 1);
    Serial.print(F(": "));
    Serial.print(values[i], 2);
    Serial.println(F("A"));
  }

  Serial.println(F("\n--- Reading Group 2 (A1) ---"));
  // Read Group 2 values (A1)
  for (int i = 0; i < 8; i++) {
    digitalWrite(pin5, (i & 0b001) ? HIGH : LOW);
    digitalWrite(pin4, (i & 0b010) ? HIGH : LOW);
    digitalWrite(pin3, (i & 0b100) ? HIGH : LOW);
    delay(50);
    values[8 + i] = analogRead(analogPinA1);;
    Serial.print(F("Ch"));
    Serial.print(i + 9);
    Serial.print(F(": "));
    Serial.print(values[8 + i], 2);
    Serial.println(F("A"));
  }
}

void setup() {
  Wire.begin();
  Serial.begin(115200);

  // Set pin modes
  pinMode(pin0, OUTPUT);
  pinMode(pin1, OUTPUT);
  pinMode(pin2, OUTPUT);
  pinMode(pin3, OUTPUT);
  pinMode(pin4, OUTPUT);
  pinMode(pin5, OUTPUT);
  pinMode(analogPinA0, INPUT);
  pinMode(analogPinA1, INPUT);

  // Initialize displays
  initializeDisplay(CH1);
  initializeDisplay(CH2);
  initializeDisplay(CH3);
  initializeDisplay(CH4);
}

void loop() {
  float values[16];

  // Read analog values from both groups
  readAnalogValues(values);

  Serial.println(F("\n--- Sorted Values Displayed ---"));
  // Display values on the OLED screens
  displayValues(CH1, values, 0);  // Display CH1-CH4 on Display 1
  displayValues(CH2, values, 4);  // Display CH5-CH8 on Display 2
  displayValues(CH3, values, 8);  // Display CH9-CH12 on Display 3
  displayValues(CH4, values, 12); // Display CH13-CH16 on Display 4

  delay(1000);
}