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

#define COULEUR_BLEU 1
#define COULEUR_JAUNE 0

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

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

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

void gestion_VL53L8CX(void);

const 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;
	bool fin_match = false;



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

	

	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(gestion_affichage);
	multicore_launch_core1(gestion_VL53L8CX);
	sleep_ms(5000);
	printf("Demarrage...\n");
	
	

	configure_trajet(identifiant_lire(), get_couleur());

	
	float vitesse_init =300;
	vitesse = vitesse_init;

	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;

	while(get_tirette(identifiant_lire()));
	gpio_put(LED1PIN, 0);

	// Seul le premier PAMI doit attendre 90s, les autres démarrent lorsque celui de devant part
	if(identifiant_lire() == 3){
		sleep_ms(90000);
	}

	temps_depart_ms = Temps_get_temps_ms();

	while(1){
		
		// Fin du match 
		if((Temps_get_temps_ms() -temps_depart_ms) >10000 || (fin_match == 1)){
			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();
					if(Asser_Position_panic_angle()){
						fin_match=1;
					}
				}
				AsserMoteur_Gestion(step_ms);
			}
		}

		
		
	}
}

/// @brief Obtient la distance de l'obstacle le plus proche.
/// @param  
void gestion_VL53L8CX(void){
	VL53L8CX_ResultsData Results;
	float distance_obstacle;
	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);
			VL53L8_min_distance(Results, &distance_obstacle);
			Trajet_set_obstacle_mm(distance_obstacle);
		}
		affichage();
	}
}


extern float delta_x_mm, delta_y_mm, delta_orientation_radian;

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

void affichage(void){
/*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(">distance_obstacle:%f\n",Trajet_get_obstacle_mm());

	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>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette(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(int id){
	if(id == 3){
		return !gpio_get(TIRETTE_PIN);
	}
	return (VL53L8_get_old_min_distance() <50);
	
}

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

/// @brief !! Arg la GPIO 26 ne répond pas ! => Réglé ADC_VREF était à la masse
/// @return identifiant du robot (PDI switch)
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;
	Trajet_config(TRAJECT_CONFIG_RAPIDE);

	switch(couleur){
		case COULEUR_JAUNE:
			switch (identifiant)
			{
				case 0:
					Localisation_set(3000-1249, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
					3000-750, 1400, 3000-750, 2100, 0, 0);
					break;
				case 1:
					Localisation_set(3000-1249, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1250, 2000-63, 3000-1050, 2000-63, 
					3000-750, 1400, 3000-750, 2100, 0, 0);
					break;
				case 2:
					Localisation_set(3000-1245, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1244, 2000-63, 3000-950, 2000-63, 
					3000-540, 1400, 3100, 1400, M_PI, M_PI);
					break;
				case 3:
					Localisation_set(3000-1130, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1122, 2000-63, 3000-905, 2000-63, 
					3000-606, 2000-590, 3000-225, 2000-225, -M_PI, -M_PI);
					break;
				case 4:
					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
					Localisation_set(3000-1364, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1363, 2000-63, 3000-550, 2000-63, 
					2700-900, 600, 2700-0, 0, 0, 0);
					break;
				case 5:
					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
					Localisation_set(3000-1450, 2000-63, 0);
					Trajectoire_bezier(&trajectoire, 3000-1449, 2000-63, 3000-675, 2000-63, 
					3000-930, 970, 0, 1200, -M_PI / 2., M_PI);
					break;
				case 6:
					break;
				case 7:
					break;
				
				default:
					break;
			}
			break;

		case COULEUR_BLEU:
			switch (identifiant)
			{
				case 0:
					Localisation_set(1249, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
					750, 1400, 750, 2100, M_PI, M_PI);
					break;
				case 1:
					Localisation_set(1249, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1250, 2000-63, 1050, 2000-63, 
					750, 1400, 750, 2100, M_PI, M_PI);
					break;
				case 2:
					Localisation_set(1245, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1244, 2000-63, 950, 2000-63, 
					540, 1400, -100, 1400, M_PI, M_PI);
					break;
				case 3:
					Localisation_set(1121, 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:
					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
					Localisation_set(1364, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1363, 2000-63, 550, 2000-63, 
					900, 600, 0, 0, -M_PI / 2., M_PI);
					break;
				case 5:
					Trajet_config(TRAJECT_CONFIG_RAPIDE_ROUGE);
					Localisation_set(1450, 2000-63, M_PI);
					Trajectoire_bezier(&trajectoire, 1449, 2000-63, 675, 2000-63, 
					930, 970, 3000, 1200, -M_PI / 2., M_PI);
					break;
				case 6:
					break;
				case 7:
					break;
				
				default:
					break;
			}
			break;
	}
	
	Trajet_debut_trajectoire(trajectoire);

}