/*****
 * 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 "hardware/i2c.h"
#include "i2c_slave.h"
#include "Temps.h"
#include "VL53L8_2024.h"
#include "vl53l8cx_api.h"
#include "ws2812.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

#define OFFSET_CAPTEUR_GAUCHE_X_MM (-170)
#define OFFSET_CAPTEUR_DROIT_X_MM  (170)

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

#define I2C_SLAVE_SDA_PIN SDA
#define I2C_SLAVE_SCL_PIN SCL
#define I2C_SLAVE_ADDRESS 0x19

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 gauche, droit;

uint8_t capteur_init(VL53L8CX_Configuration * capteur);


// The slave implements a 256 byte memory. To write a series of bytes, the master first
// writes the memory address, followed by the data. The address is automatically incremented
// for each byte transferred, looping back to 0 upon reaching the end. Reading is done
// sequentially from the current memory address.
static struct
{
    uint8_t mem[256];
    uint8_t mem_address;
    bool mem_address_written;
} context;

// Our handler is called from the I2C ISR, so it must complete quickly. Blocking calls /
// printing to stdio may interfere with interrupt handling.
static void i2c_slave_handler(i2c_inst_t *i2c, i2c_slave_event_t event) {
    switch (event) {
    case I2C_SLAVE_RECEIVE: // master has written some data
        if (!context.mem_address_written) {
            // writes always start with the memory address
            context.mem_address = i2c_read_byte_raw(i2c);
            context.mem_address_written = true;
        } else {
            // save into memory
            context.mem[context.mem_address] = i2c_read_byte_raw(i2c);
            context.mem_address++;
        }
        break;
    case I2C_SLAVE_REQUEST: // master is requesting data
        // load from memory
        i2c_write_byte_raw(i2c, context.mem[context.mem_address]);
        context.mem_address++;
        break;
    case I2C_SLAVE_FINISH: // master has signalled Stop / Restart
        context.mem_address_written = false;
        break;
    default:
        break;
    }
}

void i2c_envoi_32bits(int32_t value, char adresse){
  context.mem[adresse] = value >> 24;
  context.mem[adresse+1] = (value >> 16) & 0xFF;
  context.mem[adresse+2] = (value >> 8) & 0xFF;
  context.mem[adresse+3] = value & 0xFF;
}

static void i2c_setup_slave() {
    gpio_init(I2C_SLAVE_SDA_PIN);
    gpio_set_function(I2C_SLAVE_SDA_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SLAVE_SDA_PIN);

    gpio_init(I2C_SLAVE_SCL_PIN);
    gpio_set_function(I2C_SLAVE_SCL_PIN, GPIO_FUNC_I2C);
    gpio_pull_up(I2C_SLAVE_SCL_PIN);

    i2c_slave_init(i2c1, I2C_SLAVE_ADDRESS, &i2c_slave_handler);
}

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

	ws2812_init();

	

	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 SPI0
	gpio_set_function(2, GPIO_FUNC_SPI);
	gpio_set_function(3, GPIO_FUNC_SPI);
	gpio_set_function(4, GPIO_FUNC_SPI);

	// Initialisation de la liaison SPI1
	gpio_set_function(26, GPIO_FUNC_SPI);
	gpio_set_function(27, GPIO_FUNC_SPI);
	gpio_set_function(28, 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);

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


	gauche.platform.address = 1;
	droit.platform.address = 29;
	
	spi_set_format(spi0, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
	spi_init(spi0, 2000000);

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

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

	i2c_setup_slave();

	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;

}



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 gauche_planche_pos_x, gauche_planche_pos_y, gauche_planche_angle;
float droit_planche_pos_x, droit_planche_pos_y, droit_planche_angle;

float planche_centre_x, planche_centre_y, planche_angle_rad;
float taille_planche;
int echec;

void gestion_VL53L8CX(void){
	VL53L8CX_ResultsData results_gauche, results_droit;
	int echec_gauche, echec_droit;
	float distance_obstacle;
	capteur_init(&gauche);
	capteur_init(&droit);
	sleep_ms(100);
    capteur_actualise( &gauche, &results_gauche); // une première lecture 
	capteur_actualise( &droit, &results_droit); // une première lecture 
    uint8_t status, isReady;
	while(1){
		isReady = 0;
		isReady |= capteur_actualise( &gauche, &results_gauche);
		isReady |= capteur_actualise( &droit, &results_droit);
		if(isReady){
			capteurs_affiche_donnees(&results_gauche, &results_droit);
			//VL53L8_lecture( &gauche, &Results);
			echec = 0;
			echec_gauche = VL53L8_pos_planche_gauche(results_gauche, &gauche_planche_pos_x, &gauche_planche_pos_y, &gauche_planche_angle);
			echec_droit = VL53L8_pos_planche_droit(results_droit, &droit_planche_pos_x, &droit_planche_pos_y, &droit_planche_angle);

			droit_planche_pos_x = -droit_planche_pos_x + OFFSET_CAPTEUR_DROIT_X_MM;
			gauche_planche_pos_x = -gauche_planche_pos_x + OFFSET_CAPTEUR_GAUCHE_X_MM;

			echec = echec_gauche + echec_droit;

			if(echec == 2){
				// Aucun capteur valide
				ws2812_set(0x0F,0,0);
				context.mem[0] = 0;
			}else if(echec == 1){
				// Un seul capteur valide
				if(echec_gauche == 0 && ! isnan(gauche_planche_angle)){
					// capteur gauche permet de déterminer la position de la planche
					ws2812_set(0x0F,0x8,0);
					context.mem[0] = 1;
					planche_centre_x = gauche_planche_pos_x + 200 * cos(gauche_planche_angle);
					planche_centre_y = gauche_planche_pos_y + 200 * sin(gauche_planche_angle);
					planche_angle_rad = gauche_planche_angle;
					
				}else if(echec_droit == 0 && ! isnan(droit_planche_angle)){
					// capteur droit permet de déterminer la position de la planche
					ws2812_set(0x0F,0x8,0);
					context.mem[0] = 1;
					planche_centre_x = droit_planche_pos_x - 200 * cos(droit_planche_angle);
					planche_centre_y = droit_planche_pos_y - 200 * sin(droit_planche_angle);
					planche_angle_rad = droit_planche_angle;
					
				}else{
					// On a un bout de la planche mais pas d'angle, c'est un echec
					echec = 2;
					ws2812_set(0x0F,0,0);
					context.mem[0] = 0;
				}
				
			}else{
				// 2 capteurs valides
				ws2812_set(0,0x0F,0);
				planche_centre_x = (droit_planche_pos_x + gauche_planche_pos_x)/2;
				planche_centre_y = (droit_planche_pos_y + gauche_planche_pos_y)/2;
				planche_angle_rad = atan2f(droit_planche_pos_y - gauche_planche_pos_y, droit_planche_pos_x - gauche_planche_pos_x);
				i2c_envoi_32bits(planche_centre_x, 1);
				i2c_envoi_32bits(planche_centre_y, 5);
				i2c_envoi_32bits((int)(planche_angle_rad * 1000), 9);
				context.mem[0] = 2;
			}

			
		}
		affichage();
		printf("\n");
		sleep_ms(150);
	}
}


extern float delta_x_mm, delta_y_mm, delta_orientation_radian;

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

void affichage(void){
	
		printf(">planche_g_pos_x:%f\n",gauche_planche_pos_x);
		printf(">planche_g_pos_y:%f\n",gauche_planche_pos_y);
		printf(">planche_d_pos_x:%f\n",droit_planche_pos_x);
		printf(">planche_d_pos_y:%f\n",droit_planche_pos_y);
		printf(">planche_centre_x:%f\n",planche_centre_x);
		printf(">planche_centre_y:%f\n",planche_centre_y);
		printf(">planche_angle:%f\n",planche_angle_rad / M_PI * 180);
		printf(">gauche_planche_angle:%f\n",gauche_planche_angle / M_PI * 180);
		printf(">droit_planche_angle:%f\n",droit_planche_angle / M_PI * 180);
		printf(">echec:%d\n", echec);
		

}