#include "moteur.h"

// Define pins
#define MOTEUR1_PIN_SENS1 4
#define MOTEUR1_PIN_SENS2 5
#define MOTEUR1_PIN_ACTIVATION 0
#define MOTEUR2_PIN_SENS1 6
#define MOTEUR2_PIN_SENS2 7
#define MOTEUR2_PIN_ACTIVATION 1
#define MOTEUR3_PIN_SENS1 11
#define MOTEUR3_PIN_SENS2 10
#define MOTEUR3_PIN_ACTIVATION 2
#define MOTEUR4_PIN_SENS1 8
#define MOTEUR4_PIN_SENS2 11
#define MOTEUR4_PIN_ACTIVATION 3

// Init all motion motors pins
void Init_motion_motor(void)
{
	// Init 1/0 pin for control motion motors
	gpio_init(MOTEUR1_PIN_SENS1);
	gpio_init(MOTEUR1_PIN_SENS2);
	gpio_init(MOTEUR2_PIN_SENS1);
	gpio_init(MOTEUR2_PIN_SENS2);
	gpio_init(MOTEUR3_PIN_SENS1);
	gpio_init(MOTEUR3_PIN_SENS2);
	gpio_init(MOTEUR4_PIN_SENS1);
	gpio_init(MOTEUR4_PIN_SENS2);
	gpio_set_dir(MOTEUR1_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR1_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR2_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR2_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR3_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR3_PIN_SENS2, GPIO_OUT);
	gpio_set_dir(MOTEUR4_PIN_SENS1, GPIO_OUT);
	gpio_set_dir(MOTEUR4_PIN_SENS2, GPIO_OUT);

	// Set direction to 0 (disactivate)
	gpio_put(MOTEUR1_PIN_SENS1, 0);
	gpio_put(MOTEUR1_PIN_SENS2, 0);
	gpio_put(MOTEUR2_PIN_SENS1, 0);
	gpio_put(MOTEUR2_PIN_SENS2, 0);
	gpio_put(MOTEUR3_PIN_SENS1, 0);
	gpio_put(MOTEUR3_PIN_SENS2, 0);
	gpio_put(MOTEUR4_PIN_SENS1, 0);
	gpio_put(MOTEUR4_PIN_SENS2, 0);

	// Init pwm pins for motion motors
	gpio_init(MOTEUR1_PIN_ACTIVATION);
	gpio_init(MOTEUR2_PIN_ACTIVATION);
	gpio_init(MOTEUR3_PIN_ACTIVATION);
	gpio_init(MOTEUR4_PIN_ACTIVATION);
	gpio_set_function(MOTEUR1_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR2_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR3_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR4_PIN_ACTIVATION, GPIO_FUNC_PWM);

	// Set wrap of pwm slices
	pwm_set_wrap(0, 127);
	pwm_set_wrap(1, 127);

	// Active all pwm slices
	pwm_set_enabled(0, true);
	pwm_set_enabled(1, true);

	// Set speed to 0
	pwm_set_chan_level(0, PWM_CHAN_A, 0);
	pwm_set_chan_level(0, PWM_CHAN_B, 0);
	pwm_set_chan_level(1, PWM_CHAN_A, 0);
	pwm_set_chan_level(1, PWM_CHAN_B, 0);

}

// Set motor 1 speed forward
void Motor1_forward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_A, speed);
	gpio_put(MOTEUR1_PIN_SENS1, 0);
	gpio_put(MOTEUR1_PIN_SENS2, 1);
}

// Set motor 1 speed backward
void Motor1_backward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_A, speed);
	gpio_put(MOTEUR1_PIN_SENS1, 1);
	gpio_put(MOTEUR1_PIN_SENS2, 0);
}

// Set motor 1 speed and direction (negative value : backward / positive value : forward)
void Motor1_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor1_backward(speed);
	}
	else
	{
		Motor1_forward(speed);
	}
}

// Set motor 2 speed forward
void Motor2_forward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_B, speed);
	gpio_put(MOTEUR2_PIN_SENS1, 0);
	gpio_put(MOTEUR2_PIN_SENS2, 1);
}

// Set motor 2 speed backward
void Motor2_backward(int speed)
{
	pwm_set_chan_level(0, PWM_CHAN_B, speed);
	gpio_put(MOTEUR2_PIN_SENS1, 1);
	gpio_put(MOTEUR2_PIN_SENS2, 0);
}

// Set motor 2 speed and direction (negative value : backward / positive value : forward)
void Motor2_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor2_backward(speed);
	}
	else
	{
		Motor2_forward(speed);
	}
}

// Set motor 3 speed forward
void Motor3_forward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_A, speed);
	gpio_put(MOTEUR3_PIN_SENS1, 0);
	gpio_put(MOTEUR3_PIN_SENS2, 1);
}

// Set motor 3 speed backward
void Motor3_backward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_A, speed);
	gpio_put(MOTEUR3_PIN_SENS1, 1);
	gpio_put(MOTEUR3_PIN_SENS2, 0);
}

// Set motor 3 speed and direction (negative value : backward / positive value : forward)
void Motor3_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor3_backward(speed);
	}
	else
	{
		Motor3_forward(speed);
	}
}

// Set motor 4 speed forward
void Motor4_forward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_B, speed);
	gpio_put(MOTEUR4_PIN_SENS1, 0);
	gpio_put(MOTEUR4_PIN_SENS2, 1);
}

// Set motor 4 speed backward
void Motor4_backward(int speed)
{
	pwm_set_chan_level(1, PWM_CHAN_B, speed);
	gpio_put(MOTEUR4_PIN_SENS1, 1);
	gpio_put(MOTEUR4_PIN_SENS2, 0);
}

// Set motor 4 speed and direction (negative value : backward / positive value : forward)
void Motor4_speed(int speed)
{
	if(speed < 0)
	{
		speed = -speed;
		Motor4_backward(speed);
	}
	else
	{
		Motor4_forward(speed);
	}
}