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