/*****
 * Copyright (c) 2023 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/stdlib.h"
#include "hardware/pwm.h"
#include "hardware/adc.h"
#include <stdio.h>

#define MOTEUR1_PIN_SENS1 2
#define MOTEUR1_PIN_SENS2 3
#define MOTEUR1_PIN_ACTIVATION 0

#define MOTEUR2_PIN_SENS1 4
#define MOTEUR2_PIN_SENS2 5
#define MOTEUR2_PIN_ACTIVATION 1

int sens_moteur1 = 0;
int sens_moteur2 = 0;

void upload_motor(void);

void main(void)
{
    stdio_init_all();
	
	gpio_init(MOTEUR1_PIN_SENS1);
	gpio_set_dir(MOTEUR1_PIN_SENS1, GPIO_OUT);
	gpio_init(MOTEUR1_PIN_SENS2);
	gpio_set_dir(MOTEUR1_PIN_SENS2, GPIO_OUT);
	gpio_init(MOTEUR2_PIN_SENS1);
	gpio_set_dir(MOTEUR2_PIN_SENS1, GPIO_OUT);
	gpio_init(MOTEUR2_PIN_SENS2);
	gpio_set_dir(MOTEUR2_PIN_SENS2, GPIO_OUT);

	gpio_init(MOTEUR1_PIN_ACTIVATION);
	gpio_init(MOTEUR2_PIN_ACTIVATION);

	gpio_set_function(MOTEUR1_PIN_ACTIVATION, GPIO_FUNC_PWM);
	gpio_set_function(MOTEUR2_PIN_ACTIVATION, GPIO_FUNC_PWM);

	pwm_set_wrap(0, 1000);
	pwm_set_chan_level(0, PWM_CHAN_A, 800);

	pwm_set_enabled(0, true);

	sens_moteur1 = 1;
	sens_moteur2 = 1;

	upload_motor();

	while(1){
		tight_loop_contents();
	}
}

void upload_motor(void)
{
	switch(sens_moteur1)
	{
	case 0:
		gpio_put(MOTEUR1_PIN_SENS1, 0);
		gpio_put(MOTEUR1_PIN_SENS2, 0);
		break;

	case 1:
		gpio_put(MOTEUR1_PIN_SENS1, 1);
		gpio_put(MOTEUR1_PIN_SENS2, 0);
		break;
	
	case 2:
		gpio_put(MOTEUR1_PIN_SENS1, 0);
		gpio_put(MOTEUR1_PIN_SENS2, 1);
		break;
	}
	
	switch(sens_moteur2)
	{
	case 0:
		gpio_put(MOTEUR2_PIN_SENS1, 0);
		gpio_put(MOTEUR2_PIN_SENS2, 0);
		break;

	case 1:
		gpio_put(MOTEUR2_PIN_SENS1, 1);
		gpio_put(MOTEUR2_PIN_SENS2, 0);
		break;
	
	case 2:
		gpio_put(MOTEUR2_PIN_SENS1, 0);
		gpio_put(MOTEUR2_PIN_SENS2, 1);
		break;
	}

}