#include "pico/stdlib.h"
#include "hardware/adc.h"
#include "communication.h"
#include <stdio.h>
#include "moteur.h"

// 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

// Convert te joystick value to an char value
uint8_t Conversions_joystick2uint8(uint16_t joystick_value);

void main(void)
{
	char message[256], reception[256];
	
	// Init "all"
    stdio_init_all();
	Init_motion_motor();
	communication_init();
	
	uint vitesse_x = 0;
	uint vitesse_y = 0;

	while(1){

		if (!(communication_read_message(reception) == I2C_ECHEC))
		{
			vitesse_x = reception[0]-128;
			vitesse_y = reception[1]-128;
		}
		else
		{
			vitesse_x = 0;
			vitesse_y = 0;
		}

		Motor1_speed(vitesse_x);
		Motor2_speed(vitesse_x);
		Motor3_speed(vitesse_y);
		Motor4_speed(vitesse_y);
	}
}



/// @brief Prend un nombre entre 0 et 4096, et sort un nombre entre 0 et 255
/// @return un nombre entre 0 et 255
uint8_t Conversions_joystick2uint8(uint16_t joystick_value)
{
	uint8_t valeur = joystick_value / 4;
	return valeur;
}