#include "driver/pcnt.h"

#define ENCODER_1_A GPIO_NUM_32
#define ENCODER_1_B GPIO_NUM_33
#define ENCODER_2_A GPIO_NUM_25
#define ENCODER_2_B GPIO_NUM_26
#define ENCODER_3_A GPIO_NUM_27
#define ENCODER_3_B GPIO_NUM_14

#define NUM_ENCODERS 3

pcnt_unit_t pcnt_units[NUM_ENCODERS] = {PCNT_UNIT_0, PCNT_UNIT_1, PCNT_UNIT_2};

// Positions des encodeurs
volatile int32_t encoder_positions[NUM_ENCODERS] = {0, 0, 0};

void setup_encoders() {
    int encoder_pins[NUM_ENCODERS][2] = {
        {ENCODER_1_A, ENCODER_1_B},
        {ENCODER_2_A, ENCODER_2_B},
        {ENCODER_3_A, ENCODER_3_B}
    };

    for (int i = 0; i < NUM_ENCODERS; i++) {
        pcnt_config_t pcnt_config = {
            .pulse_gpio_num = encoder_pins[i][0],  // Signal A
            .ctrl_gpio_num = encoder_pins[i][1],  // Signal B
            .channel = PCNT_CHANNEL_0,
            .unit = pcnt_units[i],
            .pos_mode = PCNT_COUNT_INC,           // Incrémenter sur front montant de A
            .neg_mode = PCNT_COUNT_DEC,           // Décrémenter sur front descendant de A
            .lctrl_mode = PCNT_MODE_REVERSE,      // Si B = 1, inverser le comptage
            .hctrl_mode = PCNT_MODE_KEEP,         // Si B = 0, garder le sens
            .counter_h_lim = 32767,               // Limite haute
            .counter_l_lim = -32768               // Limite basse
        };

        // Configurer le PCNT pour cet encodeur
        pcnt_unit_config(&pcnt_config);

        // Activer le filtre matériel pour ignorer les rebonds (filtre faible pour signaux rapides)
        pcnt_set_filter_value(pcnt_units[i], 5);  // 5 µs (ajustez selon les besoins)
        pcnt_filter_enable(pcnt_units[i]);

        // Réinitialiser le compteur
        pcnt_counter_clear(pcnt_units[i]);

        // Démarrer le compteur
        pcnt_counter_resume(pcnt_units[i]);
    }
}
void read_encoders() {
    for (int i = 0; i < NUM_ENCODERS; i++) {
        int16_t count;
        pcnt_get_counter_value(pcnt_units[i], &count);

        // Accumuler la position (en cas de débordement)
        encoder_positions[i] += count;

        // Réinitialiser le compteur
        pcnt_counter_clear(pcnt_units[i]);
    }
}
void setup() {
    Serial.begin(115200);
    setup_encoders();
    Serial.println("Encoders initialized for high-speed signals.");
}

void loop() {
    read_encoders();

    // Afficher les positions des encodeurs
    Serial.printf("Encoder 1: %d, Encoder 2: %d, Encoder 3: %d\n",
                  encoder_positions[0], encoder_positions[1], encoder_positions[2]);

    delay(10); // Ajustez pour réduire la fréquence d'affichage
}