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

#define MOTEUR_A 0
#define MOTEUR_B 1
#define MOTEUR_C 2

#define MOTEUR_A_VITESSE 6
#define MOTEUR_B_VITESSE 8
#define MOTEUR_C_VITESSE 10

#define MOTEUR_A_SENS 5
#define MOTEUR_B_SENS 7
#define MOTEUR_C_SENS 9

#define M1_SENS1 7
#define M1_SENS2 13
#define M1_VITESSE 27 //5B
#define M2_SENS1 10
#define M2_SENS2 5
#define M2_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(M1_SENS1);
	gpio_init(M1_SENS2);
	gpio_init(M2_SENS1);
	gpio_init(M2_SENS2);
	gpio_set_dir(M1_SENS1, GPIO_OUT);
	gpio_set_dir(M1_SENS2, GPIO_OUT);
	gpio_set_dir(M2_SENS1, GPIO_OUT);
	gpio_set_dir(M2_SENS2, GPIO_OUT);
	gpio_put(M1_SENS1, 0);
	gpio_put(M1_SENS2, 0);
	gpio_put(M2_SENS1, 0);
	gpio_put(M2_SENS2, 0);

	gpio_set_function(M1_VITESSE, GPIO_FUNC_PWM);
	gpio_set_function(M2_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_SetVitesse(MOTEUR_A, 0);
    Moteur_SetVitesse(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_SetVitesse(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(M1_SENS1, 0);
                gpio_put(M1_SENS2, 1);
            }else{
                gpio_put(M1_SENS1, 1);
                gpio_put(M1_SENS2, 0);
            }
            break;

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

    }

}

void Moteur_Stop(void){
    Moteur_SetVitesse(MOTEUR_A, 0);
    Moteur_SetVitesse(MOTEUR_B, 0);
}