/*****
 * 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_Position.h"
#include "Asser_Moteurs.h"
#include "Commande_vitesse.h"
#include "Localisation.h"
#include "Moteurs.h"
#include "Temps.h"
#include "Trajectoire.h"
#include "Trajet.h"
#include "VL53L8_2024.h"
#include "vl53l8cx_api.h"
#include <stdio.h>
#include <math.h>
#include "QEI.h"

#define LED1PIN 20
#define TIRETTE_PIN 6
#define COULEUR_PIN 4

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

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

int get_tirette(void);
int get_couleur(void);
void configure_trajet(int identifiant, int couleur);

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

// DEBUG
extern float abscisse; 
extern struct point_xyo_t point;
float vitesse;
VL53L8CX_Configuration Dev;

void main(void)
{
	int ledpower = 500;
	VL53L8CX_ResultsData Results;



    stdio_init_all();
	Temps_init();
	//tension_batterie_init();
	identifiant_init();
	Localisation_init();
	Trajet_init();
	i2c_maitre_init();

	VL53L8_init(&Dev);
	sleep_ms(100);
    VL53L8_lecture( &Dev, &Results); // une première lecture 
    uint8_t status, isReady;
	while(1){
		status = vl53l8cx_check_data_ready(&Dev, &isReady);
		if(isReady){
			VL53L8_lecture( &Dev, &Results);
		}
		
	}

	uint32_t temps_ms = Temps_get_temps_ms();
	uint32_t temps_depart_ms;
	struct position_t position_robot={.x_mm=0, .y_mm=0, .angle_radian=0};
	float vitesse_mm_s=100;
	

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

	
	multicore_launch_core1(affichage);
	

	
	/*Localisation_set(1130, 2000-63, M_PI);
	struct trajectoire_t trajectoire;
	Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
		606, 2000-590, 225, 2000-225, M_PI, M_PI);
	Trajet_config(TRAJECT_CONFIG_RAPIDE);
	Trajet_debut_trajectoire(trajectoire);*/

	/*Localisation_set(0, 0, 0);
	struct trajectoire_t trajectoire;
	Trajectoire_droite(&trajectoire, 0,0,1000,0, M_PI, M_PI);
	Trajet_config(TRAJECT_CONFIG_AVANCE_ET_TOURNE);
	Trajet_debut_trajectoire(trajectoire);*/

	configure_trajet(identifiant_lire(), 0);

	
	float vitesse_init =300;
	vitesse = vitesse_init;

	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;

	while(get_tirette());

	// sleep_ms(90000);

	temps_depart_ms = Temps_get_temps_ms();

	while(1){
		
		// Fin du match 
		if((Temps_get_temps_ms() -temps_depart_ms) >10000 ){
			Moteur_Stop();
			while(1);
		}
		if(temps_ms != Temps_get_temps_ms()){
			temps_ms = Temps_get_temps_ms();
			if(temps_ms % step_ms == 0){
				QEI_update();
				Localisation_gestion();				
				if(etat_trajet != TRAJET_TERMINE){
					etat_trajet = Trajet_avance((float)step_ms/1000.);
				}else{
					Asser_Position_maintien();
				}
				AsserMoteur_Gestion(step_ms);
			}
		}

		
		
	}
}
extern float delta_x_mm, delta_y_mm, delta_orientation_radian;
void affichage(void){
	while(1){
		/*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);
		//printf(">abscisse:%f\n",abscisse);
		
		struct position_t position_actuelle;
		position_actuelle = Localisation_get();
		printf(">delta_orientation_radian:%.2f\n>angle_delta:%.2f\n",delta_orientation_radian, atan2f(delta_y_mm, delta_x_mm));
		printf(">pos_x:%.2f\n>pos_y:%.2f\n", position_actuelle.x_mm, position_actuelle.y_mm);
		printf(">con_x:%.2f\n>con_y:%.2f\n", point.point_xy.x, point.point_xy.y);
		
		//printf(">couleur:%d\n>id:%d\n", get_couleur(), identifiant_lire());
	}
}

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

	// Tirette
	gpio_init(TIRETTE_PIN);
	gpio_pull_up(TIRETTE_PIN);
	gpio_set_dir(TIRETTE_PIN, GPIO_IN);

	// Couleur
	gpio_init(COULEUR_PIN);
	gpio_pull_up(COULEUR_PIN);
	gpio_set_dir(COULEUR_PIN, GPIO_IN);
}

int get_tirette(void){
	return !gpio_get(TIRETTE_PIN);
}

int get_couleur(void){
	return !gpio_get(COULEUR_PIN);
}

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

void configure_trajet(int identifiant, int couleur){
	
	struct trajectoire_t trajectoire;
	
	
	switch (identifiant)
	{
	case 0:
		break;
	case 1:
		Localisation_set(1465, 2000-63, M_PI);
		Trajectoire_bezier(&trajectoire, 1465, 2000-63, 1260, 2000-63, 
		430, 1240, 430, 2000, M_PI, M_PI);
		break;
	case 2:
		break;
	case 3:
		Localisation_set(1130, 2000-63, M_PI);
		Trajectoire_bezier(&trajectoire, 1122, 2000-63, 905, 2000-63, 
		606, 2000-590, 225, 2000-225, M_PI, M_PI);
		break;
	case 4:
		break;
	case 5:
		break;
	case 6:
		break;
	case 7:
		break;
	
	default:
		break;
	}

	Trajet_config(TRAJECT_CONFIG_RAPIDE);
	Trajet_debut_trajectoire(trajectoire);

}