/*****
 * Copyright (c) 2024 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "Asser_Moteurs.h"
#include "Moteurs.h"
#include "QEI.h"
#include "Teleplot.h"
#include "Temps.h"
#include <stdio.h>
#include <math.h>

void setup(void);
void loop(void);

int valeur = 1;
int consigne_vitesse_moteur=200;
uint32_t m_temps_ms;
uint32_t pas_de_temps_ms=1;

void gestion_affichage(void);

void main(void)
{
    setup();
	while(1){
		loop();
	}
}

void setup(void){
	stdio_init_all();
	Teleplot_init();
	Temps_init();
	Moteur_init();
	QEI_init();
	multicore_launch_core1(gestion_affichage);
}

void loop(void){
	if(Temps_get_temps_ms()!= m_temps_ms){
		m_temps_ms = Temps_get_temps_ms();
		if(m_temps_ms % pas_de_temps_ms == 0){
			QEI_update();
			AsserMoteur_gestion(pas_de_temps_ms);
			
		}
		if(m_temps_ms % 1200 == 0){
			consigne_vitesse_moteur = -consigne_vitesse_moteur; 
			AsserMoteur_setConsigne_mm_s(MOTEUR_A, consigne_vitesse_moteur);
			AsserMoteur_setConsigne_mm_s(MOTEUR_B, consigne_vitesse_moteur);
		}
	}
}

void affichage(void){
	Teleplot_fige_temps();
	Teleplot_add_variable_int("consigne", consigne_vitesse_moteur);
	// 
	Teleplot_add_variable_float_2decimal("Vitesse_A", QEI_get_mm(QEI_A_NAME) * 1000);
	Teleplot_add_variable_float_2decimal("Vitesse_B", QEI_get_mm(QEI_B_NAME) * 1000);
	Teleplot_envoie_tampon();
}

void gestion_affichage(void){
	while(1){
		affichage();
	}

}