#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include "VL53L1X_api.h"
#include "VL53L1X_calibration.h"
#include "SelectionCapteur.h"
#include "hardware/pio.h"
#include "ws2812.h"
#include "ws2812.pio.h"

#define I2C_SDA_PIN 0
#define I2C_SCL_PIN 1


#define TEST_TIMEOUT_US 10000000

void i2c_master_init(void);
int continuous_reading(uint8_t device);
int calibration(uint8_t device);
int change_address(uint8_t * device, uint8_t new_i2c_7bits_address);
void initialise_adresses(void);
void init_sensors_1_et_2(void);
void init_sensors(void);
void test_changement_adresse_1(void);
void test_changement_adresse_2(void);
void test_changement_adresse_3(void);
void display_menu();



void main(void)
{
    int status;
    int answer_at_least_once=0;
    uint8_t VL53L1X_device = 0x29;

    stdio_init_all();
    i2c_master_init();
    Selection_capteur_init();
    //Selection_capteur_select(1);

    ws2812_init();

    printf("End waiting\n");

    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 adresses\n");
            initialise_adresses();
            printf("Initialisation des capteurs\n");
            init_sensors();
            break;
        case 'j':
        case 'J':
            init_sensors_1_et_2();
            break;
        case 'k':
        case 'K':
            ws2812_arc_en_ciel();
            break;
        case 'l':
        case 'L':
            test_changement_adresse_1();
            break;
        case 'm':
        case 'M':
            test_changement_adresse_2();
            break;
        case 'n':
        case 'N':
            test_changement_adresse_3();
            break;
        case 'o':
        case 'O':
            while(calibration(VL53L1X_device));
            break;
        case 'r':
        case 'R':
            while(continuous_reading(VL53L1X_device));
            break;
        default :
            break;

        }
    }

}

void initialise_adresses(void){
    uint8_t VL53L1X_device = 0x29;
    for(uint capteur=1; capteur<12; capteur++){
        Selection_capteur_select(capteur);
        if(change_address(&VL53L1X_device, VL53L1X_device+1)){
            printf("Erreur change adresse : %x => %x, capteur : %d\n", VL53L1X_device, VL53L1X_device+1, capteur);
            VL53L1X_device++;
        }else{
            printf("change adresse : %x => %x, sauf capteur : %d\n", VL53L1X_device-1, VL53L1X_device, capteur);
        }
    }
}

void init_sensors(void){
    uint8_t VL53L1X_device = 0x29;
    uint8_t adresse;
    Selection_capteur_deselect();
    for(uint capteur=0; capteur<12; capteur++){
        adresse = VL53L1X_device + capteur;
        if(VL53L1X_SensorInit(adresse)){
            // Bad init
            ws2812_set_buffer_rgb(1,0,0, capteur);
            printf("Init KO : capteur %d, adresse %x\n", capteur, adresse);
        }else{
            // Good init
            ws2812_set_buffer_rgb(0,1,0, capteur);
            printf("Init ok : capteur %d, adresse %x\n", capteur, adresse);
        }
    }
    ws2812_affiche_buffer();
}

void init_sensors_1_et_2(void){
    const uint8_t tmp_i2c_adresse = 0x28;
    const uint8_t default_i2c_adresse = 0x29;
    uint8_t VL53L1X_device_1 = default_i2c_adresse;
    uint8_t VL53L1X_device_2 = default_i2c_adresse;
    uint8_t VL53L1X_device_3 = default_i2c_adresse;
    Selection_capteur_deselect();    
    sleep_ms(10);
    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_1, 0x30);
    if(change_address(&VL53L1X_device_1, 0x30)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }
    
    /* // réinitialisation du capteur 1.
    printf("Capteur #1 par defaut : 0x29\n");
    Selection_capteur_select(1);
    sleep_ms(1);
    Selection_capteur_deselect();    
    sleep_ms(1);
    VL53L1X_device_1 = 0x29;*/

    /*printf("Changement d'adresse capteur #1 %x => %x\n", VL53L1X_device_1, 0x30);
    if(change_address(&VL53L1X_device_1, 0x30)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }*/

    if(VL53L1X_SensorInit(VL53L1X_device_1)){
        printf("Init KO : adresse %x\n", VL53L1X_device_1);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_1);
    }

    Selection_capteur_select(1);
    sleep_ms(1);
    Selection_capteur_deselect();    
    sleep_ms(1);

    if(VL53L1X_SensorInit(VL53L1X_device_1)){
        printf("Init KO : adresse %x\n", VL53L1X_device_1);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_1);
    }

    if(VL53L1X_SensorInit(default_i2c_adresse)){
        printf("Init KO : adresse %x\n", default_i2c_adresse);
    }else{
        printf("Init OK : adresse %x\n", default_i2c_adresse);
    }


    /* // réinitialisation du capteur 2.
    Selection_capteur_select(2);
    sleep_ms(1);
    Selection_capteur_deselect();    
    sleep_ms(1);
    VL53L1X_device_2 = 0x29;

    printf("Changement d'adresse capteur #2 %x => %x\n", VL53L1X_device_2, 0x31);
    if(change_address(&VL53L1X_device_2, 0x31)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }*/
}

void test_changement_adresse_1(void){
    const uint8_t tmp_i2c_adresse = 0x28;
    const uint8_t default_i2c_adresse = 0x29;
    uint8_t VL53L1X_device_1 = default_i2c_adresse;
    uint8_t VL53L1X_device_2 = default_i2c_adresse;
    uint8_t VL53L1X_device_3 = default_i2c_adresse;
    Selection_capteur_deselect();    
    sleep_ms(10);
    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_1, 0x30);
    if(change_address(&VL53L1X_device_1, 0x30)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

    /*if(VL53L1X_SensorInit(VL53L1X_device_1)){
        printf("Init KO : adresse %x\n", VL53L1X_device_1);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_1);
    }*/

    VL53L1X_device_1 = 0x29;

    printf("Changement d'adresse %x => %x\n", VL53L1X_device_1, 0x31);
    if(change_address(&VL53L1X_device_1, 0x31)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }
    if(VL53L1X_SensorInit(0x30)){
        printf("Init KO : adresse %x\n", 0x30);
    }else{
        printf("Init OK : adresse %x\n", 0x30);
    }
    if(VL53L1X_SensorInit(0x31)){
        printf("Init KO : adresse %x\n", 0x31);
    }else{
        printf("Init OK : adresse %x\n", 0x31);
    }
}

void test_changement_adresse_2(void){
    const uint8_t tmp_i2c_adresse = 0x30;
    const uint8_t default_i2c_adresse = 0x29;
    uint8_t VL53L1X_device_1 = default_i2c_adresse;
    uint8_t VL53L1X_device_2 = default_i2c_adresse;
    uint8_t VL53L1X_device_3 = default_i2c_adresse;
    Selection_capteur_deselect();    
    sleep_ms(10);
    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_1, tmp_i2c_adresse);
    if(change_address(&VL53L1X_device_1, tmp_i2c_adresse)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_2, tmp_i2c_adresse);
    if(change_address(&VL53L1X_device_2, tmp_i2c_adresse)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

    // Réinitialisation du capteur 1.
    printf("Capteur #1 par defaut : 0x29\n");
    Selection_capteur_select(1);
    sleep_ms(1);
    Selection_capteur_deselect();    
    sleep_ms(1);
    VL53L1X_device_1 = 0x29;


    printf("Changement d'adresse %x => %x\n", VL53L1X_device_1, 0x31);
    if(change_address(&VL53L1X_device_1, 0x31)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

    /* // Réinitialisation du capteur 2.
    printf("Capteur #2 par defaut : 0x29\n");
    Selection_capteur_select(2);
    sleep_ms(1);
    Selection_capteur_deselect();    
    sleep_ms(1);

    VL53L1X_device_2 = 0x29;
    printf("Changement d'adresse %x => %x\n", VL53L1X_device_2, 0x32);
    if(change_address(&VL53L1X_device_2, 0x32)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }*/

    
    if(VL53L1X_SensorInit(VL53L1X_device_1)){
        printf("Init KO : adresse %x\n", VL53L1X_device_1);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_1);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_2)){
        printf("Init KO : adresse %x\n", VL53L1X_device_2);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_2);
    }
    if(VL53L1X_SensorInit(tmp_i2c_adresse)){
        printf("Init KO : adresse %x\n", tmp_i2c_adresse);
    }else{
        printf("Init OK : adresse %x\n", tmp_i2c_adresse);
    }
}

void test_changement_adresse_3(void){
    const uint8_t tmp_i2c_adresse = 0x30;
    const uint8_t default_i2c_adresse = 0x29;
    uint8_t VL53L1X_device_1 = default_i2c_adresse;
    uint8_t VL53L1X_device_2 = default_i2c_adresse;
    uint8_t VL53L1X_device_3 = default_i2c_adresse;
    Selection_capteur_deselect();    
    sleep_ms(10);
    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_1, tmp_i2c_adresse);
    if(change_address(&VL53L1X_device_1, tmp_i2c_adresse)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

    printf("Changement d'adresse (tous) %x => %x\n", VL53L1X_device_2, tmp_i2c_adresse);
    if(change_address(&VL53L1X_device_2, tmp_i2c_adresse)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

    // Réinitialisation du capteur 1.
    printf("Capteur #1 par defaut : 0x29\n");
    Selection_capteur_select(1);
    sleep_ms(1);
    Selection_capteur_deselect();   
    sleep_ms(1);
    VL53L1X_device_1 = 0x29;


    printf("Changement d'adresse %x => %x\n", VL53L1X_device_1, 0x31);
    if(change_address(&VL53L1X_device_1, 0x31)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

     // Réinitialisation du capteur 3.
    printf("Capteur #2 par defaut : 0x29\n");
    Selection_capteur_select(3);
    sleep_ms(1);
    Selection_capteur_deselect();
    sleep_ms(1);

    VL53L1X_device_3 = 0x29;
    printf("Changement d'adresse %x => %x\n", VL53L1X_device_3, 0x33);
    if(change_address(&VL53L1X_device_3, 0x33)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

    if(VL53L1X_SensorInit(default_i2c_adresse)){
        printf("Init KO : adresse %x\n", default_i2c_adresse);
    }else{
        printf("Init OK : adresse %x\n", default_i2c_adresse);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_1)){
        printf("Init KO : adresse %x\n", VL53L1X_device_1);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_1);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_2)){
        printf("Init KO : adresse %x\n", VL53L1X_device_2);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_2);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_3)){
        printf("Init KO : adresse %x\n", VL53L1X_device_3);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_3);
    }
    if(VL53L1X_SensorInit(tmp_i2c_adresse)){
        printf("Init KO : adresse %x\n", tmp_i2c_adresse);
    }else{
        printf("Init OK : adresse %x\n", tmp_i2c_adresse);
    }




     // Réinitialisation du capteur 2.
    printf("Capteur #2 par defaut : 0x29\n");
    Selection_capteur_select(2);
    sleep_ms(1);

    if(VL53L1X_SensorInit(default_i2c_adresse)){
        printf("Init KO : adresse %x\n", default_i2c_adresse);
    }else{
        printf("Init OK : adresse %x\n", default_i2c_adresse);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_1)){
        printf("Init KO : adresse %x\n", VL53L1X_device_1);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_1);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_2)){
        printf("Init KO : adresse %x\n", VL53L1X_device_2);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_2);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_3)){
        printf("Init KO : adresse %x\n", VL53L1X_device_3);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_3);
    }
    if(VL53L1X_SensorInit(tmp_i2c_adresse)){
        printf("Init KO : adresse %x\n", tmp_i2c_adresse);
    }else{
        printf("Init OK : adresse %x\n", tmp_i2c_adresse);
    }



    Selection_capteur_deselect();
    sleep_ms(1);

    VL53L1X_device_2 = 0x29;
    printf("Changement d'adresse %x => %x\n", VL53L1X_device_2, 0x32);
    if(change_address(&VL53L1X_device_2, 0x32)){
        printf("-> KO\n");
    }else{
        printf("-> ok;\n");
    }

    
    if(VL53L1X_SensorInit(default_i2c_adresse)){
        printf("Init KO : adresse %x\n", default_i2c_adresse);
    }else{
        printf("Init OK : adresse %x\n", default_i2c_adresse);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_1)){
        printf("Init KO : adresse %x\n", VL53L1X_device_1);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_1);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_2)){
        printf("Init KO : adresse %x\n", VL53L1X_device_2);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_2);
    }
    if(VL53L1X_SensorInit(VL53L1X_device_3)){
        printf("Init KO : adresse %x\n", VL53L1X_device_3);
    }else{
        printf("Init OK : adresse %x\n", VL53L1X_device_3);
    }
    if(VL53L1X_SensorInit(tmp_i2c_adresse)){
        printf("Init KO : adresse %x\n", tmp_i2c_adresse);
    }else{
        printf("Init OK : adresse %x\n", tmp_i2c_adresse);
    }

}


void display_menu(){
    printf("Select action :\n");
    printf("A - Change I2C address\n");
    printf("I - Init I2C address\n");
    printf("J - Init catpeur 1 et 2\n");
    printf("K - Arc en ciel\n");
    printf("L - Test changement d'adresse, 1 capteur\n");
    printf("M - Test changement d'adresse, 2 capteurs\n");
    printf("N - Test changement d'adresse, 3 capteurs\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);

    printf("Initialisation des broches\n");
    for(int i=0; i++; i<=28){
        if(gpio_get_function(i) == GPIO_FUNC_I2C){
            printf("Pin I2C : %d\n", i);
            gpio_set_function(i, GPIO_FUNC_NULL);
        }
    }

    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);

}