#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

// Allumage de la led verte
#define PIN_LED_VERTE 25

void main(void)
{
	char message[256], reception[256];
	
	// Init "all"
    stdio_init_all();
	Init_motion_motor();
	communication_init();
	
	gpio_init(PIN_LED_VERTE);
	gpio_set_function(PIN_LED_VERTE, GPIO_FUNC_PWM);
	pwm_set_clkdiv(4, 100);
	pwm_set_wrap(4, 100);
	pwm_set_enabled(4, true);
	pwm_set_chan_level(4, PWM_CHAN_B, 100);

	int vitesse_angle = 0;
	int vitesse_m1 = 0;
	int vitesse_m2 = 0;
	int vitesse_m3 = 0;
	int vitesse_m4 = 0;

	while(1){

		if (!(communication_read_message(reception) == I2C_ECHEC))
		{
			vitesse_m1 = reception[0] - 128;
			vitesse_m2 = reception[0] - 128;
			vitesse_m3 = reception[1] - 128;
			vitesse_m4 = reception[1] - 128;

			vitesse_angle = reception[2] - 128;

			switch(reception[3]){
				case 0: Servo_pince_ouverte(); break;
				case 1: Servo_pince_plante(); break;
				case 2: Servo_pince_pot(); break;
			}

			switch(reception[4]){
				case 0: Servo_ascenseur_bas(); break;
				case 1: Servo_ascenseur_haut(); break;
				case 2: Servo_ascenseur_lache_pot_jardiniere(); break;
				case 3: Servo_ascenseur_lache_plante(); break;
			}

		}
		else
		{
			vitesse_angle = 0;
			vitesse_m1 = 0;
			vitesse_m2 = 0;
			vitesse_m3 = 0;
			vitesse_m4 = 0;

			pwm_set_chan_level(4, PWM_CHAN_B, 10);
		
		}

		if(vitesse_angle != 0)
		{
			vitesse_m1 = vitesse_angle;
			vitesse_m2 = -vitesse_angle;
			vitesse_m3 = vitesse_angle;
			vitesse_m4 = -vitesse_angle;
		}

		printf(">vitesseM1:%d\n", vitesse_m1);
		printf(">vitesseM2:%d\n", vitesse_m2);
		printf(">vitesseM3:%d\n", vitesse_m3);
		printf(">vitesseM4:%d\n", vitesse_m4);

		Motor1_speed(vitesse_m1);
		Motor2_speed(vitesse_m2);
		Motor3_speed(vitesse_m3);
		Motor4_speed(vitesse_m4);

		
	}
}