#include "i2c_annexe.h"
#include "i2c_maitre.h"
#include "stdio.h"

#define ADRESSE_PICO_ANNEXE 0x17
#define ADRESSE_DEBUT_W 0x05
#define ADRESSE_DEBUT_R 0x0A
#define ADRESSE_SCORE 0x09
#define ADRESSE_VL53L8_MODE 0x0A
#define ADRESSE_CONTACTEURS 0x0B
#define ADRESSE_VL53L1X 0x0C
#define ADRESSE_TENSION_BATTERIE 0x18
#define ADRESSE_COULEUR 0x05
#define ADRESSE_MASQUE_LED_1 0x06
#define ADRESSE_MASQUE_LED_2 0x07
#define ADRESSE_VL53L8_VALIDITE 0x19
#define ADRESSE_VL53L8_ANGLE 0x1A
#define ADRESSE_VL53L8_DISTANCE 0x1E
#define TAILLE_DONNEES_EMISSION 6
#define TAILLE_DONNEES_RECEPTION 24

#define ADRESSE_PIC18F4550 0x31
#define ADRESSE_PIC18F4550_DEBUT_W 0x00
#define ADRESSE_ACTION_PINCE 6
#define TAILLE_DONNEES_PIC18F4550_EMISSION 7

uint8_t donnees_emission[TAILLE_DONNEES_EMISSION];
uint8_t donnees_emission_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION];
uint8_t donnees_reception_pic18f4550[TAILLE_DONNEES_PIC18F4550_EMISSION];
uint8_t donnees_reception[TAILLE_DONNEES_RECEPTION];

union float_or_octet_t
{
    float f_value;
    unsigned char octet[4];
};

uint donnees_a_envoyer=0;
uint donnees_a_envoyer_pic=0;

void i2c_annexe_gestion(){
    static enum {
        EMISSION_DONNEES_RPi,
        EMISSION_TEMPO_RPi,
        RECEPTION_DONNEES_RPi,
        EMISSION_PIC18F,
        EMISSION_TEMPO_PIC18F,
    } etat_i2c_annexe=EMISSION_PIC18F;

    enum i2c_resultat_t retour_i2c;
    static uint32_t temps;
    const uint32_t tempo = 1000;

    switch(etat_i2c_annexe){
        case EMISSION_DONNEES_RPi:
            if(donnees_a_envoyer){
                retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PICO_ANNEXE, ADRESSE_DEBUT_W, donnees_emission, TAILLE_DONNEES_EMISSION);
                if(retour_i2c == I2C_SUCCES){
                    etat_i2c_annexe = EMISSION_TEMPO_RPi;
                    donnees_a_envoyer=0;
                    temps = time_us_32();
                }else if(retour_i2c == I2C_ECHEC){
                    printf("ECHEC I2C address: %d\n", ADRESSE_PICO_ANNEXE);
                }
            }else{
                etat_i2c_annexe = EMISSION_TEMPO_RPi;
                temps = time_us_32();
            }
            break;
        
        case EMISSION_TEMPO_RPi:
            if(temps + tempo < time_us_32() ){
                etat_i2c_annexe = RECEPTION_DONNEES_RPi;
            }
            break;
        
        case RECEPTION_DONNEES_RPi:
            retour_i2c = i2c_lire_registre_nb(ADRESSE_PICO_ANNEXE, ADRESSE_DEBUT_R, donnees_reception, TAILLE_DONNEES_RECEPTION);
            if(retour_i2c == I2C_SUCCES){
                etat_i2c_annexe = EMISSION_PIC18F;
                temps = time_us_32();
            }else if(retour_i2c == I2C_ECHEC){
                printf("ECHEC I2C address: %d\n", ADRESSE_PICO_ANNEXE);
            }
            break;

        case EMISSION_PIC18F:
            if(donnees_a_envoyer_pic){
                retour_i2c = i2c_ecrire_registre_nb(ADRESSE_PIC18F4550, ADRESSE_PIC18F4550_DEBUT_W, 
                                donnees_emission_pic18f4550, TAILLE_DONNEES_PIC18F4550_EMISSION);
                if(retour_i2c == I2C_SUCCES){
                    etat_i2c_annexe = EMISSION_TEMPO_PIC18F;
                    temps = time_us_32();
                    donnees_a_envoyer_pic=0;
                    donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE]=0;
                }else if(retour_i2c == I2C_ECHEC){
                    printf("ECHEC I2C address: %d\n", ADRESSE_PIC18F4550);
                }
            }else{
                etat_i2c_annexe = EMISSION_TEMPO_PIC18F;
            }
            break;

        case EMISSION_TEMPO_PIC18F:
            if(temps + tempo < time_us_32() ){
                etat_i2c_annexe = EMISSION_DONNEES_RPi;
            }
            break;


    }

}

void i2c_annexe_couleur_balise(uint8_t couleur, uint16_t masque_led){
    donnees_emission[ADRESSE_COULEUR - ADRESSE_DEBUT_W] = couleur;
    donnees_emission[ADRESSE_MASQUE_LED_1 - ADRESSE_DEBUT_W] = (masque_led >> 8) & 0xFF;
    donnees_emission[ADRESSE_MASQUE_LED_2 - ADRESSE_DEBUT_W] = masque_led & 0xFF;
    donnees_a_envoyer=1;
}
void i2c_annexe_set_mode_VL53L8(enum validite_vl53l8_t mode){
    donnees_emission[ADRESSE_VL53L8_MODE - ADRESSE_DEBUT_W] = mode;
    donnees_a_envoyer=1;
}

void i2c_annexe_envoie_score(uint8_t score){
    donnees_emission[ADRESSE_SCORE - ADRESSE_DEBUT_W] = score;
    donnees_a_envoyer=1;
}

uint8_t i2c_annexe_get_contacteur_butee_A(void){
    return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] >> 3) & 0x01;
}

uint8_t i2c_annexe_get_contacteur_butee_C(void){
    return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] & 0x01);
}

uint8_t i2c_annexe_get_contacteur_longer_A(void){
    return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] >> 2) & 0x01;
}
uint8_t i2c_annexe_get_contacteur_longer_C(void){
    return (donnees_reception[ADRESSE_CONTACTEURS - ADRESSE_DEBUT_R] >> 1) & 0x01;
}

uint8_t i2c_annexe_get_tension_batterie(void){
    return donnees_reception[ADRESSE_TENSION_BATTERIE - ADRESSE_DEBUT_R];
}

void i2c_annexe_get_VL53L8(enum validite_vl53l8_t *validite, float * angle, float * distance){
    union float_or_octet_t donnee;

    *validite = donnees_reception[ADRESSE_VL53L8_VALIDITE - ADRESSE_DEBUT_R];

    donnee.octet[0]= donnees_reception[ADRESSE_VL53L8_ANGLE - ADRESSE_DEBUT_R];
    donnee.octet[1]= donnees_reception[ADRESSE_VL53L8_ANGLE+1 - ADRESSE_DEBUT_R];
    donnee.octet[2]= donnees_reception[ADRESSE_VL53L8_ANGLE+2 - ADRESSE_DEBUT_R];
    donnee.octet[3]= donnees_reception[ADRESSE_VL53L8_ANGLE+3 - ADRESSE_DEBUT_R];
    *angle = donnee.f_value;

    donnee.octet[0]= donnees_reception[ADRESSE_VL53L8_DISTANCE - ADRESSE_DEBUT_R];
    donnee.octet[1]= donnees_reception[ADRESSE_VL53L8_DISTANCE+1 - ADRESSE_DEBUT_R];
    donnee.octet[2]= donnees_reception[ADRESSE_VL53L8_DISTANCE+2 - ADRESSE_DEBUT_R];
    donnee.octet[3]= donnees_reception[ADRESSE_VL53L8_DISTANCE+3 - ADRESSE_DEBUT_R];
    *distance = donnee.f_value;
}


void i2c_annexe_get_distances(uint8_t *distance_capteur_cm){
    for (uint8_t capteur = 0; capteur < 12; capteur++){
        distance_capteur_cm[capteur] = donnees_reception[capteur + ADRESSE_VL53L1X - ADRESSE_DEBUT_R];
    }
}

/// @brief Envoie la consigne de position du servomoteur à la carte des servomoteurs
/// @param actionneur de 0 à 5, pour le "bras" correspondant".
/// @param pos_bras Code de position du bras, voir les #define BRAS_PLIE, ... définis plus haut ou dans le .h
/// @param pos_doigt Code de position du doigt, voir les #define DOIGT_LACHE, ... définis plus haut ou dans le .h
void i2c_annexe_actionneur_pot(int actionneur, uint8_t pos_bras, uint8_t pos_doigt){
    if(actionneur < 0 || actionneur > 5){
        printf("Erreur: i2c_annexe_actionneur_pot\n" );
        return;
    }
    donnees_emission_pic18f4550[actionneur] = (pos_bras << 2) | pos_doigt;
    donnees_a_envoyer_pic=1;
}

/// @brief Envoi l'ordre d'attraper une plance
/// @param action : PLANTE_BRAS_1 ou PLANTE_BRAS_6
void i2c_annexe_attrape_plante(uint8_t action){
    donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = 
        (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0xF0) | action;
    donnees_a_envoyer_pic=1;
}

void i2c_annexe_ouvre_doigt_plante(void){
    donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = 
        (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x10;
    donnees_a_envoyer_pic=1;
}

void i2c_annexe_ferme_doigt_plante(void){
    donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = 
        (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x20;
    donnees_a_envoyer_pic=1;
}

void i2c_annexe_mi_ferme_doigt_plante(void){
    donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = 
        (donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] & 0x0F) | 0x30;
    donnees_a_envoyer_pic=1;
}

void i2c_annexe_init(void){
    for(unsigned int actionneur=0; actionneur< NB_BRAS; actionneur++){
        i2c_annexe_actionneur_pot(actionneur, BRAS_PLIE, DOIGT_LACHE);
    }
}