diff --git a/.vscode/settings.json b/.vscode/settings.json
index 5481a39..c9cf880 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,6 +1,7 @@
 {
     "files.associations": {
         "vl53l1x_calibration.h": "c",
-        "vl53l1x_api.h": "c"
+        "vl53l1x_api.h": "c",
+        "i2c.h": "c"
     }
 }
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8e89e8e..929da7d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -12,11 +12,14 @@ pico_sdk_init()
 
 add_executable(Detection2023
   main.c
+  i2c_slave.c
   vl53l1_platform.c
   VL53L1X_api.c
   VL53L1X_calibration.c
+  VL53L1X_Fonctions.c
   ws2812.c
   SelectionCapteur.c
+  Tests.c
 )
 # generate the header file into the source tree as it is included in the RP2040 datasheet
 pico_generate_pio_header(Detection2023 ${CMAKE_CURRENT_LIST_DIR}/ws2812.pio)
diff --git a/Tests.c b/Tests.c
new file mode 100644
index 0000000..079d3fc
--- /dev/null
+++ b/Tests.c
@@ -0,0 +1,74 @@
+#include "pico/stdlib.h"
+#include <stdio.h>
+#include "ws2812.h"
+#include "VL53L1X_Fonctions.h"
+
+void display_menu();
+
+#define TEST_TIMEOUT_US 10000000
+
+void Tests(void){
+    int answer_at_least_once=0;
+    uint8_t VL53L1X_device = 0x29;
+    while(1){
+        int keycode;
+        display_menu();
+        
+        do{
+            keycode = getchar_timeout_us(TEST_TIMEOUT_US);
+            if(!answer_at_least_once){
+                display_menu();
+            }
+        }while(keycode == PICO_ERROR_TIMEOUT || keycode == 0);
+        answer_at_least_once = 1;
+
+        switch (keycode)
+        {
+        case 'a':
+        case 'A':
+            printf("Changement d'adresse\n");
+            change_address(&VL53L1X_device, VL53L1X_device + 3);
+            printf("New address: %d\n", VL53L1X_device);
+            break;
+        case 'i':
+        case 'I':
+            printf("Initialisation des capteurs\n");
+            initialise_adresses();
+            break;
+        case 'j':
+        case 'J':
+            while(continuous_multiple_reading());
+            break;
+        case 'k':
+        case 'K':
+            ws2812_arc_en_ciel();
+            break;
+        case 'l':
+        case 'L':
+            while(continuous_reading(0x31));
+            break;
+        case 'o':
+        case 'O':
+            while(calibration(VL53L1X_device));
+            break;
+        case 'r':
+        case 'R':
+            while(continuous_reading(VL53L1X_device));
+            break;
+        default :
+            break;
+
+        }
+    }
+}
+
+void display_menu(){
+    printf("Select action :\n");
+    printf("A - Change I2C address\n");
+    printf("I - Init I2C address\n");
+    printf("J - Lecture distance multiple\n");
+    printf("K - Arc en ciel\n");
+    printf("L - Lecture distance capteur 1\n");
+    printf("O - Offset Calibration\n");
+    printf("R - Read distance\n");
+}
\ No newline at end of file
diff --git a/Tests.h b/Tests.h
new file mode 100644
index 0000000..7eaa4fc
--- /dev/null
+++ b/Tests.h
@@ -0,0 +1 @@
+void Tests(void);
\ No newline at end of file
diff --git a/VL53L1X_Fonctions.c b/VL53L1X_Fonctions.c
new file mode 100644
index 0000000..1757b7f
--- /dev/null
+++ b/VL53L1X_Fonctions.c
@@ -0,0 +1,249 @@
+#include "pico/stdlib.h"
+#include <stdio.h>
+#include "VL53L1X_api.h"
+#include "VL53L1X_Fonctions.h"
+#include "SelectionCapteur.h"
+#include "ws2812.h"
+
+#define DISTANCE_TRES_LOIN_CM 120
+#define DISTANCE_LOIN_CM 80
+#define DISTANCE_PROCHE_CM 14
+
+// Stock les valeurs lues des capteurs
+uint8_t distance_capteur_cm[12];
+
+uint8_t statu_capteurs[13];
+
+void initialise_adresses(void){
+    const uint8_t tmp_i2c_adresse = 0x28;
+    const uint8_t default_i2c_adresse = 0x29;
+    uint8_t adresse = default_i2c_adresse;
+
+    
+
+    // On change l'adresse de tous les capteurs
+    Selection_capteur_deselect();
+    change_address(&adresse, tmp_i2c_adresse);
+
+    // Pour chaque capteur
+    for(uint capteur=1; capteur<=12; capteur++){
+        // reset du capteur
+        Selection_capteur_select(capteur);
+        sleep_ms(1);
+        Selection_capteur_deselect();
+        sleep_ms(1);
+        uint8_t VL53L1X_device = 0x29;
+
+        if(change_address(&VL53L1X_device, 0x30 + capteur)){
+            printf("Erreur change adresse : %x => %x, capteur : %d\n", VL53L1X_device, 0x30 + capteur, capteur);
+            ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
+            statu_capteurs[capteur]=0;
+        }else{
+            if(VL53L1X_SensorInit(VL53L1X_device)){
+                // bad init
+                ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
+                statu_capteurs[capteur]=0;
+            }else{
+                // good init
+                statu_capteurs[capteur]=1;
+                int status;
+                status = VL53L1X_SetDistanceMode (VL53L1X_device, 1); // Short mode
+                status |= VL53L1X_SetInterMeasurementInMs(VL53L1X_device, 200); 
+                status |= VL53L1X_SetTimingBudgetInMs(VL53L1X_device, 200);
+                if(status){
+                    printf("Custom config KO, error %d\n", status);
+                    ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
+                }else{
+                    printf("Custom config OK\n");
+                }
+
+                status=VL53L1X_StartRanging(VL53L1X_device);
+                if(!status){
+                    ws2812_set_buffer_rgb(0, 0x4, 0, capteur-1);
+                }else{
+                    ws2812_set_buffer_rgb(0x2, 0x2, 0, capteur-1);
+                }
+
+                
+            }
+        }
+    }
+    ws2812_affiche_buffer();
+
+    ws2812_affiche_buffer();
+
+}
+
+int change_address(uint8_t *device, uint8_t new_i2c_7bits_address){
+    int status;
+    status = VL53L1X_SetI2CAddress(*device, new_i2c_7bits_address << 1);
+    if(status){
+        //printf("VL53L1X_SetI2CAddress, Error :%d\n", status);
+    }else{
+        *device=new_i2c_7bits_address;
+    }
+    return status;
+}
+
+int continuous_multiple_reading(){
+    for(uint8_t device=0x31; device<0x31+12; device++){
+        if(statu_capteurs[device-0x31+1]==0){
+            continue;
+        }
+        int status;
+        uint8_t data_ready = 0;
+        uint16_t distance_mm;
+        while(!data_ready){
+            status=VL53L1X_CheckForDataReady(device, &data_ready);
+            if(status){
+                printf("CheckForDataReady KO, error %d, capteur:%x\n", status, device);
+            }
+        }
+        
+        status=VL53L1X_GetDistance(device, &distance_mm);
+        if(status){
+            printf("GetDistance KO, error %d, capteur:%x\n", status, device);
+            return 0;
+        }else{
+            printf(">distance%x:%d\n", device, distance_mm);
+            distance_capteur_cm[device-0x31] = distance_mm / 10;
+        }
+        
+        status=VL53L1X_ClearInterrupt(device);
+        if(status){
+            printf("ClearInterrupt KO, error %d, capteur:%x\n", status, device);
+            return 0;
+        }
+
+        int lettre = getchar_timeout_us(0);
+        if(lettre != PICO_ERROR_TIMEOUT && lettre != 0){
+            //return 0;
+        }
+    }
+    affiche_distance_sur_led();
+    return 1;
+}
+
+void affiche_distance_sur_led(){
+    uint8_t distance_cm;
+    uint32_t couleur;
+    for(uint8_t capteur=0; capteur<12; capteur++){
+        distance_cm = distance_capteur_cm[capteur];
+        if(distance_cm == 0 ||distance_cm > DISTANCE_TRES_LOIN_CM){
+            ws2812_set_buffer_rgb(COULEUR_TRES_LOIN, capteur);
+        }else if(distance_cm > DISTANCE_LOIN_CM){
+            ws2812_set_buffer_rgb(COULEUR_LOIN, capteur);
+        }else if(distance_cm > DISTANCE_PROCHE_CM){
+            ws2812_set_buffer_rgb(COULEUR_PROCHE, capteur);
+        }else{
+            ws2812_set_buffer_rgb(COULEUR_TROP_PROCHE, capteur);
+        }
+        
+    }
+    ws2812_affiche_buffer();
+}
+
+int calibration(uint8_t device){
+    uint16_t offset;
+    int status;
+    uint8_t boot_state=0;
+    printf("Calibration...\n");
+    while(!boot_state){
+        VL53L1X_BootState(device, &boot_state);
+    }
+    printf("Sensor boot ok\n");
+
+    status=VL53L1X_SensorInit(device);
+    if(status){
+        printf("Sensor Init KO, error %d\n", status);
+    }else{
+        printf("Sensor Init OK\n");
+    }
+
+    status = VL53L1X_CalibrateOffset(device, 140, &offset);
+    if(status != 0){
+        printf("Error while calibrating : %d\n",status);
+    }else{
+        printf("Offset : %d\n", offset);
+
+    }
+    return 0;
+}
+
+int continuous_reading(uint8_t device){
+    int status;
+    uint8_t data_ready, boot_state=0;
+    uint16_t distance;
+
+    printf("Reading distance...\nSend any character to quit.");
+
+    while(!boot_state){
+        VL53L1X_BootState(device, &boot_state);
+    }
+    printf("Sensor boot ok\n");
+
+    status=VL53L1X_SensorInit(device);
+    if(status){
+        printf("Sensor Init KO, error %d\n", status);
+        return 0;
+    }else{
+        printf("Sensor Init OK\n");
+    }
+
+
+    // Custom configuration
+    status = VL53L1X_SetDistanceMode (device, 1); // Short mode
+    status |= VL53L1X_SetInterMeasurementInMs(device, 200); 
+    status |= VL53L1X_SetTimingBudgetInMs(device, 200);
+    if(status){
+        printf("Custom config KO, error %d\n", status);
+        return 0;
+    }else{
+        printf("Custom config OK\n");
+    }
+
+    status=VL53L1X_StartRanging(device);
+    if(status){
+        printf("Start ranging KO, error %d\n", status);
+        return 0;
+    }else{
+        printf("Start ranging OK\n");
+    }
+
+    while(1){
+    // Reading data
+        data_ready = 0;
+        while(!data_ready){
+            status=VL53L1X_CheckForDataReady(device, &data_ready);
+            if(status){
+                printf("CheckForDataReady KO, error %d\n", status);
+                return 0;
+            }else{
+                //printf("CheckForDataReady OK\n");
+            }
+        }
+        
+        status=VL53L1X_GetDistance(device, &distance);
+        if(status){
+            printf("GetDistance KO, error %d\n", status);
+            return 0;
+        }else{
+            //printf("GetDistance OK, distance %u mm\n", distance);
+            printf(">distance:%d\n", distance);
+        }
+        
+        status=VL53L1X_ClearInterrupt(device);
+        if(status){
+            printf("ClearInterrupt KO, error %d\n", status);
+            return 0;
+        }else{
+            //printf("ClearInterrupt OK\n");
+        }
+
+        int lettre = getchar_timeout_us(0);
+        if(lettre != PICO_ERROR_TIMEOUT && lettre != 0){
+            return 0;
+        }
+    }
+    return 0;
+}
\ No newline at end of file
diff --git a/VL53L1X_Fonctions.h b/VL53L1X_Fonctions.h
new file mode 100644
index 0000000..2de0b56
--- /dev/null
+++ b/VL53L1X_Fonctions.h
@@ -0,0 +1,21 @@
+#include "pico/stdlib.h"
+
+// Couleur quand la distance est trop loin pour s'en servir avec fiabilité
+#define COULEUR_TRES_LOIN 0x00, 0x00, 0x01
+// Couleur quand la distance fiable mais loin
+#define COULEUR_LOIN 0x00, 0x01, 0x00
+// Couleur quand la distance fiable et que le robot doit ralentir
+#define COULEUR_PROCHE 0x01, 0x01, 0x00
+// Couleur trop proche : rien ne devrait être si près
+#define COULEUR_TROP_PROCHE 0x01, 0x00, 0x01
+
+
+int continuous_multiple_reading(void);
+void affiche_distance_sur_led(void);
+void initialise_adresses(void);
+int change_address(uint8_t *device, uint8_t new_i2c_7bits_address);
+int calibration(uint8_t device);
+int continuous_reading(uint8_t device);
+extern uint8_t distance_capteur_cm[];
+
+extern uint8_t statu_capteurs[];
diff --git a/i2c_fifo.h b/i2c_fifo.h
new file mode 100644
index 0000000..805c00e
--- /dev/null
+++ b/i2c_fifo.h
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#ifndef _I2C_FIFO_H_
+#define _I2C_FIFO_H_
+
+#include <hardware/i2c.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file i2c_fifo.h
+ *
+ * \brief I2C non-blocking r/w.
+ */
+
+/**
+ * \brief Pop a byte from I2C Rx FIFO.
+ * 
+ * This function is non-blocking and assumes the Rx FIFO isn't empty.
+ * 
+ * \param i2c I2C instance.
+ * \return uint8_t Byte value.
+ */
+static inline uint8_t i2c_read_byte(i2c_inst_t *i2c) {
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+    assert(hw->status & I2C_IC_STATUS_RFNE_BITS); // Rx FIFO must not be empty
+    return (uint8_t)hw->data_cmd;
+}
+
+/**
+ * \brief Push a byte into I2C Tx FIFO.
+ * 
+ * This function is non-blocking and assumes the Tx FIFO isn't full.
+ * 
+ * \param i2c I2C instance.
+ * \param value Byte value.
+ */
+static inline void i2c_write_byte(i2c_inst_t *i2c, uint8_t value) {
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+    assert(hw->status & I2C_IC_STATUS_TFNF_BITS); // Tx FIFO must not be full
+    hw->data_cmd = value;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _I2C_FIFO_H_
diff --git a/i2c_slave.c b/i2c_slave.c
new file mode 100644
index 0000000..1eb21dc
--- /dev/null
+++ b/i2c_slave.c
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#include "i2c_slave.h"
+#include "hardware/irq.h"
+
+typedef struct i2c_slave_t
+{
+    i2c_inst_t *i2c;
+    i2c_slave_handler_t handler;
+    bool transfer_in_progress;
+} i2c_slave_t;
+
+static i2c_slave_t i2c_slaves[2];
+
+static inline void finish_transfer(i2c_slave_t *slave) {
+    if (slave->transfer_in_progress) {
+        slave->handler(slave->i2c, I2C_SLAVE_FINISH);
+        slave->transfer_in_progress = false;
+    }
+}
+
+static void __not_in_flash_func(i2c_slave_irq_handler)(i2c_slave_t *slave) {
+    i2c_inst_t *i2c = slave->i2c;
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+
+    uint32_t intr_stat = hw->intr_stat;
+    if (intr_stat == 0) {
+        return;
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_TX_ABRT_BITS) {
+        hw->clr_tx_abrt;
+        finish_transfer(slave);
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_START_DET_BITS) {
+        hw->clr_start_det;
+        finish_transfer(slave);
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_STOP_DET_BITS) {
+        hw->clr_stop_det;
+        finish_transfer(slave);
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_RX_FULL_BITS) {
+        slave->transfer_in_progress = true;
+        slave->handler(i2c, I2C_SLAVE_RECEIVE);
+    }
+    if (intr_stat & I2C_IC_INTR_STAT_R_RD_REQ_BITS) {
+        hw->clr_rd_req;
+        slave->transfer_in_progress = true;
+        slave->handler(i2c, I2C_SLAVE_REQUEST);
+    }
+}
+
+static void __not_in_flash_func(i2c0_slave_irq_handler)() {
+    i2c_slave_irq_handler(&i2c_slaves[0]);
+}
+
+static void __not_in_flash_func(i2c1_slave_irq_handler)() {
+    i2c_slave_irq_handler(&i2c_slaves[1]);
+}
+
+void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler) {
+    assert(i2c == i2c0 || i2c == i2c1);
+    assert(handler != NULL);
+
+    uint i2c_index = i2c_hw_index(i2c);
+    i2c_slave_t *slave = &i2c_slaves[i2c_index];
+    slave->i2c = i2c;
+    slave->handler = handler;
+
+    // Note: The I2C slave does clock stretching implicitly after a RD_REQ, while the Tx FIFO is empty.
+    // There is also an option to enable clock stretching while the Rx FIFO is full, but we leave it
+    // disabled since the Rx FIFO should never fill up (unless slave->handler() is way too slow).
+    i2c_set_slave_mode(i2c, true, address);
+
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+    // unmask necessary interrupts
+    hw->intr_mask = I2C_IC_INTR_MASK_M_RX_FULL_BITS | I2C_IC_INTR_MASK_M_RD_REQ_BITS | I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS | I2C_IC_INTR_MASK_M_STOP_DET_BITS | I2C_IC_INTR_MASK_M_START_DET_BITS;
+
+    // enable interrupt for current core
+    uint num = I2C0_IRQ + i2c_index;
+    irq_set_exclusive_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler);
+    irq_set_enabled(num, true);
+}
+
+void i2c_slave_deinit(i2c_inst_t *i2c) {
+    assert(i2c == i2c0 || i2c == i2c1);
+
+    uint i2c_index = i2c_hw_index(i2c);
+    i2c_slave_t *slave = &i2c_slaves[i2c_index];
+    assert(slave->i2c == i2c); // should be called after i2c_slave_init()
+
+    slave->i2c = NULL;
+    slave->handler = NULL;
+    slave->transfer_in_progress = false;
+
+    uint num = I2C0_IRQ + i2c_index;
+    irq_set_enabled(num, false);
+    irq_remove_handler(num, i2c_index == 0 ? i2c0_slave_irq_handler : i2c1_slave_irq_handler);
+
+    i2c_hw_t *hw = i2c_get_hw(i2c);
+    hw->intr_mask = I2C_IC_INTR_MASK_RESET;
+
+    i2c_set_slave_mode(i2c, false, 0);
+}
diff --git a/i2c_slave.h b/i2c_slave.h
new file mode 100644
index 0000000..4505e66
--- /dev/null
+++ b/i2c_slave.h
@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) 2021 Valentin Milea <valentin.milea@gmail.com>
+ *
+ * SPDX-License-Identifier: MIT
+ */
+
+#ifndef _I2C_SLAVE_H_
+#define _I2C_SLAVE_H_
+
+#include <hardware/i2c.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/** \file i2c_slave.h
+ *
+ * \brief I2C slave setup.
+ */
+
+/**
+ * \brief I2C slave event types.
+ */
+typedef enum i2c_slave_event_t
+{
+    I2C_SLAVE_RECEIVE, /**< Data from master is available for reading. Slave must read from Rx FIFO. */
+    I2C_SLAVE_REQUEST, /**< Master is requesting data. Slave must write into Tx FIFO. */
+    I2C_SLAVE_FINISH, /**< Master has sent a Stop or Restart signal. Slave may prepare for the next transfer. */
+} i2c_slave_event_t;
+
+/**
+ * \brief I2C slave event handler
+ * 
+ * The event handler will run from the I2C ISR, so it should return quickly (under 25 us at 400 kb/s).
+ * Avoid blocking inside the handler and split large data transfers across multiple calls for best results.
+ * When sending data to master, up to `i2c_get_write_available()` bytes can be written without blocking.
+ * When receiving data from master, up to `i2c_get_read_available()` bytes can be read without blocking.
+ * 
+ * \param i2c Slave I2C instance.
+ * \param event Event type.
+ */
+typedef void (*i2c_slave_handler_t)(i2c_inst_t *i2c, i2c_slave_event_t event);
+
+/**
+ * \brief Configure I2C instance for slave mode.
+ * 
+ * \param i2c I2C instance.
+ * \param address 7-bit slave address.
+ * \param handler Called on events from I2C master. It will run from the I2C ISR, on the CPU core
+ *                where the slave was initialized.
+ */
+void i2c_slave_init(i2c_inst_t *i2c, uint8_t address, i2c_slave_handler_t handler);
+
+/**
+ * \brief Restore I2C instance to master mode.
+ *
+ * \param i2c I2C instance.
+ */
+void i2c_slave_deinit(i2c_inst_t *i2c);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // _I2C_SLAVE_H_
diff --git a/main.c b/main.c
index 36164da..cbcc606 100644
--- a/main.c
+++ b/main.c
@@ -1,31 +1,24 @@
 #include <stdio.h>
 #include "pico/stdlib.h"
 #include "hardware/i2c.h"
-#include "VL53L1X_api.h"
-#include "VL53L1X_calibration.h"
+#include "VL53L1X_Fonctions.h"
 #include "SelectionCapteur.h"
 #include "hardware/pio.h"
+#include "hardware/i2c.h"
 #include "ws2812.h"
 #include "ws2812.pio.h"
-
-#define I2C_SDA_PIN 0
-#define I2C_SCL_PIN 1
+#include "i2c_fifo.h"
+#include "i2c_slave.h"
 
 
-#define DISTANCE_TRES_LOIN_CM 120
-#define DISTANCE_LOIN_CM 80
-#define DISTANCE_PROCHE_CM 14
+#include "Tests.h"
 
-// Couleur quand la distance est trop loin pour s'en servir avec fiabilité
-#define COULEUR_TRES_LOIN 0x00, 0x00, 0x01
-// Couleur quand la distance fiable mais loin
-#define COULEUR_LOIN 0x00, 0x01, 0x00
-// Couleur quand la distance fiable et que le robot doit ralentir
-#define COULEUR_PROCHE 0x01, 0x01, 0x00
-// Couleur trop proche : rien ne devrait être si près
-#define COULEUR_TROP_PROCHE 0x01, 0x00, 0x01
+#define I2C0_SDA_PIN 0
+#define I2C0_SCL_PIN 1
+
+#define I2C1_SDA_PIN 18
+#define I2C1_SCL_PIN 19
 
-#define TEST_TIMEOUT_US 10000000
 
 void i2c_master_init(void);
 int continuous_reading(uint8_t device);
@@ -35,17 +28,81 @@ void initialise_adresses(void);
 int continuous_multiple_reading(void);
 void affiche_distance_sur_led();
 void init_sensors(void);
-void display_menu();
 
-// Stock les valeurs lues des capteurs
-uint8_t distance_capteur_cm[12];
 
-uint8_t statu_capteurs[13];
+
+#define I2C_SLAVE_ADDRESS 0x18
+
+static const uint I2C_SLAVE_SDA_PIN = I2C1_SDA_PIN; 
+static const uint I2C_SLAVE_SCL_PIN = I2C1_SCL_PIN;
+
+// The slave implements a 256 byte memory. To write a series of bytes, the master first
+// writes the memory address, followed by the data. The address is automatically incremented
+// for each byte transferred, looping back to 0 upon reaching the end. Reading is done
+// sequentially from the current memory address.
+static struct
+{
+    uint8_t mem[256];
+    uint8_t mem_address;
+    bool mem_address_written;
+} context;
+
+// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls /
+// printing to stdio may interfere with interrupt handling.
+static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
+    switch (event) {
+    case I2C_SLAVE_RECEIVE: // master has written some data
+        if (!context.mem_address_written) {
+            // writes always start with the memory address
+            context.mem_address = i2c_read_byte(i2c);
+            context.mem_address_written = true;
+        } else {
+            // save into memory
+            context.mem[context.mem_address] = i2c_read_byte(i2c);
+            context.mem_address++;
+        }
+        break;
+    case I2C_SLAVE_REQUEST: // master is requesting data
+        // load from memory
+        i2c_write_byte(i2c, context.mem[context.mem_address]);
+        context.mem_address++;
+        break;
+    case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
+        context.mem_address_written = false;
+        break;
+    default:
+        break;
+    }
+}
+
+void i2c_set_slave_mode_perso(i2c_inst_t *i2c, uint8_t addr) {
+    i2c->hw->enable = 0;
+    
+    //while( !(i2c->hw->enable_status & 0x1) );
+    
+    i2c->hw->sar = addr;
+    i2c->hw->con = 0;
+    
+    i2c->hw->enable = 1;
+}
+
+static void setup_slave() {
+    gpio_init(I2C_SLAVE_SDA_PIN);
+    gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SLAVE_SDA_PIN);
+
+    gpio_init(I2C_SLAVE_SCL_PIN);
+    gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(I2C_SLAVE_SCL_PIN);
+
+    i2c_slave_init(i2c1, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
+}
+
 
 void main(void)
 {
     int status;
-    int answer_at_least_once=0;
+    
     uint8_t VL53L1X_device = 0x29;
 
     stdio_init_all();
@@ -56,307 +113,25 @@ void main(void)
     ws2812_init();
 
     printf("End waiting\n");
+    setup_slave();
 
+    //Tests();
+
+    initialise_adresses();
     while(1){
-        int keycode;
-        display_menu();
-        
-        do{
-            keycode = getchar_timeout_us(TEST_TIMEOUT_US);
-            if(!answer_at_least_once){
-                display_menu();
-            }
-        }while(keycode == PICO_ERROR_TIMEOUT || keycode == 0);
-        answer_at_least_once = 1;
-
-        switch (keycode)
-        {
-        case 'a':
-        case 'A':
-            printf("Changement d'adresse\n");
-            change_address(&VL53L1X_device, VL53L1X_device + 3);
-            printf("New address: %d\n", VL53L1X_device);
-            break;
-        case 'i':
-        case 'I':
-            printf("Initialisation des capteurs\n");
-            initialise_adresses();
-            break;
-        case 'j':
-        case 'J':
-            while(continuous_multiple_reading());
-            break;
-        case 'k':
-        case 'K':
-            ws2812_arc_en_ciel();
-            break;
-        case 'l':
-        case 'L':
-            while(continuous_reading(0x31));
-            break;
-        case 'o':
-        case 'O':
-            while(calibration(VL53L1X_device));
-            break;
-        case 'r':
-        case 'R':
-            while(continuous_reading(VL53L1X_device));
-            break;
-        default :
-            break;
+        // Lecture des capteurs
+        continuous_multiple_reading();
 
+        for(uint8_t capteur=0; capteur<12; capteur++){
+            context.mem[capteur] = distance_capteur_cm[capteur];
         }
     }
 
-}
-
-void initialise_adresses(void){
-    const uint8_t tmp_i2c_adresse = 0x28;
-    const uint8_t default_i2c_adresse = 0x29;
-    uint8_t adresse = default_i2c_adresse;
-
     
 
-    // On change l'adresse de tous les capteurs
-    Selection_capteur_deselect();
-    change_address(&adresse, tmp_i2c_adresse);
-
-    // Pour chaque capteur
-    for(uint capteur=1; capteur<=12; capteur++){
-        // reset du capteur
-        Selection_capteur_select(capteur);
-        sleep_ms(1);
-        Selection_capteur_deselect();
-        sleep_ms(1);
-        uint8_t VL53L1X_device = 0x29;
-
-        if(change_address(&VL53L1X_device, 0x30 + capteur)){
-            printf("Erreur change adresse : %x => %x, capteur : %d\n", VL53L1X_device, 0x30 + capteur, capteur);
-            ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
-            statu_capteurs[capteur]=0;
-        }else{
-            if(VL53L1X_SensorInit(VL53L1X_device)){
-                // bad init
-                ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
-                statu_capteurs[capteur]=0;
-            }else{
-                // good init
-                statu_capteurs[capteur]=1;
-                int status;
-                status = VL53L1X_SetDistanceMode (VL53L1X_device, 1); // Short mode
-                status |= VL53L1X_SetInterMeasurementInMs(VL53L1X_device, 200); 
-                status |= VL53L1X_SetTimingBudgetInMs(VL53L1X_device, 200);
-                if(status){
-                    printf("Custom config KO, error %d\n", status);
-                    ws2812_set_buffer_rgb(0x4, 0, 0, capteur-1);
-                }else{
-                    printf("Custom config OK\n");
-                }
-
-                status=VL53L1X_StartRanging(VL53L1X_device);
-                if(!status){
-                    ws2812_set_buffer_rgb(0, 0x4, 0, capteur-1);
-                }else{
-                    ws2812_set_buffer_rgb(0x2, 0x2, 0, capteur-1);
-                }
-
-                
-            }
-        }
-    }
-    ws2812_affiche_buffer();
-
-    ws2812_affiche_buffer();
-
 }
 
 
-int continuous_multiple_reading(){
-    for(uint8_t device=0x31; device<0x31+12; device++){
-        if(statu_capteurs[device-0x31+1]==0){
-            continue;
-        }
-        int status;
-        uint8_t data_ready = 0;
-        uint16_t distance_mm;
-        while(!data_ready){
-            status=VL53L1X_CheckForDataReady(device, &data_ready);
-            if(status){
-                printf("CheckForDataReady KO, error %d, capteur:%x\n", status, device);
-            }
-        }
-        
-        status=VL53L1X_GetDistance(device, &distance_mm);
-        if(status){
-            printf("GetDistance KO, error %d, capteur:%x\n", status, device);
-            return 0;
-        }else{
-            printf(">distance%x:%d\n", device, distance_mm);
-            distance_capteur_cm[device-0x31] = distance_mm / 10;
-        }
-        
-        status=VL53L1X_ClearInterrupt(device);
-        if(status){
-            printf("ClearInterrupt KO, error %d, capteur:%x\n", status, device);
-            return 0;
-        }
-
-        int lettre = getchar_timeout_us(0);
-        if(lettre != PICO_ERROR_TIMEOUT && lettre != 0){
-            //return 0;
-        }
-    }
-    affiche_distance_sur_led();
-    return 1;
-}
-
-void affiche_distance_sur_led(){
-    uint8_t distance_cm;
-    uint32_t couleur;
-    for(uint8_t capteur=0; capteur<12; capteur++){
-        distance_cm = distance_capteur_cm[capteur];
-        if(distance_cm == 0 ||distance_cm > DISTANCE_TRES_LOIN_CM){
-            ws2812_set_buffer_rgb(COULEUR_TRES_LOIN, capteur);
-        }else if(distance_cm > DISTANCE_LOIN_CM){
-            ws2812_set_buffer_rgb(COULEUR_LOIN, capteur);
-        }else if(distance_cm > DISTANCE_PROCHE_CM){
-            ws2812_set_buffer_rgb(COULEUR_PROCHE, capteur);
-        }else{
-            ws2812_set_buffer_rgb(COULEUR_TROP_PROCHE, capteur);
-        }
-        
-    }
-    ws2812_affiche_buffer();
-}
-
-
-void display_menu(){
-    printf("Select action :\n");
-    printf("A - Change I2C address\n");
-    printf("I - Init I2C address\n");
-    printf("J - Lecture distance multiple\n");
-    printf("K - Arc en ciel\n");
-    printf("L - Lecture distance capteur 1\n");
-    printf("O - Offset Calibration\n");
-    printf("R - Read distance\n");
-}
-
-int change_address(uint8_t *device, uint8_t new_i2c_7bits_address){
-    int status;
-    status = VL53L1X_SetI2CAddress(*device, new_i2c_7bits_address << 1);
-    if(status){
-        //printf("VL53L1X_SetI2CAddress, Error :%d\n", status);
-    }else{
-        *device=new_i2c_7bits_address;
-    }
-    return status;
-}
-
-int calibration(uint8_t device){
-    uint16_t offset;
-    int status;
-    uint8_t boot_state=0;
-    printf("Calibration...\n");
-    while(!boot_state){
-        VL53L1X_BootState(device, &boot_state);
-    }
-    printf("Sensor boot ok\n");
-
-    status=VL53L1X_SensorInit(device);
-    if(status){
-        printf("Sensor Init KO, error %d\n", status);
-    }else{
-        printf("Sensor Init OK\n");
-    }
-
-    status = VL53L1X_CalibrateOffset(device, 140, &offset);
-    if(status != 0){
-        printf("Error while calibrating : %d\n",status);
-    }else{
-        printf("Offset : %d\n", offset);
-
-    }
-    return 0;
-}
-
-int continuous_reading(uint8_t device){
-    int status;
-    uint8_t data_ready, boot_state=0;
-    uint16_t distance;
-
-    printf("Reading distance...\nSend any character to quit.");
-
-    while(!boot_state){
-        VL53L1X_BootState(device, &boot_state);
-    }
-    printf("Sensor boot ok\n");
-
-    status=VL53L1X_SensorInit(device);
-    if(status){
-        printf("Sensor Init KO, error %d\n", status);
-        return 0;
-    }else{
-        printf("Sensor Init OK\n");
-    }
-
-
-    // Custom configuration
-    status = VL53L1X_SetDistanceMode (device, 1); // Short mode
-    status |= VL53L1X_SetInterMeasurementInMs(device, 200); 
-    status |= VL53L1X_SetTimingBudgetInMs(device, 200);
-    if(status){
-        printf("Custom config KO, error %d\n", status);
-        return 0;
-    }else{
-        printf("Custom config OK\n");
-    }
-
-    status=VL53L1X_StartRanging(device);
-    if(status){
-        printf("Start ranging KO, error %d\n", status);
-        return 0;
-    }else{
-        printf("Start ranging OK\n");
-    }
-
-    while(1){
-    // Reading data
-        data_ready = 0;
-        while(!data_ready){
-            status=VL53L1X_CheckForDataReady(device, &data_ready);
-            if(status){
-                printf("CheckForDataReady KO, error %d\n", status);
-                return 0;
-            }else{
-                //printf("CheckForDataReady OK\n");
-            }
-        }
-        
-        status=VL53L1X_GetDistance(device, &distance);
-        if(status){
-            printf("GetDistance KO, error %d\n", status);
-            return 0;
-        }else{
-            //printf("GetDistance OK, distance %u mm\n", distance);
-            printf(">distance:%d\n", distance);
-        }
-        
-        status=VL53L1X_ClearInterrupt(device);
-        if(status){
-            printf("ClearInterrupt KO, error %d\n", status);
-            return 0;
-        }else{
-            //printf("ClearInterrupt OK\n");
-        }
-
-        int lettre = getchar_timeout_us(0);
-        if(lettre != PICO_ERROR_TIMEOUT && lettre != 0){
-            return 0;
-        }
-    }
-    return 0;
-}
-
 void i2c_master_init(void){
     //stdio_init_all();
     i2c_init(i2c0, 100 * 1000);
@@ -369,8 +144,8 @@ void i2c_master_init(void){
         }
     }
 
-    printf("%d and %d for I2C\n", I2C_SDA_PIN, I2C_SCL_PIN);
-    gpio_set_function(I2C_SDA_PIN, GPIO_FUNC_I2C);
-    gpio_set_function(I2C_SCL_PIN, GPIO_FUNC_I2C);
+    printf("%d and %d for I2C\n", I2C0_SDA_PIN, I2C0_SCL_PIN);
+    gpio_set_function(I2C0_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(I2C0_SCL_PIN, GPIO_FUNC_I2C);
 
 }