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

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

#ifdef ROBOT_PROPULSION_2026
    #define M1_VITESSE 2 //1A
    #define M1_SENS1 3
    #define M1_SENS2 4
    #define M2_VITESSE 6 //3A
    #define M2_SENS1 7
    #define M2_SENS2 8
#else
    #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
#endif

uint slice_moteur_A, slice_moteur_B, slice_moteur_C;
int moteur_a_pwm, moteur_b_pwm;

/// @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);
#ifdef ROBOT_PROPULSION_2026
	pwm_set_wrap(1, (uint16_t)65535);
	pwm_set_wrap(3, (uint16_t)65535);
	pwm_set_chan_level(1, PWM_CHAN_A, 0);
	pwm_set_chan_level(3, PWM_CHAN_A, 0);
	pwm_set_enabled(1, true);
	pwm_set_enabled(3, true);
#else
    pwm_set_wrap(4, (uint16_t)65535);
	pwm_set_wrap(5, (uint16_t)65535);
	pwm_set_chan_level(4, PWM_CHAN_B, 0);
	pwm_set_chan_level(5, PWM_CHAN_B, 0);
	pwm_set_enabled(4, true);
	pwm_set_enabled(5, true);
#endif
    Moteur_SetVitesse(MOTEUR_A, 0);
    Moteur_SetVitesse(MOTEUR_B, 0);
}

/// @brief Renvoie la commande signée sur 16 bits du PWM
/// @param moteur MOTEUR_A ou MOTEUR_B
/// @return 
int16_t Moteur_GetVitesse(enum t_moteur moteur){
    uint16_t u_vitesse;
    switch (moteur)
    {
    case MOTEUR_A:
        return moteur_a_pwm;
        break;
    
    case MOTEUR_B:
        return moteur_b_pwm;
        break;
    
    default:
        return 0;
        break;
    }
}


/// @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:
            
#ifdef ROBOT_PROPULSION_2026
            pwm_set_chan_level(1, PWM_CHAN_A, u_vitesse);
            if(vitesse > 0){
#else
            pwm_set_chan_level(5, PWM_CHAN_B, u_vitesse);
            if(vitesse < 0){
#endif
                gpio_put(M1_SENS1, 1);
                gpio_put(M1_SENS2, 0);
            }else{
                gpio_put(M1_SENS1, 0);
                gpio_put(M1_SENS2, 1);
            }
            break;

        case MOTEUR_B:
#ifdef ROBOT_PROPULSION_2026
            pwm_set_chan_level(3, PWM_CHAN_A, u_vitesse);
            if(vitesse < 0){
#else
            pwm_set_chan_level(4, PWM_CHAN_B, u_vitesse);
            if(vitesse > 0){
#endif
                gpio_put(M2_SENS1, 1);
	            gpio_put(M2_SENS2, 0);
            }else{
                gpio_put(M2_SENS1, 0);
	            gpio_put(M2_SENS2, 1);
            }
            break;

    }

}

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