How to implement TCA9548A multiplexer in C++

Question

Currently, my team of college students and I are building an autonomous robot that requires 4 distance sensors (Adafruit VL6180x) and a gyroscope (BNO055). We are using a Teensy 4.1 for our mainboard which only has 2 I2C connectors. This is not enough for our 5 total sensors. Therefore, we are connecting the Teensy 4.1 to an I2C multiplexer (TCA9548A). This multiplexer is then connected to all of our sensors (4 distance sensors and 1 gyroscope). We are also using serial to communicate to the Teensy so that when it receives a command, the robot moves.

We tried bypassing the multiplexer by reading a sensor direction, we used another of the same multiplexer, we probe the multiplexer hardware and check the I2C hardware (which showed it works), and we also wrote a test program in circuitPython framework and it works. At this point, we do not know what to do. There are no errors, the function call to read the value never returns.

The scancode works properly. The problem is in the line:

int range = tof1.readRangeStatus();

#include <Adafruit_Sensor.h>
#include <Adafruit_BNO055.h>
#include <Wire.h>
#include <SparkFun_I2C_Mux_Arduino_Library.h>
#include <Adafruit_VL6180X.h>

// create IMU object
Adafruit_BNO055 imu1 = Adafruit_BNO055();

// create ToF objects
Adafruit_VL6180X tof1 = Adafruit_VL6180X();
Adafruit_VL6180X tof2 = Adafruit_VL6180X();
Adafruit_VL6180X tof3 = Adafruit_VL6180X();
Adafruit_VL6180X tof4 = Adafruit_VL6180X();

namespace mux_h
{
    void tcaselect(uint8_t i) {
        if (i > 7) return;

        Wire1.beginTransmission(TCAADDR);
        Wire1.write(1 << i);
        Wire1.endTransmission();  
    }
}



void setup()
{
    // Setup I2C
    Wire1.begin();

    // Setup MUX
    mux_h::tcaselect(0); tof1.begin();
    mux_h::tcaselect(1); tof2.begin();
    mux_h::tcaselect(2); tof3.begin();
    mux_h::tcaselect(3); tof4.begin();
    
    ...

    for (uint8_t t=0; t<8; t++) {
      mux_h::tcaselect(t);
      Serial.print("TCA Port #"); Serial.println(t);

      for (uint8_t addr = 0; addr<=127; addr++) {
        //if (addr == TCAADDR) continue;

        Wire1.beginTransmission(addr);
        if (!Wire1.endTransmission()) {
          Serial.print("Found I2C 0x");  Serial.println(addr,HEX);
        }
      }
    }

    tof1.begin();
    tof2.begin();
    tof3.begin();
    tof4.begin();

    Serial.write("Teensy OK\n");
}
}

void loop()
{
    ...

    else if(strcmp(command_buffer, "T1") == 0)
    {
        Serial.write("T1 ToF Sensor Test\n");
        mux_h::tcaselect(1);
        int range = tof1.readRangeStatus();
        Serial.println(range);
        {
            Serial.write("T1 OK: ");
            mux_h::tcaselect(0);
            int result = tof1.readRange();
            Serial.print(result);
            Serial.write("\n");
        }
        mux_h::tcaselect(1);
        if(tof2.readRangeStatus() == 0)
        {
            Serial.write("T2 OK: ");
            mux_h::tcaselect(1);
            int result = tof2.readRange();
            Serial.print(result);
            Serial.write("\n");
        }
    }
    
}