diff --git a/api/platform/vl53l1_platform.c b/api/platform/vl53l1_platform.c
index 549265f..84d7f58 100644
--- a/api/platform/vl53l1_platform.c
+++ b/api/platform/vl53l1_platform.c
@@ -114,24 +114,13 @@
 //	return Status;
 // }
 
-// calls the i2c write to multiplexer function
-static int (*i2c_multi_func)(uint8_t address, uint16_t reg) = NULL;
-
-// calls read_i2c_block_data(address, reg, length)
-static int (*i2c_read_func)(uint8_t address, uint16_t reg,
-					uint8_t *list, uint8_t length) = NULL;
-
-// calls write_i2c_block_data(address, reg, list)
-static int (*i2c_write_func)(uint8_t address, uint16_t reg,
-					uint8_t *list, uint8_t length) = NULL;
-
 static pthread_mutex_t i2c_mutex = PTHREAD_MUTEX_INITIALIZER;
 
-void VL53L1_set_i2c(void *multi_func, void *read_func, void *write_func)
+void VL53L1_set_i2c(VL53L1_DEV Dev, void *multi_func, void *read_func, void *write_func)
 {
-	i2c_multi_func = multi_func;
-	i2c_read_func = read_func;
-	i2c_write_func = write_func;
+	Dev->i2c_multi_func = multi_func;
+	Dev->i2c_read_func = read_func;
+	Dev->i2c_write_func = write_func;
 }
 
 static int i2c_write(VL53L1_DEV Dev, uint16_t cmd,
@@ -139,7 +128,7 @@ static int i2c_write(VL53L1_DEV Dev, uint16_t cmd,
 {
     int result = VL53L1_ERROR_NONE;
 
-    if (i2c_write_func != NULL)
+    if (Dev->i2c_write_func != NULL)
     {
         if (Dev->TCA9548A_Device < 8)
         {
@@ -149,7 +138,7 @@ static int i2c_write(VL53L1_DEV Dev, uint16_t cmd,
             pthread_mutex_lock(&i2c_mutex);
 
             // Write to the multiplexer
-            if (i2c_multi_func(Dev->TCA9548A_Address, (1 << Dev->TCA9548A_Device)) < 0)
+            if (Dev->i2c_multi_func(Dev->TCA9548A_Address, (1 << Dev->TCA9548A_Device)) < 0)
             {
                 printf("i2c bus on multiplexer not set.\n");
                 result =  VL53L1_ERROR_CONTROL_INTERFACE;
@@ -158,7 +147,7 @@ static int i2c_write(VL53L1_DEV Dev, uint16_t cmd,
 
         if (result == VL53L1_ERROR_NONE)
         {
-            if (i2c_write_func(Dev->I2cDevAddr, cmd, data, len) < 0)
+            if (Dev->i2c_write_func(Dev->I2cDevAddr, cmd, data, len) < 0)
             {
                 result =  VL53L1_ERROR_CONTROL_INTERFACE;
             }
@@ -183,7 +172,7 @@ static int i2c_read(VL53L1_DEV Dev, uint16_t cmd,
 {
     int result = VL53L1_ERROR_NONE;
 
-    if (i2c_read_func != NULL)
+    if (Dev->i2c_read_func != NULL)
     {
         if (Dev->TCA9548A_Device < 8)
         {
@@ -193,7 +182,7 @@ static int i2c_read(VL53L1_DEV Dev, uint16_t cmd,
             pthread_mutex_lock(&i2c_mutex);
 
             // Write to the multiplexer
-            if (i2c_multi_func(Dev->TCA9548A_Address, (1 << Dev->TCA9548A_Device)) < 0)
+            if (Dev->i2c_multi_func(Dev->TCA9548A_Address, (1 << Dev->TCA9548A_Device)) < 0)
             {
                 printf("i2c bus on multiplexer not set.\n");
                 result =  VL53L1_ERROR_CONTROL_INTERFACE;
@@ -202,7 +191,7 @@ static int i2c_read(VL53L1_DEV Dev, uint16_t cmd,
 
         if (result == VL53L1_ERROR_NONE)
         {
-            if (i2c_read_func(Dev->I2cDevAddr, cmd, data, len) < 0)
+            if (Dev->i2c_read_func(Dev->I2cDevAddr, cmd, data, len) < 0)
             {
                 result =  VL53L1_ERROR_CONTROL_INTERFACE;
             }
diff --git a/api/platform/vl53l1_platform.h b/api/platform/vl53l1_platform.h
index df0754e..5da2c77 100644
--- a/api/platform/vl53l1_platform.h
+++ b/api/platform/vl53l1_platform.h
@@ -76,7 +76,7 @@ extern "C"
 #endif
 
 
-void VL53L1_set_i2c(void *multi_func, void *read_func, void *write_func);
+void VL53L1_set_i2c(VL53L1_DEV pdev, void *multi_func, void *read_func, void *write_func);
 
 VL53L1_Error VL53L1_CommsInitialise(
 	VL53L1_DEV pdev,
diff --git a/api/platform/vl53l1_platform_user_data.h b/api/platform/vl53l1_platform_user_data.h
index 3bcde48..73e5ee3 100644
--- a/api/platform/vl53l1_platform_user_data.h
+++ b/api/platform/vl53l1_platform_user_data.h
@@ -87,6 +87,17 @@ typedef struct {
 	uint8_t   TCA9548A_Device;           /*!< Device number on TCA9548A I2C Multiplexer or 255 if TCA9548A not being used */
     uint8_t   TCA9548A_Address;          /*!< Address of TCA9548A I2C Multiplexer or 255 if TCA9548A not being used */
 
+	// calls the i2c write to multiplexer function
+	int (*i2c_multi_func)(uint8_t address, uint16_t reg);
+
+	// calls read_i2c_block_data(address, reg, length)
+	int (*i2c_read_func)(uint8_t address, uint16_t reg,
+						uint8_t *list, uint8_t length);
+
+	// calls write_i2c_block_data(address, reg, list)
+	int (*i2c_write_func)(uint8_t address, uint16_t reg,
+						uint8_t *list, uint8_t length);
+
 } VL53L1_Dev_t;
 
 typedef VL53L1_Dev_t *VL53L1_DEV;
diff --git a/python/VL53L1X.py b/python/VL53L1X.py
index 52cc250..81b5a91 100644
--- a/python/VL53L1X.py
+++ b/python/VL53L1X.py
@@ -110,7 +110,14 @@ def __init__(self, i2c_bus=1, i2c_address=0x29, tca9548a_num=255, tca9548a_addr=
     def open(self, reset=False):
         self._i2c.open(bus=self._i2c_bus)
         self._configure_i2c_library_functions()
-        self._dev = _TOF_LIBRARY.initialise(self.i2c_address, self._tca9548a_num, self._tca9548a_addr, reset)
+        self._dev = _TOF_LIBRARY.initialise(
+            self.i2c_address,
+            self._tca9548a_num,
+            self._tca9548a_addr,
+            reset,
+            self._i2c_multi_func,
+            self._i2c_read_func,
+            self._i2c_write_func)
 
     def close(self):
         self._i2c.close()
@@ -159,7 +166,7 @@ def _i2c_multi(address, reg):
         self._i2c_multi_func = _I2C_MULTI_FUNC(_i2c_multi)
         self._i2c_read_func = _I2C_READ_FUNC(_i2c_read)
         self._i2c_write_func = _I2C_WRITE_FUNC(_i2c_write)
-        _TOF_LIBRARY.VL53L1_set_i2c(self._i2c_multi_func, self._i2c_read_func, self._i2c_write_func)
+        # _TOF_LIBRARY.VL53L1_set_i2c(self._i2c_multi_func, self._i2c_read_func, self._i2c_write_func)
 
     # The ROI is a square or rectangle defined by two corners: top left and bottom right.
     # Default ROI is 16x16 (indices 0-15). The minimum ROI size is 4x4.
diff --git a/python_lib/vl53l1x_python.c b/python_lib/vl53l1x_python.c
index 7d85d8a..0efb9ee 100644
--- a/python_lib/vl53l1x_python.c
+++ b/python_lib/vl53l1x_python.c
@@ -45,7 +45,7 @@ static VL53L1_RangingMeasurementData_t *pRangingMeasurementData = &RangingMeasur
  *              being used. If not being used, set to 0.
  * @retval  The Dev Object to pass to other library functions.
  *****************************************************************************/
-VL53L1_DEV *initialise(uint8_t i2c_address, uint8_t TCA9548A_Device, uint8_t TCA9548A_Address, uint8_t perform_reset)
+VL53L1_DEV *initialise(uint8_t i2c_address, uint8_t TCA9548A_Device, uint8_t TCA9548A_Address, uint8_t perform_reset, void *multi_func, void *read_func, void *write_func)
 {
     VL53L1_Error Status = VL53L1_ERROR_NONE;
     uint32_t refSpadCount;
@@ -78,6 +78,10 @@ VL53L1_DEV *initialise(uint8_t i2c_address, uint8_t TCA9548A_Device, uint8_t TCA
     dev->TCA9548A_Device = TCA9548A_Device;
     dev->TCA9548A_Address = TCA9548A_Address;
 
+    dev->i2c_multi_func = multi_func;
+    dev->i2c_read_func = read_func;
+    dev->i2c_write_func = write_func;
+
     if(perform_reset){
         Status = VL53L1_software_reset(dev);
         dev->I2cDevAddr = 0x29; // Resetting will reset i2c address back to default
@@ -120,7 +124,7 @@ VL53L1_DEV *initialise(uint8_t i2c_address, uint8_t TCA9548A_Device, uint8_t TCA
     VL53L1_PerformRefSpadManagement(dev);
     VL53L1_SetXTalkCompensationEnable(dev, 0); // Disable crosstalk compensation (bare sensor)
 
-    return dev;
+    return (VL53L1_DEV *)dev;
 }
 
 VL53L1_Error setDeviceAddress(VL53L1_Dev_t *dev, int i2c_address)