#include "pico/stdlib.h"
#include "SelectionCapteur.h"

#define PIN_ADRESSE_A0 5
#define PIN_ADRESSE_A1 4
#define PIN_ADRESSE_A2 3
#define PIN_ADRESSE_A3 2

#define PIN_COM 6

void Selection_capteur_init_pin_low(uint pin);

void Selection_capteur_init(void){

    Selection_capteur_init_pin_low(PIN_COM);
    Selection_capteur_init_pin_low(PIN_ADRESSE_A0);
    Selection_capteur_init_pin_low(PIN_ADRESSE_A1);
    Selection_capteur_init_pin_low(PIN_ADRESSE_A2);
    Selection_capteur_init_pin_low(PIN_ADRESSE_A3);
    Selection_capteur_deselect();

}

/// @brief Désactive le capteur en question
/// @param capteur capteur, numéroté de 1 à 12
void Selection_capteur_select(uint32_t capteur){
    uint32_t io_value;
    capteur = capteur - 1;
    /*gpio_put(PIN_ADRESSE_A0, capteur & 0x01);
    gpio_put(PIN_ADRESSE_A1, (capteur & 0x02) >> 1);
    gpio_put(PIN_ADRESSE_A2, (capteur & 0x04) >> 2);
    gpio_put(PIN_ADRESSE_A3, (capteur & 0x08) >> 3);*/

    io_value = 0;
    io_value |= ((capteur & 0x08) >> 3) << 2;
    io_value |= ((capteur & 0x04) >> 2) << 3;
    io_value |= ((capteur & 0x02) >> 1) << 4;
    io_value |= (capteur & 0x01) << 5;

    printf("io_value : %x, capteur : %d\n", io_value, capteur);

    gpio_put_masked(0b111100, io_value);
}

void Selection_capteur_init_pin_low(uint pin){
    gpio_init(pin);
    gpio_set_dir(pin, GPIO_OUT);
    gpio_put(pin, 0);
}

void Selection_capteur_deselect(){
    Selection_capteur_select(15);
}