#include <Wire.h>

// Define the addresses of the CD74HC4067
#define CD74HC4067_1_ADDRESS 0x48
#define CD74HC4067_2_ADDRESS 0x49

// Define the pins on ESP32 connected to CD74HC4067
#define S0_PIN 32   // 18
#define S1_PIN 33   //  5
#define S2_PIN 25   //  2
#define S3_PIN 26   // 15
#define SIG_PIN 4  //  //35
#define EN_MUX1_PIN 12 // 19
#define EN_MUX2_PIN 14 // 21

int currentMux = 0;
int currentChannel = 0;

// Function to select a channel on CD74HC4067
void selectChannel(int mux, int channel) {
  if (mux == 0) {
    digitalWrite(EN_MUX1_PIN, HIGH);
    digitalWrite(EN_MUX2_PIN, LOW);
  } else {
    digitalWrite(EN_MUX1_PIN, LOW);
    digitalWrite(EN_MUX2_PIN, HIGH);
  }

  digitalWrite(S0_PIN, channel & 0x01);
  digitalWrite(S1_PIN, (channel >> 1) & 0x01);
  digitalWrite(S2_PIN, (channel >> 2) & 0x01);
  digitalWrite(S3_PIN, (channel >> 3) & 0x01);
}

void setup() {
  Wire.begin();

  // Set the pins as outputs
  pinMode(S0_PIN, OUTPUT);
  pinMode(S1_PIN, OUTPUT);
  pinMode(S2_PIN, OUTPUT);
  pinMode(S3_PIN, OUTPUT);
  pinMode(EN_MUX1_PIN, OUTPUT);
  pinMode(EN_MUX2_PIN, OUTPUT);

  // Set SIG_PIN as input
  pinMode(SIG_PIN, INPUT);

  // Set all channels as outputs for both MUX
  for (int i = 0; i < 16; i++) {
    selectChannel(0, i);
    pinMode(SIG_PIN, OUTPUT);
    digitalWrite(SIG_PIN, LOW);

    selectChannel(1, i);
    pinMode(SIG_PIN, OUTPUT);
    digitalWrite(SIG_PIN, LOW);
  }
}

void loop() {
  // Turn off the previous channel
  selectChannel(currentMux, currentChannel);
  digitalWrite(SIG_PIN, LOW);
  delay(10);  // Allow some time for the signal to stabilize

  // Increment to the next channel
  currentChannel = (currentChannel + 1) % 16;

  // If all channels in the current MUX have been processed, switch to the next MUX
  if (currentChannel == 0) {
    currentMux = (currentMux + 1) % 2;
  }

  // Turn on the current channel
  selectChannel(currentMux, currentChannel);
  digitalWrite(SIG_PIN, HIGH);

  delay(500);  // Delay for half a second
}