/*****
 * 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/spi.h"
#include "i2c_maitre.h"
#include "Temps.h"
#include "VL53L8_2024.h"
#include "vl53l8cx_api.h"
#include <stdio.h>
#include <math.h>

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

#define COULEUR_BLEU 1
#define COULEUR_JAUNE 0

// XIAO RP2040
#define SCK 2
#define MISO 4
#define MOSI 3
#define D0 26
#define D1 27
#define D2 28
#define D3 29


void affichage(void);
void gestion_affichage(void);
void tension_batterie_init(void);
uint16_t tension_batterie_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;

uint8_t capteur_init(VL53L8CX_Configuration * capteur);

void main(void)
{
	int ledpower = 500;
	uint8_t tampon[10];
	uint8_t temp, status;
	VL53L8CX_ResultsData Results;
	bool fin_match = false;



    stdio_init_all();
	stdio_set_translate_crlf(&stdio_usb, false);
	Temps_init();
	//tension_batterie_init();
	spi_init(spi0, 2000000);

	

	uint32_t temps_ms = Temps_get_temps_ms();
	uint32_t temps_depart_ms;
	float vitesse_mm_s=100;
	

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

	// Initialisation de la liaison SPI
	gpio_set_function(2, GPIO_FUNC_SPI);
	gpio_set_function(3, GPIO_FUNC_SPI);
	gpio_set_function(4, GPIO_FUNC_SPI);

	gpio_set_function(16, GPIO_FUNC_NULL);
	gpio_set_function(17, GPIO_FUNC_NULL);
	gpio_set_function(18, GPIO_FUNC_NULL);
	gpio_set_function(19, GPIO_FUNC_NULL);

	gpio_init(1); // CS
	gpio_set_dir(1, GPIO_OUT );
	gpio_put(1, 1);

	Dev.platform.address = 1;

	spi_set_format(spi0, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
	spi_init(spi0, 2000000);

	tampon[0] = 0x55;
	tampon[1] = 0x55;

	sleep_ms(5000);
	printf("Demarrage...\n");
	
	//multicore_launch_core1(gestion_affichage);
	
	
	gestion_VL53L8CX();
	
	

}


uint8_t capteur_init(VL53L8CX_Configuration * capteur){
  uint8_t status=0;
  printf("debut init\n");
  //pinMode(capteur->platform.address, OUTPUT);

  status = vl53l8cx_init(capteur);
  if(status != 0){
    while(1){
      printf("Status init = %d\n", status);
      WaitMs(&(capteur->platform), 1000);
      break;
    }
  }
  sleep_ms(100);

  status = vl53l8cx_set_resolution(capteur, VL53L8CX_RESOLUTION_8X8);
	if(status !=0){
		while(1){
			printf("vl53l8cx_set_resolution failed :%d\n", status);
			WaitMs(&(capteur->platform), 1000);
      break;
		}
	}

	status = vl53l8cx_set_ranging_frequency_hz(capteur, 15);
	if(status !=0){
		while(1){
			printf("vl53l8cx_set_ranging_frequency_hz  (15hz) failed :%d\n", status);
			WaitMs(&(capteur->platform), 1000);
      break;
		}
	}

	//vl53l8cx_set_target_order(&Dev, VL53L8CX_TARGET_ORDER_CLOSEST);
	vl53l8cx_set_target_order(capteur, VL53L8CX_TARGET_ORDER_STRONGEST);

  status = vl53l8cx_start_ranging(capteur);
  
  printf("Capteur : %d, fin init: %d\n", capteur->platform.address, status);
  return status;

}

uint8_t capteur_actualise(VL53L8CX_Configuration * capteur, VL53L8CX_ResultsData * results){
  uint8_t isReady, status;

  status = vl53l8cx_check_data_ready(capteur, &isReady);
  if(status){
		printf("erreur:%d\n", status);
	}
  if(isReady)
  {
    printf(">r%d:1\n",capteur->platform.address);
    status = vl53l8cx_get_ranging_data(capteur, results);
    if(status){
      printf("erreur:%d\n", status);
    }
  }else{
    //Serial.printf(">r:0\n");
  }
  return isReady;
}

void capteurs_affiche_donnees(VL53L8CX_ResultsData * results_gauche, VL53L8CX_ResultsData * results_droit){
  uint8_t row, col;
  for(col=0; col<8; col++){
    for(row=0; row<8; row++){
      printf("%4d ", results_gauche->distance_mm[col*8 + row]);
    }
    printf("  -  ");
    for(row=0; row<8; row++){
      printf("%4d ", results_droit->distance_mm[col*8 + row]);
    }

    printf("\n");
  }
  printf("\n");
}

/// @brief Obtient la distance de l'obstacle le plus proche.
/// @param  

float planche_pos_x, planche_pos_y;
void gestion_VL53L8CX(void){
	VL53L8CX_ResultsData Results;
	float distance_obstacle;
	capteur_init(&Dev);
	sleep_ms(100);
    capteur_actualise( &Dev, &Results); // une première lecture 
    uint8_t status, isReady;
	while(1){
		isReady = capteur_actualise( &Dev, &Results);
		if(isReady){
			capteurs_affiche_donnees(&Results, &Results);
			//VL53L8_lecture( &Dev, &Results);
			VL53L8_min_distance(Results, &distance_obstacle);
			VL53L8_pos_planche(Results, &planche_pos_x, &planche_pos_y);
		}
		//affichage();
		sleep_ms(50);
	}
}


extern float delta_x_mm, delta_y_mm, delta_orientation_radian;

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

void affichage(void){
		printf(">planche_pox_x:%f\n",planche_pos_x);
		printf(">planche_pox_y:%f\n",planche_pos_y);
/*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()));*/
}