/*****
 * Copyright (c) 2023 - Poivron Robotique
 *
 * SPDX-License-Identifier: BSD-3-Clause
*/
#include "pico/stdlib.h"
#include "pico/multicore.h"
#include "hardware/adc.h"
#include "hardware/pwm.h"
#include "Asser_Moteurs.h"
#include "Localisation.h"
#include "Commande_vitesse.h"
#include "Moteurs.h"
#include "Temps.h"
#include <stdio.h>
#include "QEI.h"

#define LED1PIN 20

void affichage(void);
void tension_batterie_init(void);
uint16_t tension_batterie_lire(void);

void identifiant_init(void);
uint identifiant_lire(void);

uint32_t step_ms=1;
float distance1_mm=0, distance2_mm=0;

void main(void)
{
	int ledpower = 500;
	


    stdio_init_all();
	AsserMoteur_Init();
	Temps_init();
	tension_batterie_init();
	identifiant_init();
	Localisation_init();
	uint32_t temps_ms = Temps_get_temps_ms();
	

	gpio_init(LED1PIN);
	gpio_set_dir(LED1PIN, GPIO_OUT );
	gpio_put(LED1PIN, 1);

	
	multicore_launch_core1(affichage);
	
	commande_vitesse(100 , 1.);


	while(1){
		
		if(temps_ms != Temps_get_temps_ms()){
			temps_ms = Temps_get_temps_ms();
			if(temps_ms % step_ms == 0){
				QEI_update();
				AsserMoteur_Gestion(step_ms);
				Localisation_gestion();
			}
			if(temps_ms % 100 == 0){
				identifiant_lire();
				//printf(">c1:%d\n>c2:%d\n", QEI_get(QEI_A_NAME), QEI_get(QEI_B_NAME) );	
			}
		}

		
		
	}
}

void affichage(void){
	while(1){
		printf(">c1_mm:%f\n>c2_mm:%f\n", QEI_get_mm(QEI_A_NAME), QEI_get_mm(QEI_B_NAME) );
		
		/*printf(">m1:%f\n>m2:%f\n", AsserMoteur_getVitesse_mm_s(MOTEUR_A, step_ms), AsserMoteur_getVitesse_mm_s(MOTEUR_B, step_ms) );
		printf(">m1_c:%f\n>m2_c:%f\n", AsserMoteur_getConsigne_mm_s(MOTEUR_A), AsserMoteur_getConsigne_mm_s(MOTEUR_B) );*/
		printf(">pos_x:%.1f\n>pos_y:%.1f\n>pos_angle:%.1f\n", Localisation_get().x_mm, Localisation_get().y_mm, Localisation_get().angle_radian);
		sleep_ms(100);
	}
}

void tension_batterie_init(void){
	adc_init();
    adc_gpio_init(28); // Analog_2
    adc_select_input(2);
    adc_run(1); // Free running mode
}

uint16_t tension_batterie_lire(){
	uint16_t result = (uint16_t) adc_hw->result;
	const float conversion_factor = 3.3f / (1 << 12);
	float v_bat = result * conversion_factor * 11.;

	return result;
}

void identifiant_init(){
	gpio_init(21);
	gpio_init(22);
	gpio_init(26);
	gpio_pull_up(21);
	gpio_pull_up(22);
	gpio_pull_up(26);
	gpio_set_dir(21, GPIO_IN);
	gpio_set_dir(22, GPIO_IN);
	gpio_set_dir(26, GPIO_IN);
}


/// @brief !! Arg la GPIO 26 ne répond pas !
/// @return 
uint identifiant_lire(){
	return (gpio_get(21) << 2)+ (gpio_get(22) << 1) + gpio_get(26);
}