#include <stdint.h>
#include "Asser_Moteurs.h"
#include "Plateforme.h"

#define NB_MOTEURS 2

float consigne_mm_s[NB_MOTEURS]; // Consigne de vitesse (en mm/s)
float commande_I[NB_MOTEURS]; // Terme integral
int asser_actif[NB_MOTEURS];

void AsserMoteur_Init(){
    QEI_init();
    Moteur_Init();
    for(unsigned int i =0; i< NB_MOTEURS; i ++){
        commande_I[i] = 0;
        consigne_mm_s[i] = 0;
        asser_actif[i] = 0;
    }
}

/// @brief Défini une consigne de vitesse pour le moteur indiqué.
/// @param  moteur : Moteur à asservir
/// @param _consigne_mm_s : consigne de vitesse en mm/s
void AsserMoteur_setConsigne_mm_s(enum t_moteur moteur, float _consigne_mm_s){
    consigne_mm_s[moteur] = _consigne_mm_s;
    asser_actif[moteur] = 1;

}

/// @brief Envoie la consigne du moteur
/// @param  moteur : Moteur à asservir
float AsserMoteur_getConsigne_mm_s(enum t_moteur moteur){
    return consigne_mm_s[moteur];
}



float AsserMoteur_getVitesse_mm_s(enum t_moteur moteur, int step_ms){
    enum QEI_name_t qei;
    float distance, temps;
    switch (moteur)
    {
    case MOTEUR_A: qei = QEI_A_NAME; break;
    case MOTEUR_B: qei = QEI_B_NAME; break;
    default: break;
    }
    distance = QEI_get_mm(qei);
    temps = step_ms / 1000.0f;

    return distance / temps;
}

/// @brief Indique si le robot est à l'arrêt
/// @param step_ms : pas de temps (utilisé pour déterminer les vitesses)
/// @return 1 si le robot est immobile, 0 s'il est en mouvement.
uint32_t AsserMoteur_RobotImmobile(int step_ms){
    const float seuil_vitesse_immobile_mm_s = 0.1;
    if(AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms) < seuil_vitesse_immobile_mm_s &&
            AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) < seuil_vitesse_immobile_mm_s ){
        return 1;
    }
    return 0;
}

void AsserMoteurs_stop(void){
    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
    AsserMoteur_setConsigne_mm_s(MOTEUR_A, 0);
}

/// @brief Fonction d'asservissement des moteurs, à appeler périodiquement
/// @param step_ms 
void AsserMoteur_Gestion(int step_ms){
    // Pour chaque moteur
    for(enum t_moteur moteur=MOTEUR_A; moteur<NB_MOTEURS; moteur++ ){
        if(asser_actif[moteur] == 1){
            float erreur; // Erreur entre la consigne et la vitesse actuelle
            float commande_P; // Terme proportionnel
            float commande;
            
            // Calcul de l'erreur
            erreur = consigne_mm_s[moteur] - AsserMoteur_getVitesse_mm_s(moteur, step_ms);

            // Calcul du terme propotionnel
            commande_P = erreur * ASSERMOTEUR_GAIN_P;

            // Calcul du terme integral
            commande_I[moteur] = commande_I[moteur] + (erreur * ASSERMOTEUR_GAIN_I * step_ms);

            commande = commande_P + commande_I[moteur];

            //Saturation de la commande
            if(commande > 32760) {commande = 32760;}
            if(commande < -32760) {commande = -32760;}

            Moteur_SetVitesse(moteur, commande);
        }

    }
}