#include <stdio.h>
#include "pico/stdlib.h"
#include "hardware/pio.h"
#include "hardware/timer.h"
#include "QEI.h"
#include "quadrature_encoder.pio.h"


/*** C'est ici que se fait la conversion en mm 
 * ***/

// Roues 60 mm de diamètre, 188,5 mm de circonférence
// Réduction Moteur 30:1
// Réduction poulie 16:12
// Nombre d'impulsions par tour moteur : 200
// Nombre d'impulsions par tour réducteur : 6000
// Nombre d'impulsions par tour de roue : 8000
// Impulsion / mm : 42,44

#define IMPULSION_PAR_MM_50_1 (12.45f)
#define IMPULSION_PAR_MM_30_1 (7.47f)

float impulsion_par_mm;


struct QEI_t QEI_A, QEI_B;

bool QEI_est_init = false;

PIO pio_QEI = pio0;


void QEI_init(int identifiant){
    // Initialisation des 3 modules QEI
    // Chaque module QEI sera dans une machine à état du PIO 0
    if(!QEI_est_init){
        // Offset le début du programme
        // Si ce n'est pas 0, le programme ne marchera pas
        uint offset = pio_add_program(pio_QEI, &quadrature_encoder_program); 
        if(offset != 0){
            printf("PIO init error: offset != 0");
        }
        // Initialisation des "machines à états" :
        // QEI1 : broche 11 et 12 - pio : pio0, sm : 0, Offset : 0, GPIO 11 et 12, clock div : 0 pour commencer
        quadrature_encoder_program_init(pio_QEI, 0, offset, 11, 0);
        // QEI2 : broche 2 et 3 - pio : pio0, sm : 1, Offset : 0, GPIO 2 et 3, clock div : 0 pour commencer
        quadrature_encoder_program_init(pio_QEI, 1, offset, 2, 0);

        QEI_A.value=0;
        QEI_B.value=0;
        QEI_est_init=true;
    }
    impulsion_par_mm = IMPULSION_PAR_MM_50_1;
    if(identifiant == 0 || identifiant >= 4){
        impulsion_par_mm = IMPULSION_PAR_MM_30_1;
    }

}

/// @brief Lit les modules QEI et stock l'écart en cette lecture et la lecture précédente.
void QEI_update(void){
    
    int old_value;

    old_value = QEI_A.value;
    QEI_A.value = quadrature_encoder_get_count(pio_QEI, 0);
    QEI_A.delta = QEI_A.value - old_value;

    old_value = QEI_B.value;
    QEI_B.value = quadrature_encoder_get_count(pio_QEI, 1);
    QEI_B.delta = QEI_B.value - old_value;

}

/// @brief Renvoi le nombre d'impulsion du module QEI depuis la lecture précédente
/// Les signe sont inversés (sauf A) car le reducteur inverse le sens de rotation.
/// Attention, le signe du QEI_A est inversé par rapport aux autres à cause d'un soucis sur la carte électornique
/// @param  qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
/// @return Nombre d'impulsion calculé lors du dernier appel de la function QEI_Update()
int QEI_get(enum QEI_name_t qei){
    switch (qei)
    {
    case QEI_A_NAME:
        return QEI_A.delta;
        break;

    case QEI_B_NAME:
        return -QEI_B.delta;
        break;
    
    default:
        break;
    }
}

/// @brief Renvoi la distance parcourue en mm depuis la lecture précédente
/// @param  qei : Nom du module à lire (QEI_A_NAME, QEI_B_NAME ou QEI_C_NAME)
/// @return la distance parcourue en mm calculée lors du dernier appel de la function QEI_Update()
float QEI_get_mm(enum QEI_name_t qei){
    return ((float) QEI_get(qei)) / impulsion_par_mm;
}