#include "hardware/pwm.h"
#include "Moteurs.h"

#define MOTEUR_A_SENS1 7
#define MOTEUR_A_SENS2 13
#define MOTEUR_A_VITESSE 27 //5B
#define MOTEUR_B_SENS1 10
#define MOTEUR_B_SENS2 5
#define MOTEUR_B_VITESSE 9 //4B

uint slice_moteur_A, slice_moteur_B, slice_moteur_C;

/// @brief Initialisation les entrées / sorties requises pour les moteurs
void Moteur_init(){
    gpio_init(MOTEUR_A_SENS1);
	gpio_init(MOTEUR_A_SENS2);
	gpio_init(MOTEUR_B_SENS1);
	gpio_init(MOTEUR_B_SENS2);
	gpio_set_dir(MOTEUR_A_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR_A_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR_B_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR_B_SENS2, GPIO_OUT);
	gpio_put(MOTEUR_A_SENS1, 0);
	gpio_put(MOTEUR_A_SENS2, 0);
	gpio_put(MOTEUR_B_SENS1, 0);
	gpio_put(MOTEUR_B_SENS2, 0);

	gpio_set_function(MOTEUR_A_VITESSE, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR_B_VITESSE, GPIO_FUNC_PWM);
	pwm_set_wrap(5, (uint16_t)65535);
	pwm_set_wrap(4, (uint16_t)65535);
	pwm_set_chan_level(5, PWM_CHAN_B, 0);
	pwm_set_chan_level(4, PWM_CHAN_B, 0);
	pwm_set_enabled(5, true);
	pwm_set_enabled(4, true);

    Moteur_set_commande(MOTEUR_A, 0);
    Moteur_set_commande(MOTEUR_B, 0);
}


/// @brief Configure le PWM et la broche de sens en fonction de la vitesse et du moteur
/// @param moteur : Moteur (voir enum t_moteur)
/// @param vitesse : Vitesse entre -32767 et 32767
void Moteur_set_commande(enum t_moteur moteur, int16_t vitesse ){
    uint16_t u_vitesse;

    // Le PWM accepte 16 bits de résolution, on se remet sur 16 bits (et non sur 15 + signe)
    if (vitesse < 0){
        u_vitesse = -vitesse;
    }else{
        u_vitesse = vitesse;
    }
    u_vitesse = u_vitesse * 2;

    switch(moteur){
        case MOTEUR_A:
            pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse);
            if(vitesse > 0){
                gpio_put(MOTEUR_A_SENS1, 0);
                gpio_put(MOTEUR_A_SENS2, 1);
            }else{
                gpio_put(MOTEUR_A_SENS1, 1);
                gpio_put(MOTEUR_A_SENS2, 0);
            }
            break;

        case MOTEUR_B:
            pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
            if(vitesse < 0){
                gpio_put(MOTEUR_B_SENS1, 0);
	            gpio_put(MOTEUR_B_SENS2, 1);
            }else{
                gpio_put(MOTEUR_B_SENS1, 1);
	            gpio_put(MOTEUR_B_SENS2, 0);
            }
            break;

    }

}

void Moteur_stop(void){
    Moteur_set_commande(MOTEUR_A, 0);
    Moteur_set_commande(MOTEUR_B, 0);
}