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

#define LED_VERTE 25
#define PIN_VITESSE_M1 2
#define PIN_SENS_A_M1 0
#define PIN_SENS_B_M1 1

// Juste pour information - les broches sont re-définies dans i2c_slave
#define I2C0_SDA_PIN 16
#define I2C0_SCL_PIN 17

// Juste pour information - les broches sont re-définies dans i2c_master
#define I2C1_SDA_PIN 18
#define I2C1_SCL_PIN 19

int M1_INITIALISE()
{
	gpio_init(PIN_VITESSE_M1);
	gpio_init(PIN_SENS_A_M1);
	gpio_init(PIN_SENS_B_M1);

    gpio_set_dir(PIN_VITESSE_M1, GPIO_OUT);
	gpio_set_dir(PIN_SENS_A_M1, GPIO_OUT);
	gpio_set_dir(PIN_SENS_B_M1, GPIO_OUT);
    
}

int M1_AVANCE()
{

	gpio_put(PIN_VITESSE_M1, 1);
	gpio_put(PIN_SENS_A_M1, 1);
	gpio_put(PIN_SENS_B_M1, 0);



}

int M1_RECULE()
{

	gpio_put(PIN_VITESSE_M1, 1);
	gpio_put(PIN_SENS_A_M1, 0);
	gpio_put(PIN_SENS_B_M1, 1);



}







void main()
{
	char message [256];
	stdio_init_all();


	// CLignottement LED
	gpio_set_function(LED_VERTE, GPIO_FUNC_PWM);
	pwm_set_wrap(4, 100);
	pwm_set_chan_level(4, PWM_CHAN_B, 25);
	pwm_set_enabled(4, true);

	//Moteur 1

	M1_INITIALISE();

	// Exemple de communication entre 2 Rpi Pico
	/*while(1){

		printf("Envoi message\n");
		communication_envoyer_message("Bonjour !\n", 11);

		printf("Lire message\n");
		if (communication_lire_message(message) == I2C_ECHEC){
			printf("Echec de la lecture du message\n");
		}else{
			printf("Succes\n");
			printf("%s",message);
		}
		
		sleep_ms(1000);
	}*/



	while(1)
	{

		M1_AVANCE();
		sleep_ms(3000);
		M1_RECULE();
		sleep_ms(3000);
	
	}

}