#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");
}