#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_TURBINE_PORTE 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 TAILLE_DONNEES_EMISSION 6
#define TAILLE_DONNEES_RECEPTION 15

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


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,
    } 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{
                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();
            }
            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_DONNEES_RPi;
                    temps = time_us_32();
                    donnees_a_envoyer_pic=0;
                    donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE]=0;
                }
            }else{
                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_active_turbine(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x01;
    donnees_a_envoyer=1;
}
void i2c_annexe_desactive_turbine(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xFE;
    donnees_a_envoyer=1;
}

void i2c_annexe_ouvre_porte(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9;
    donnees_a_envoyer=1;
}
void i2c_annexe_ferme_porte(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9;
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= (1 << 1);
    donnees_a_envoyer=1;
}
void i2c_annexe_mi_ferme_porte(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF9;
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= (2 << 1);
    donnees_a_envoyer=1;
}

void i2c_annexe_active_propulseur(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x08;
    donnees_a_envoyer=1;
}
void i2c_annexe_desactive_propulseur(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xF7;
    donnees_a_envoyer=1;
}

void i2c_annexe_active_deguisement(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x10;
    donnees_a_envoyer=1;
}
void i2c_annexe_desactive_deguisement(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xEF;
    donnees_a_envoyer=1;
}

void i2c_annexe_deplie_bras(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] |= 0x20;
    donnees_a_envoyer=1;
}
void i2c_annexe_plie_bras(void){
    donnees_emission[ADRESSE_TURBINE_PORTE - ADRESSE_DEBUT_W] &= 0xDF;
    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_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 1 à 6, 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 < 1 || actionneur > 6){
        printf("Erreur: i2c_annexe_actionneur_pot\n" );
        return;
    }
    donnees_emission_pic18f4550[actionneur-1] = (pos_bras << 2) | pos_doigt;
    donnees_a_envoyer_pic=1;
}

void i2c_annexe_attrape_plante(uint8_t action){
    donnees_emission_pic18f4550[ADRESSE_ACTION_PINCE] = action;
    donnees_a_envoyer_pic=1;
}

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