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

// Pins
#define PIN_BUTTON_CLIC_JOYSTICK 1 // Unused
#define PIN_BUTTON_ELEVATOR 2
#define PIN_BUTTON_LEFT_ROTATION 5
#define PIN_BUTTON_POT 6
#define PIN_BUTTON_RIGHT_ROTATION 9
#define PIN_BUTTON_CLAW 10
#define PIN_BUTTON_MID_ELEVATOR 13
#define PIN_LED_VERTE 25

// Différents états de la pince
#define PINCE_OUVERTE 0
#define PINCE_PLANTE 1
#define PINCE_POT 2

// Différent états de l'ascenseur
#define ASCENSEUR_BAS 0
#define ASCENSEUR_HAUT 1
#define ASCENSEUR_LACHE_POT_JARDINIERE 2
#define ASCENSEUR_LACHE_PLANTE 2

#define STATU_PINCE_LACHE 0
#define STATU_PINCE_TIENT 1

#define STATU_CAME_NEUTRE 0
#define STATU_CAME_POUSSE 1

#define STATU_BANDEROLLE_NEUTRE 0
#define STATU_BANDEROLLE_DEPLIE 1



unsigned result0, result1;
int joystic_clicker;
int pince;
int ascenseur;

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

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

int Adc_Init()
{
	adc_init();
	adc_gpio_init(26);
	adc_gpio_init(27);
}

int Adc_Read(short input)
{
	adc_select_input(input);
	uint16_t result_adc = adc_read();
	return result_adc;
}

bool bouton_appui(uint8_t button)
{
	bool retour;
	

	bool is_button_pressed = gpio_get(button);
	
	retour =  is_button_pressed && (is_button_pressed != etat_bouton[button]);
	etat_bouton[button] = is_button_pressed;
	return retour;
}

void main()
{
	stdio_init_all();

	// Communication
	char message [256];
	char statu_pince = STATU_PINCE_LACHE;
	char statu_came = STATU_CAME_NEUTRE;
	char statu_banderolle = STATU_BANDEROLLE_NEUTRE;
	communication_init();

	// ADC
	Adc_Init();

	// Boutons
	init_bouton(PIN_BUTTON_ELEVATOR);
	init_bouton(PIN_BUTTON_LEFT_ROTATION);
	init_bouton(PIN_BUTTON_POT);
	init_bouton(PIN_BUTTON_RIGHT_ROTATION);
	init_bouton(PIN_BUTTON_CLAW);
	init_bouton(PIN_BUTTON_MID_ELEVATOR);

	ascenseur = ASCENSEUR_BAS;
	pince = PINCE_OUVERTE;
	
	gpio_init(PIN_LED_VERTE);
   	gpio_set_dir(PIN_LED_VERTE, GPIO_OUT);
	gpio_put(PIN_LED_VERTE, true);

	while(true) 
	{
		// Voie X
		result1 = Adc_Read(1) / 16;
		// Voie Y
		result0 = Adc_Read(0) / 16;

		message[0] = result0;
		message[1] = 255 - result1;

		// Set rotation speed according to DA rotations buttons
		message[2] = 128 - 95 * (gpio_get(PIN_BUTTON_LEFT_ROTATION) - gpio_get(PIN_BUTTON_RIGHT_ROTATION));
		message[3] = statu_pince;
		message[4] = statu_banderolle;
		message[5] = statu_came;
/*
		// Voie X
		result1 = AdcRead(1) / 16;
		// Voie Y
		result0 = AdcRead(0) / 16;

		message[0] = result0;
		message[1] = 255 - result1;
		
		//clic sur le joystic
		joystic_clicker = gpio_get(PIN_BUTTON_CLIC_JOYSTICK);

		if (!joystic_clicker)
		{
			if(result1 > 135)
			{
				message[2] = 30;
			}
			else if (result1 < 122)
			{
				message[2] = 220;
			}else
			{
				message[2] = 128;
			}
			message[1] = 128;
			message[0] = 128;
		}
		else
		{
			message[2] = 128;
		}
*/

		// Pince
		// Bouton plante
		if(bouton_appui(6)){
			if(statu_pince == STATU_PINCE_TIENT){
				statu_pince = STATU_PINCE_LACHE;
			}else{
				statu_pince = STATU_PINCE_TIENT;
			}
		}else{
			
			
		}

		// Bouton Pot
		if(bouton_appui(10)){
			if(statu_came == STATU_CAME_POUSSE){
				statu_came = STATU_CAME_NEUTRE;
			}else{
				statu_came = STATU_CAME_POUSSE;
			}

		}

		// Ascenseur
		// Commande simple
		if(bouton_appui(2))
		{
			if(statu_banderolle == STATU_BANDEROLLE_DEPLIE){
				statu_banderolle = STATU_BANDEROLLE_NEUTRE;
			}else{
				statu_banderolle = STATU_BANDEROLLE_DEPLIE;
			}
		}

		// Commande évoluée
		if(bouton_appui(14))
		{
			if(ascenseur == ASCENSEUR_HAUT)
			{
				if(pince == PINCE_PLANTE)
				{
					ascenseur = ASCENSEUR_LACHE_PLANTE;
				}
				if(pince == PINCE_POT)
				{
					ascenseur = ASCENSEUR_LACHE_POT_JARDINIERE;
				}
				// Optimisation : lorsque la pince est fermée et l'ascenseur au niveau milieu, la pince s'ouvre, l'ascenseur monte et le robot recule
			}
			else if((ascenseur == ASCENSEUR_LACHE_POT_JARDINIERE && pince == PINCE_POT) ||
					(ascenseur == ASCENSEUR_LACHE_PLANTE         && pince == PINCE_PLANTE))
			{
				pince = PINCE_OUVERTE;
				message [3] = pince;
				communication_envoyer_message(message, 254);

				sleep_ms(500);

				ascenseur = ASCENSEUR_HAUT;
				message[4] = ascenseur;
				communication_envoyer_message(message, 254);

				sleep_ms(500);

				message[0] = 20;
				communication_envoyer_message(message, 254);
				sleep_ms(1000);
			}
			else
			{
				ascenseur = ASCENSEUR_HAUT;
			}
		}		


		//printf(">x:%d\n", message[0]);
		//printf(">Y:%d\n", message[1]);
		printf(">pince:%d\n", message[2]);
		printf(">banderole:%d\n", message[3]);
		printf(">ascenseur:%d\n", message[4]);
		printf(">came:%d\n", message[5]);
		//printf(">c10_1:%d\n", gpio_get(10));
		//printf(">b10:%d\n", bouton_appui(10));
		//printf(">etat_10:%d\n", etat_bouton[10]);
		

		communication_envoyer_message(message, 254);

		sleep_ms(25);
	}
}