#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/i2c.h"
#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "Test_i2c.h"
#include <stdio.h>

#define TEST_TIMEOUT_US 10000000

void affiche_contacteur(void);

int test_i2c_bus(void);
int test_i2c_lecture_pico_annex(void);
int test_i2c_lecture_pico_annex_nb2(void);
int test_i2c_ecriture_pico_annex_nb(void);
int test_i2c_ecriture_pico_annex_nb_2(void);
bool reserved_addr(uint8_t addr);


int test_i2c(){
    int lettre;

    while(1){

        do{
            printf("A : Scan bus I2C\n");
            printf("B : Lecture I2C bloquante\n");
            printf("C : Lecture I2C non bloquante\n");
            printf("D : Ecriture I2C non bloquante\n");
            printf("E : Ecriture I2C non bloquante - fonctions encapsulées\n");            
            printf("Q : Quitter\n");
            
            lettre = getchar_timeout_us(TEST_TIMEOUT_US);
            stdio_flush();
        }while(lettre == PICO_ERROR_TIMEOUT ||lettre == 0);

        switch(lettre){

            case 'A':
            case 'a':
                while(test_i2c_bus());
                break;

            case 'B':
            case 'b':
                while(test_i2c_lecture_pico_annex());
                break;

            case 'C':
            case 'c':
                while(test_i2c_lecture_pico_annex_nb2());
                break;

            case 'D':
            case 'd':
                while(test_i2c_ecriture_pico_annex_nb());
                break;

            case 'E':
            case 'e':
                while(test_i2c_ecriture_pico_annex_nb_2());
                break;

            case 'Q':
            case 'q':
                return 0;
        }
    }
    
}

bool reserved_addr(uint8_t addr) {
    return (addr & 0x78) == 0 || (addr & 0x78) == 0x78;
}

/// @brief Scan le bus I2C
/// @return 0
int test_i2c_bus(){
    uint8_t reception[8];
    uint8_t emission[8];
    //uint8_t adresse = 0b0100000;
    int statu;
    int lettre;

    emission[0]=6; // Registre à lire

    i2c_maitre_init();
    // Scan bus I2C - cf SDK
    printf("\nI2C Bus Scan\n");
    printf("   0 1 2 3 4 5 6 7 8 9 A B C D E F\n");
    for (int addr = 0; addr < (1 << 7); ++addr) {
        if (addr % 16 == 0) {
            printf("%02x ", addr);
        }
        int ret;
        uint8_t rxdata=0x55;
        if (reserved_addr(addr))
            ret = PICO_ERROR_GENERIC;
        else{
            absolute_time_t time_out = get_absolute_time();
            time_out += 100000; // Ajout 100 ms
            ret = i2c_read_blocking_until(i2c_default, addr, &rxdata, 1, false, time_out);
        }
        
        printf(ret < 0 ? "." : "@");
        printf(addr % 16 == 15 ? "\n" : " ");
    }
    printf("Done.\n");
    return 0;

}


/// @brief Test de lecture I2C - Attention cette fonction écrit 1 octet avant de lire... 
/// @return 0
int test_i2c_lecture_pico_annex(){
    i2c_maitre_init();
    uint8_t tampon[10];
    uint8_t registre=0;
    uint8_t adresse = 0x17;
    int ret; 
    
    ret = i2c_write_blocking(i2c0, adresse, &registre, 1, false);
    if(ret < 0){
        printf("Erreur I2C : %d", ret);
        return 0;
    }else{
        printf("Envoi I2C: OK\n");
    }

    ret = i2c_read_blocking(i2c_default, adresse, tampon, 10, false);
    if(ret < 0){
        printf("Erreur I2C : %d", ret);
    }else{
        for(int i=0; i<10; i++){
            printf("%c", tampon[i]);
        }
        printf("\n");

        for(int i=0; i<10; i++){
            printf("%2x ", tampon[i]);
        }
        printf("\n");
    }
    return 0;
}

/// @brief Lecture I2C non bloquante
/// @return 0
int test_i2c_lecture_pico_annex_nb2(){
    i2c_maitre_init();

    uint8_t tampon[10];
    uint8_t registre=8;
    uint8_t adresse = 0x17;
    uint32_t time_i2c[5];
    const uint8_t T_MAX_I2C = 10;
    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;

    time_i2c[0] = time_us_32();
    time_i2c[2] = 0;

    while(retour_i2c == I2C_EN_COURS){
        time_i2c[1] = time_us_32(); // Pour mesurer le temps d'execution
        i2c_gestion(i2c0);
        retour_i2c = i2c_lire_registre_nb(adresse, registre, tampon, T_MAX_I2C);
        time_i2c[2] += time_us_32() - time_i2c[1]; // Pour mesurer le temps d'execution
        sleep_us(100); // Attente, ou le reste du code
    }
    time_i2c[3] = time_us_32() - time_i2c[0];

    // Affichage
    for(int i=0; i<T_MAX_I2C; i++){
        printf("%c", tampon[i]);
    }
    printf("\n");

    for(int i=0; i<T_MAX_I2C; i++){
        printf("%2x ", tampon[i]);
    }
    printf("\n");

    printf("Temps lecture : %u microsecondes, temps specifique i2c : %u microsecondes.\n", time_i2c[3], time_i2c[2]);

    return 0;
}

/// @brief Ecrit sur le bus I2C 
/// @return 1 pour continuer le test, 0 pour quitter
int test_i2c_ecriture_pico_annex_nb(){
    i2c_maitre_init();

    uint8_t tampon[10];
    uint8_t registre=0x09;
    uint8_t adresse = 0x17;
    uint32_t time_i2c[5];
    const uint8_t T_I2C_ENVOI = 2;
    static uint8_t commande=0;
    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;


    printf("F - Ferme porte\n");
    printf("O - Ouvre porte\n");
    printf("T - Turbine On\n");
    printf("U - Turbine Off\n");
    printf("P - Propulseur On\n");
    printf("M - Propulseur Off\n");
    printf("Q pour quitter\n");

    int lettre;
    do{ 
        lettre = getchar_timeout_us(0);
        stdio_flush();

    }while(lettre == PICO_ERROR_TIMEOUT || lettre == '\0');

    tampon[1] = 0x0;
    switch(lettre){
        case 'F':
        case 'f':
            commande = commande | 0x02; // 0b0000 0010
            printf("=> Ferme porte\n");
            break;

        case 'O':
        case 'o':
            commande = commande & 0xFD; // 0b1111 1101
            printf("=> Ouvre porte\n");
            break;
        
        case 't':
        case 'T':
            commande = commande | 0x01; // 0b0000 0001
            printf("=> Active turbine\n");
            break;
        
        case 'u':
        case 'U':
            commande = commande & 0xFE; // 0b1111 1110
            printf("=> Arrete turbine\n");
            break;
        
        case 'p':
        case 'P':
            commande = commande | 0x08; // 0b0000 1000
            printf("=> Active propulseur\n");
            break;
        
        case 'm':
        case 'M':
            commande = commande & 0xF7; // 0b1111 0111
            printf("=> Arrete propulseur\n");
            break;
            
        case 'q':
        case 'Q':
            return 0;
            break;

    }

    tampon[0] = 54;
    tampon[1] = commande;

    time_i2c[0] = time_us_32();
    time_i2c[2] = 0;

    while(retour_i2c == I2C_EN_COURS){
        time_i2c[1] = time_us_32(); // Pour mesurer le temps d'execution
        i2c_gestion(i2c0);
        retour_i2c = i2c_ecrire_registre_nb(adresse, registre, tampon, T_I2C_ENVOI);
        time_i2c[2] += time_us_32() - time_i2c[1]; // Pour mesurer le temps d'execution
        sleep_us(100); // Attente, ou le reste du code
    }
    time_i2c[3] = time_us_32() - time_i2c[0];

    printf("Temps lecture : %u microsecondes, temps specifique i2c : %u microsecondes.\n", time_i2c[3], time_i2c[2]);

    return 1;
}


/// @brief Test les fonctions définies dans I2C_Annexe
/// @return 0
int test_i2c_ecriture_pico_annex_nb_2(){
    i2c_maitre_init();

    uint32_t time_i2c[5];
    const uint8_t T_I2C_ENVOI = 2;
    static uint8_t commande=0;
    enum i2c_resultat_t retour_i2c = I2C_EN_COURS;


    printf("A - Aimant lache pot\n");
    printf("B - Bras attrape pot\n");
    printf("C - Aimant tient pot\n");
    printf("D - Bras lève pot\n");
    printf("E - Attrape plante - bras 1\n");
    printf("F - Attrape plante - bras 6\n");
    printf("G - Detection bordure\n");
    printf("H - Detection plante\n");
    printf("I - Detection stop\n");
    printf("J - Detection distance loin\n");
    printf("K - Ouvre doigt plante\n");
    printf("L - Ferme doigt plante\n");
    printf("M - ouvre doigt + attrape plante\n");
    printf("N - Pots avant levite\n");
    printf("O - Pots ecarte et tient\n");
    printf("P - Pots ecarte et lache\n");
    
    printf("S - Score + 1\n");
    printf("\nQ - Quitter\n");


    int lettre;
    int continue_test=1;
    uint8_t score=0;
    enum validite_vl53l8_t validite_vl53l8;
    float angle, distance;

    time_i2c[0] = time_us_32();
    time_i2c[1] = time_us_32();
    time_i2c[2] = 0;

    i2c_annexe_init();

    //multicore_launch_core1(affiche_contacteur);


    while(1){
        lettre = getchar_timeout_us(0);
        if(lettre != PICO_ERROR_TIMEOUT && lettre != '\0'){
            printf("lettre !\n");
            switch(lettre){
                case 'a':
                case 'A':
                    for(int i=0; i<6; i++){
                        i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_LACHE);
                    }
                    printf("=> Lache pot\n");
                    break;
                
                case 'b':
                case 'B':
                    for(int i=0; i<6; i++){
                        i2c_annexe_actionneur_pot(i, BRAS_POT_SOL, DOIGT_TIENT);
                    }
                    printf("=> Attrape pot\n");
                    break;

                case 'c':
                case 'C':
                    for(int i=0; i<6; i++){
                        i2c_annexe_actionneur_pot(i, BRAS_PLIE, DOIGT_TIENT);
                    }
                    printf("=> Tient pot\n");
                    break;

                case 'D':
                case 'd':
                    for(int i=0; i<6; i++){
                        i2c_annexe_actionneur_pot(i, BRAS_HAUT, DOIGT_TIENT);
                    }
                    printf("=>Pot haut\n");
                    break;

                case 'E':
                case 'e':
                    i2c_annexe_attrape_plante(PLANTE_BRAS_1);
                    printf("=> Attrape plante - bras 1\n");
                    break;
                    
                case 'F':
                case 'f':
                    i2c_annexe_attrape_plante(PLANTE_BRAS_6);
                    printf("=> Attrape plante - bras 6\n");
                    break;

                case 'G':
                case 'g':
                    i2c_annexe_set_mode_VL53L8(VL53L8_BORDURE);
                    printf("=> Detection bordure\n");
                    break;

                case 'H':
                case 'h':
                    i2c_annexe_set_mode_VL53L8(VL53L8_PLANTE);
                    printf("=> Detection plante\n");
                    break;

                case 'I':
                case 'i':
                    i2c_annexe_set_mode_VL53L8(VL53L8_INVALIDE);
                    printf("=> Detection STOP\n");
                    break;
                case 'J':
                case 'j':
                    i2c_annexe_set_mode_VL53L8(VL53L8_DISTANCE_LOIN);
                    printf("=> Detection distance loin\n");
                    break;

                case 'k':
                case 'K':
                    i2c_annexe_ouvre_doigt_plante();
                    printf("=> Ouvre doigt plante\n");
                    break;

                case 'l':
                case 'L':
                    i2c_annexe_ferme_doigt_plante();
                    printf("=> ferme doigt plante\n");
                    break;

                case 'm':
                case 'M':
                    i2c_annexe_attrape_plante(PLANTE_BRAS_1);
                    i2c_annexe_ouvre_doigt_plante();
                    
                    printf("=> ouvre doigt plante + attrape plante\n");
                    break;

                case 'n':
                case 'N':
                    i2c_annexe_actionneur_pot(0, BRAS_LEVITE, DOIGT_TIENT);
                    i2c_annexe_actionneur_pot(5, BRAS_LEVITE, DOIGT_TIENT);
                    
                    printf("=> Levite pots avant\n");
                    break;

                case 'o':
                case 'O':
                    for(int i=0; i<6; i++){
                        i2c_annexe_actionneur_pot(i, BRAS_ECARTE, DOIGT_TIENT);
                    }
                    printf("=> Ecarte et tient pot\n");
                    break;

                case 'p':
                case 'P':
                    for(int i=0; i<6; i++){
                        i2c_annexe_actionneur_pot(i, BRAS_ECARTE, DOIGT_LACHE);
                    }
                    printf("=> Ecarte et lache pot\n");
                    break;



                case 'q':
                case 'Q':
                    multicore_reset_core1();
                    return 0;

                case 's':
                case 'S':
                    score++;
                    i2c_annexe_envoie_score(score);
                    break;
                
                default:
                    printf("lettre non reconnue: %d %c\n", lettre, lettre);
            }
        }
        sleep_ms(1);

        i2c_gestion(i2c0);        
        i2c_annexe_gestion();

        i2c_annexe_get_VL53L8(&validite_vl53l8, &angle, &distance);
        printf(">v:%d\n", validite_vl53l8);
        switch(validite_vl53l8){
            case VL53L8_BORDURE:
                printf(">b_angle:%.2f\n>b_distance:%.2f\n", angle, distance);
                break;
            case VL53L8_PLANTE:
                printf(">p_angle:%.2f\n>p_distance:%.2f\n", angle, distance);
                break;
            case VL53L8_DISTANCE_LOIN:
                printf("\n>v_distance:%.2f\n", distance);
                break;
        }

    }
    

}

void affiche_contacteur(){
    while(1){
        printf(">contacteur_butee_A:%d\n", i2c_annexe_get_contacteur_butee_A());
        printf(">contacteur_butee_C:%d\n", i2c_annexe_get_contacteur_butee_C());
        printf(">contacteur_longer_A:%d\n", i2c_annexe_get_contacteur_longer_A());
        printf(">contacteur_longer_C:%d\n", i2c_annexe_get_contacteur_longer_C());
        printf(">v_bat:%d\n", i2c_annexe_get_tension_batterie());
    }
}