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

#include "communication.h"
#include "messagerie.h"


void stdio_usb_out_chars(const char *buf, int length);

#define NB_MORCEAUX 100

absolute_time_t envoie_nb_char(int nb_char);
char chaine_envoie[10000];



void envoie_donnees(char * donnees, uint16_t length){
	for (int i=0; i<length; i++){
		putchar(donnees[i]);
	}
}

/// @brief Fonction pour envoyer une structure
/// @param donnees données à envoyer
/// @param size taille des données
void envoie_structure(const void * donnees, size_t size){
	const unsigned char *byte;
	// Entête du message : debut
	putchar(0xFF);
	putchar(0xFF);
	putchar(size + 1);
	// Entête du message : fin
	// Données
	for ( byte = donnees; size--; ++byte )                                     
	{   
		putchar(*byte);
		//printf("%2X ", *byte);
	}   
	// Caractère de fin
	putchar(0x00);
	printf("\n");
}

void main(void)
{
	absolute_time_t measured_time_us;
	absolute_time_t temps_moyen, temps_max, temps_m[NB_MORCEAUX];
	absolute_time_t current_time_us, start_time_us;
	absolute_time_t temps_message_detection;
	struct message_t message;
	struct message_requete_t message_requete_detection;
	unsigned char message_binaire[100];

	message_requete_detection.commande = 'd';
	message_requete_detection.id_carte = 'D';
	message_requete_detection.adresse_registre = 0;
	message_requete_detection.taille_donnees = 19;
	

	message_binaire[0]='r';
	message_binaire[1]='A';
	message_binaire[2]= 0;
	message_binaire[3]= 5;

	message_binaire[4]= 'a';
	message_binaire[5]= 'b';
	message_binaire[6]= 'c';
	message_binaire[7]= 'd';
	message_binaire[8]= 'e';

	message_binaire[9]= 0;

	struct message_applicatif_t message_applicatif;


	
	for(int i=0; i<10000; i++){
		/*j++;
		if(j>25){
			j=0;
		}
		chaine_envoie[i]= j + 'a';*/
		chaine_envoie[i]= i%26 + 'a';
	}
	int nb_ech = 10;
	int taille_morceau = 60;
	int nb_message=0;
	float f;
	stdio_init_all();
	sleep_ms(5000);

	printf(">Device 1 - demarrage\n");
	printf("struct float:%d\n", sizeof(f));
	printf("struct message applicatif:%d\n", sizeof(message_applicatif));


	gpio_init(PICO_DEFAULT_LED_PIN);
	gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
	gpio_put(PICO_DEFAULT_LED_PIN, 0);

	temps_message_detection = get_absolute_time();
	unsigned int temps_ms; 
	temps_ms = get_absolute_time() / 1000.;

	communication_init();
	

	while(1){
		/*
		if(get_absolute_time() - temps_message_detection > MESSAGE_TIMEOUT_US){
			start_time_us = get_absolute_time();
			envoie_structure(&message_requete_detection, sizeof(message_requete_detection) );
			temps_message_detection = get_absolute_time();
			current_time_us = get_absolute_time();
			//printf(">temps_emission:%lld\n", current_time_us - start_time_us);
		}

		start_time_us = get_absolute_time();*/
		communication_reception_message();
		
		if(messagerie_message_disponible()){
			uint8_t id_carte, registre;
			while(messagerie_message_disponible()){
				message = messagerie_get_message();
				nb_message++;

				if(message.type == 'b'){
					switch(message.donnees[0]){
						case 'r': // réception de données
							// Reception de données
							// données[0]: commande ('r' ici)
							// données[1]: id de la carte
							// données[2]: 1er registre
							// données[3 à tailles_données -1]: valeurs
							
							printf("type:%d %c\n", message.type, message.type);
							printf("id_carte:%d %c\n", message.id_carte, message.id_carte);
							for(int i=0; i< 4;i++){
								printf("%x ", message.donnees[i]);
							}
							printf("\n");
							memcpy(&message_applicatif, message.donnees, message.taille_donnees);

							printf("id_carte:%d %c\n", message_applicatif.id_carte, message_applicatif.id_carte);
							printf("commande:%d %c\n", message_applicatif.commande, message_applicatif.commande);
							printf("add_reg:%d\n", message_applicatif.adresse_registre);
							printf("taille_donnee:%d\n", message_applicatif.taille_donnees);

							if(message_applicatif.id_carte == USB_ID_PROPULSION){
								printf("registre:%d\n", message_applicatif.adresse_registre);
								if(message_applicatif.adresse_registre == REG_PROPULSION_POSITION){
									printf("x:%x, y:%x, rot:%x\n", message_applicatif.donnees_applicative.msg_propulsion_position.position_x_mm,
									message_applicatif.donnees_applicative.msg_propulsion_position.position_y_mm,
									message_applicatif.donnees_applicative.msg_propulsion_position.orientation_rad);
								}
							}

							id_carte = message.donnees[1];
							registre = message.donnees[2];
							if(id_carte == 'D'){
								temps_message_detection = get_absolute_time();
								if(message.donnees[4] > 0 && message.donnees[4] < 30){
									printf(">LED ON\n");
									gpio_put(PICO_DEFAULT_LED_PIN, 1);
								}else{
									printf(">LED OFF\n");
									gpio_put(PICO_DEFAULT_LED_PIN, 0);
								}
							}
							break;
					}
				}
			}
			current_time_us = get_absolute_time();
			//printf(">temps_reception:%lld\n", current_time_us - start_time_us);
			//printf(">nb_message:%u\n",nb_message);
			
		}
		
		if(get_absolute_time() / 1000. - temps_ms > 100){
			temps_ms = get_absolute_time() / 1000.;
			printf(">nb_message:%u\n", nb_message);
		}
	}
}

absolute_time_t envoie_nb_char(int nb_char) {
	absolute_time_t current_time_us, start_time_us;
	int j=0;
	
	if (nb_char == 0){
		return 0;
	}
	chaine_envoie[nb_char - 1] = '\n';
	chaine_envoie[nb_char] = '\0';
	start_time_us = get_absolute_time();
	printf("%s", chaine_envoie);	
	envoie_donnees(chaine_envoie, strlen(chaine_envoie));

	current_time_us = get_absolute_time();
	chaine_envoie[nb_char - 1] = 'a';
	chaine_envoie[nb_char] = 'a';
	return current_time_us - start_time_us;
}