/*****
 * 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 "Strategie.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(void);
int get_couleur(void);

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)
{
	VL53L8CX_ResultsData Results;
	bool fin_match = false;



    stdio_init_all();
	Temps_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);
	// TODO: A remettre - quand on aura récupéré un capteur
	//multicore_launch_core1(gestion_VL53L8CX);
	sleep_ms(5000);
	printf("Demarrage...\n");
	
	
	float vitesse_init =300;
	vitesse = vitesse_init;

	enum etat_trajet_t etat_trajet=TRAJET_EN_COURS;

	while(get_tirette());
	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) >15000 || (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(Strategie_super_star(step_ms) == ACTION_TERMINEE){
					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) );*/

	long temps_ms = (time_us_64()/1000);
	printf(">temps_ms:%lu\n", temps_ms);
	printf(">pos_angle:%.1f\n", 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:%lu:%.2f\n>angle_delta:%lu:%.2f\n", temps_ms, delta_orientation_radian, temps_ms, atan2f(delta_y_mm, delta_x_mm));
	printf(">pos_x:%lu:%.2f\n>pos_y:%lu:%.2f\n", temps_ms, position_actuelle.x_mm, temps_ms, position_actuelle.y_mm);
	printf(">con_x:%lu:%.2f\n>con_y:%lu:%.2f\n", temps_ms, point.point_xy.x, temps_ms, point.point_xy.y);
	
	printf(">couleur:%d\n>id:%d\n>Tirette:%d\n", get_couleur(), identifiant_lire(), get_tirette());
}

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(){
	return !gpio_get(TIRETTE_PIN);
	
}

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