/* --- CONFIGURATION (Change these to match your PCB) --- */
const int S0_PIN = 16;
const int S1_PIN = 17;
const int S2_PIN = 18;

// The 'COM' or 'SIG' pin on the MUX connected to RP2040
const int MUX_IN_PIN = 26; 

// The channel your IR sensor is plugged into (0-7)
const int SENSOR_CHANNEL = 2;

void setup() {
  Serial.begin(115200);
  
  // Set MUX selection pins as outputs
  pinMode(S0_PIN, OUTPUT);
  pinMode(S1_PIN, OUTPUT);
  pinMode(S2_PIN, OUTPUT);
  
  // Set MUX signal pin as input
  pinMode(MUX_IN_PIN, INPUT);
  
  Serial.println("--- IR Sensor MUX Test Starting ---");
}

void selectMuxChannel(int channel) {
  // Write the binary address to the selection pins
  digitalWrite(S0_PIN, channel & 0x01);
  digitalWrite(S1_PIN, (channel >> 1) & 0x01);
  digitalWrite(S2_PIN, (channel >> 2) & 0x01);
}

void loop() {
  // 1. Select the correct MUX channel
  selectMuxChannel(SENSOR_CHANNEL);
  
  // 2. Small delay for the signal to stabilize
  delayMicroseconds(50);
  
  // 3. Read the sensor (Digital signal)
  int sensorState = digitalRead(MUX_IN_PIN);
  
  // IR modules usually output LOW (0) when an obstacle is detected
  if (sensorState == LOW) {
    Serial.print("Channel ");
    Serial.print(SENSOR_CHANNEL);
    Serial.println(": OBSTACLE DETECTED!");
  } else {
    Serial.print("Channel ");
    Serial.print(SENSOR_CHANNEL);
    Serial.println(": Clear");
  }
  
  delay(200); // Wait 200ms before next read
}