#include "pico/stdlib.h"
#include "pico/multicore.h"
#include <stdio.h>
#include "ws2812.h"
#include "VL53L1X_Fonctions.h"

void display_menu();
void test_couleur_led();

#define TEST_TIMEOUT_US 10000000


void blink_test(void){
    const uint LED_PIN = PICO_DEFAULT_LED_PIN;
    uint8_t * distance_capteur_cm = get_distance_capteur_cm();
    while(1){
        for(uint8_t capteur=0; capteur<12; capteur++){
            printf(">distance%x:%d\n", capteur, distance_capteur_cm[capteur]);
        }
        sleep_ms(50);

    }

}

void Tests(void){
    int answer_at_least_once=0;
    uint8_t VL53L1X_device = 0x31;
    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 'c':
        case 'C':
            printf("Affichage couleur sur LED\n");
            test_couleur_led();
            break;
        case 'd':
        case 'D':
            printf("Reset affichage couleur sur LED\n");
            reset_affichage_led();
            break;
        case 'i':
        case 'I':
            printf("Initialisation des capteurs\n");
            initialise_adresses();
            break;
        case 'j':
        case 'J':
            printf("Lecture continue des capteurs inutilisés.\nAppuyer sur une touche pour quitter\n");
            multicore_launch_core1(blink_test);
            do{
                static uint8_t capteur_courant=0;
                uint8_t * distance_capteur_cm = get_distance_capteur_cm();
                keycode = getchar_timeout_us(0);
                // Lecture des capteurs
                if(capteur_pret(capteur_courant)){
                    uint8_t distance_cm;
                    if(capteur_lire_distance_cm(capteur_courant, &distance_cm)){
                        distance_capteur_cm[capteur_courant]= distance_cm;
                    }
                }
                capteur_courant++;
                if(capteur_courant > 11){
                    capteur_courant = 0;
                }
                // Affichage des distances sur les LEDs.
                affiche_distance_sur_led(distance_capteur_cm);
            }while(keycode == PICO_ERROR_TIMEOUT || keycode == 0);
            break;
        case 'k':
        case 'K':
            ws2812_arc_en_ciel();
            break;
        case 'l':
        case 'L':
            while(continuous_reading(0x38));
            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("C - Couleur Led\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");
}

/// @brief Force une couleur sur une led qui ne doit pas être effacée par la distance lue par le capteur.
/// A utiliser après l'initialisation des catpeurs et avant la lecture continue
void test_couleur_led(){
    unsigned int couleur, led;
    int scanf1, scanf2;
    printf("Choississez la led (0 - 11)\n");
    scanf1 = scanf("%u", &led);
    printf("Choississez la couleur (0 - 255)\n");
    scanf2 = scanf("%u", &couleur);
    if(scanf1 == 1 && scanf2 == 1){
        printf("Actualisation de la led %d avec la couleur %d\n", led, couleur);
        affiche_couleur_sur_led(couleur, led);
    }else{
        printf("Erreur scanf...\n");
    }
    ws2812_affiche_buffer();
}