/*****
 * 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>
#include "hardware/adc.h"
#define LED_VERTE 25
#define PIN_VITESSE_M1 2
#define PIN_SENS_A_M1 0
#define PIN_SENS_B_M1 1

#define PINCE_OUVERTE 0
#define PINCE_PLANTE 1
#define PINCE_POT 1

#define ASCENSEUR_BAS 0
#define ASCENSEUR_HAUT 1
#define ASCENSEUR_LACHE_POT_JARDINIERE 2
#define ASCENSEUR_LACHE_PLANTE 2



unsigned result0, result1;
int joystic_clicker;
int pince = PINCE_OUVERTE;
int ascenceur = ASCENSEUR_BAS;


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);

	gpio_set_function(PIN_VITESSE_M1, GPIO_FUNC_PWM);
	pwm_set_wrap(PIN_VITESSE_M1, 100);
	pwm_set_chan_level(PIN_VITESSE_M1, PWM_CHAN_B, 50);
    
}

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);



}
int Adc_Init()
{
	adc_init();
	adc_gpio_init(26);
	adc_gpio_init(27);
}
int AdcRead0()
{
	adc_select_input(0);
	uint16_t resultadc = adc_read();
	return resultadc;
}





int AdcRead1()
{
	adc_select_input(1);
	uint16_t resultadc = adc_read();
	return resultadc;
}



void init_bouton(uint8_t bouton){
	gpio_init(bouton);
	gpio_set_dir(bouton, GPIO_IN);
	gpio_pull_up(bouton);
}

int bouton_appui(uint8_t bouton){
	static uint8_t etat_bouton[32]={1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1, 1,1,1,1};
	if(gpio_get(bouton) == 1 && gpio_get(bouton) != etat_bouton[bouton]){
		etat_bouton[bouton] = gpio_get(bouton);
		return 1;
	}else{
		etat_bouton[bouton] = gpio_get(bouton);
	}
	return 0;

}


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

	// Communication
	communication_init();

	// Pour envoyer un message
	// communication_envoyer_message(message, 254);


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

	// ADC
	adc_init();
	adc_gpio_init(26);
	adc_gpio_init(27);

	// Boutons
	init_bouton(2);
	init_bouton(6);
	init_bouton(10);
	init_bouton(14);

	ascenceur = ASCENSEUR_BAS;
	pince = PINCE_OUVERTE;
	
	
	while (1) 
	{
		// Voie X
		result1 = AdcRead1()/16;
		// Voie Y
		result0 = AdcRead0()/16;

		message[0] = 255 - result0;
		message[1] = result1;
		
		//clic sur le joystic
		gpio_init(1);
		gpio_pull_up(1);
    	gpio_set_dir(1, GPIO_IN);
		joystic_clicker = gpio_get(1);
		if (!joystic_clicker)
		{
			if(result1 > 135){
				message[2] = 220;
			}else if (result1 < 122){
				message[2] = 30;
			}else{
				message[2] = 128;
			}
			message[1] = 128;
			message[0] = 128;
		}
		else
		{
			message[2] = 128;
		}

		// Pince
		// Bouton plante
		if(bouton_appui(6)){
			if(pince == PINCE_OUVERTE){
				pince = PINCE_PLANTE;
			}else{
				pince = PINCE_OUVERTE;
			}
		}
		// Bouton Pot
		if(bouton_appui(10)){
			if(pince == PINCE_OUVERTE){
				pince = PINCE_POT;
			}else{
				pince = PINCE_OUVERTE;
			}
		}

		// Ascenseur
		// Commande simple
		if(bouton_appui(2)){
			if(ascenceur == ASCENSEUR_BAS){
				ascenceur = ASCENSEUR_HAUT;
			}else{
				ascenceur = ASCENSEUR_BAS;
			}
		};
		// Commande évoluée
		if(bouton_appui(14)){
			if(ascenceur == ASCENSEUR_HAUT){
				if(pince == PINCE_PLANTE){
					ascenceur = ASCENSEUR_LACHE_PLANTE;
				}
				if(pince == PINCE_POT){
					ascenceur = ASCENSEUR_LACHE_POT_JARDINIERE;
				}
			}else{
				ascenceur = ASCENSEUR_HAUT;
			}
		}
		
		

		message [3] = pince;
		message[4] = ascenceur;


		printf(">x:%d\n", message[0]);
		printf(">Y:%d\n", message[1]);
		printf(">Rz:%d\n", message[2]);
		printf(">pince:%d\n", message[3]);
		printf(">ascenceur:%d\n", message[4]);
		printf(">result0:%d\n", result0);
		sleep_ms(25);
		communication_envoyer_message(message, 254);
	}
	M1_INITIALISE();
	while(1)
	{
		M1_AVANCE();
		sleep_ms(1000);
		M1_RECULE();
		sleep_ms(1000);
	
	}

}