#include "vl53l8cx_api.h"
#include <stdio.h>
#include <math.h>

#ifndef M_PI
#define M_PI 3.314159
#endif

#define DEGREE_EN_RADIAN (M_PI / 180.)
#define PLAGE_ANGLE_DEGREE (40*DEGREE_EN_RADIAN)

int masque[64]={

	3000,3000,3000,3000,3000,3000,3000,3000,
	3000,3000,3000,3000,3000,3000,3000,3000,
	3000,3000,3000,3000,3000,3000,3000,3000,
	3000,3000,3000,3000,3000,3000,3000,3000,
	3000,3000,3000,3000,3000,3000,3000,3000,
	3000,3000,3000,3000,3000,3000,3000,3000,
	3000,3000,3000,3000,3000,3000,3000,3000,
	3000,3000,3000,3000,3000,3000,3000,3000,
};


float angles_VL53L8[]={
		7./16. * PLAGE_ANGLE_DEGREE,
		5./16. * PLAGE_ANGLE_DEGREE, 
		3./16. * PLAGE_ANGLE_DEGREE, 
		1./16. * PLAGE_ANGLE_DEGREE, 
		-1./16. * PLAGE_ANGLE_DEGREE, 
		-3./16. * PLAGE_ANGLE_DEGREE, 
		-5./16. * PLAGE_ANGLE_DEGREE, 
		-7./16. * PLAGE_ANGLE_DEGREE
	};

float old_min_distance;

void VL53L8_init(VL53L8CX_Configuration * Dev){
	uint8_t status, isAlive, isReady, i;


	/*********************************/
	/*      Customer platform        */
	/*********************************/

	/* Fill the platform structure with customer's implementation. For this
	* example, only the I2C address is used.
	*/
	Dev->platform.address = VL53L8CX_DEFAULT_I2C_ADDRESS;

	/* (Optional) Reset sensor toggling PINs (see platform, not in API) */
	//Reset_Sensor(&(Dev.platform));

	/* (Optional) Set a new I2C address if the wanted address is different
	* from the default one (filled with 0x20 for this example).
	*/
	//status = vl53l8cx_set_i2c_address(&Dev, 0x20);


	/*********************************/
	/*   Power on sensor and init    */
	/*********************************/

	/* (Optional) Check if there is a VL53L8CX sensor connected */
	do{
		status = vl53l8cx_is_alive(Dev, &isAlive);
		if(status){

			printf("VL53L8CX not detected at requested address\n");
			WaitMs(&(Dev->platform), 1000);
		}
		if(!isAlive){
			printf("VL53L8CX not alived\n");
			WaitMs(&(Dev->platform), 1000);
		}
	}while(!isAlive || status);
	printf("VL53L8CX detected !\n");
    

	/* (Mandatory) Init VL53L8CX sensor */
	status = vl53l8cx_init(Dev);
	if(status)
	{
		while(1){
			printf("VL53L8CX ULD Loading failed :%d\n", status);
			WaitMs(&(Dev->platform), 1000);
		}
		
		return;
	}

	printf("VL53L8CX ULD ready ! (Version : %s)\n",
			VL53L8CX_API_REVISION);
	
	status = vl53l8cx_set_resolution(Dev, VL53L8CX_RESOLUTION_8X8);
	if(status !=0){
		while(1){
			printf("vl53l8cx_set_resolution failed :%d\n", status);
			WaitMs(&(Dev->platform), 1000);
		}
	}

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

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


	/*********************************/
	/*         Ranging loop          */
	/*********************************/

	status = vl53l8cx_start_ranging(Dev);

}

/// @brief Lit le capteur, filtre les données avec les status renvoyés et le masque de distance défini plus haut
/// @param Dev Capteur à lire
/// @param Results Stockage des résultats
/// @return 0 si tout s'est bien passé, une erreur sinon
uint8_t VL53L8_lecture(VL53L8CX_Configuration * Dev, VL53L8CX_ResultsData * Results){
	uint8_t error=0;
    error = vl53l8cx_get_ranging_data(Dev, Results);
	if(error){
		return error;
	}


    /* As the sensor is set in 4x4 mode by default, we have a total 
        * of 16 zones to print. For this example, only the data of first zone are 
        * print */
    
    // Filtrage des données
    for(int row=0; row < 8; row++){
        for(int col=0; col < 8; col++){
            // Seul status 100% valid
            if(Results->target_status[col+ 8*row] != 5){
                Results->distance_mm[col+ 8*row] = 3000;
            }
            if(Results->distance_mm[col+ 8*row] > 0.9 * masque[col+ 8*row]){
                Results->distance_mm[col+ 8*row] = 3000;
            }
            if(Results->distance_mm[col+ 8*row] < 0){
                printf("Valeur négative %d, status:%d\n", Results->distance_mm[col+ 8*row], Results->target_status[col+ 8*row]);
            }
        }
    }
	return error;	
}

/// @brief Contrôle si des données sont disponibles, si oui, lit le données
/// @param capteur capteur à lire
/// @param results stockage des résultats
/// @return 1 si des données ont été lues
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 = VL53L8_lecture(capteur, results);
    if(status){
      printf("erreur:%d\n", status);
    }
  }else{
    //Serial.printf(">r:0\n");
  }
  return isReady;
}

int VL53L8_min_distance(VL53L8CX_ResultsData Results, float *distance){
	uint16_t mesure1, mesure2;
	int min_distance = 2000;
	int col, row;
	for(col=0; col<8; col++){
		for(row=0; row<=3; row++){
			if(min_distance >  Results.distance_mm[col + 8*row]){
				min_distance = Results.distance_mm[col + 8*row];
			}
		}
	}
	*distance = 0;
	if(min_distance-50 > 0){
		*distance = min_distance-50;
	}
	old_min_distance = *distance;
}

const int selected_row = 5;

/// @brief Renvoie la position de l'extrémité gauche de la planche
/// @param Results Valeurs brutes du capteurs
/// @param pos_x position de la planche 
/// @return 0 sucess, 1 failed, 2 failed: no detection, 3 failed: full detection
int VL53L8_pos_planche_gauche(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle){
	uint16_t mesure1, mesure2;
	int min_distance = 2000;
	int col, row;
	int distance_ref_mm = 500;
	int last_col_avec_planche=0;
	int full_detection = 1;
	int no_detection = 1;

	*angle = NAN;
	// Contrôle si la planche est à droite
	for(col=7; col>=0; col--){
		printf("%d, ", Results.distance_mm[col + 8*3]);
		for(row=selected_row; row<=selected_row; row++){
			// SI c'est la première mesure, on la prend comme référence
			if(Results.distance_mm[col + 8*row] >  500){
				// Au moins une mesure loin
				full_detection = 0;
			}
			
			if(distance_ref_mm >=  500){
				distance_ref_mm = Results.distance_mm[col + 8*row];
			}else{
				// Au moins une mesure proche
				no_detection = 0;
				// Si on est plus proche que la distance de référence, on la prend comme référence.
				if(distance_ref_mm > Results.distance_mm[col + 8*row]){
					distance_ref_mm = Results.distance_mm[col + 8*row];
				}
				// Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche
				if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){
					last_col_avec_planche = col+2;
					// Double break;
					col = 0;
					break;
				}
			}
		}
	}
	printf("\n");
	row=selected_row;
	
	if(last_col_avec_planche == 0 || last_col_avec_planche > 6){
		// Echec
		*pos_x = 0;
		if (no_detection){
			return 2;
		}
		if(full_detection){
			return 3;
		}
		return 1;
	}

	if(last_col_avec_planche < 7){
		/// On peut déterminer un angle de la planche
		float angle_planche;
		angle_planche = atan2f((float) (Results.distance_mm[7 + 8*row] - Results.distance_mm[last_col_avec_planche + 8*row]), 
			-(Results.distance_mm[7 + 8*row] * sinf(angles_VL53L8[7]) - Results.distance_mm[last_col_avec_planche + 8*row]* sinf(angles_VL53L8[last_col_avec_planche])));
		*angle = angle_planche;
	}
	*pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]);
	*pos_y = (float)distance_ref_mm;

	return 0;
}

/// @brief Renvoie la position de l'extrémité droite de la planche
/// @param Results Valeurs brutes du capteurs
/// @param pos_x position de la planche 
/// @return 0 sucess, 1 failed
int VL53L8_pos_planche_droit(VL53L8CX_ResultsData Results, float *pos_x, float *pos_y, float *angle){
	uint16_t mesure1, mesure2;
	int min_distance = 2000;
	int col, row;
	int distance_ref_mm = 500;
	int last_col_avec_planche=0;
	int full_detection = 1;
	int no_detection = 1;

	*angle = NAN;
	// Controle si la planche est à gauche
	for(col=0; col<8; col++){
		printf("%d,", Results.distance_mm[col + 8*3]);
		for(row=selected_row; row<=selected_row; row++){
			
			if(Results.distance_mm[col + 8*row] >  500){
				// Au moins une mesure loin
				full_detection = 0;
			}

			// SI c'est la première mesure, on la prend comme référence
			if(distance_ref_mm >=  500){
				distance_ref_mm = Results.distance_mm[col + 8*row];
			}else{
				// Au moins une mesure proche
				no_detection = 0;
				// Si on est plus proche que la distance de référence, on la prend comme référence.
				if(distance_ref_mm > Results.distance_mm[col + 8*row]){
					distance_ref_mm = Results.distance_mm[col + 8*row];
				}
				// Si la distance est plus éloignée de 5 cm que celle de référence, nous sommes arrivé au bout de la planche
				if(distance_ref_mm + 50 < Results.distance_mm[col + 8*row]){
					last_col_avec_planche = col-2;
					// Double break;
					col = 9;
					break;
				}
			}
		}
	}
	printf("\n");
	row=selected_row;
	
	if(last_col_avec_planche == 0 || last_col_avec_planche > 7){
		// Echec
		*pos_x = 0;
		if (no_detection){
			return 2;
		}
		if(full_detection){
			return 3;
		}
		return 1;
	}

	if(last_col_avec_planche > 0){
		/// On peut déterminer un angle de la planche
		float angle_planche;
		angle_planche = atan2f(-(Results.distance_mm[0 + 8*row] - Results.distance_mm[last_col_avec_planche + 8*row]), 
			Results.distance_mm[0 + 8*row] * sinf(angles_VL53L8[0]) - Results.distance_mm[last_col_avec_planche + 8*row]* sinf(angles_VL53L8[last_col_avec_planche]));
		*angle = angle_planche;
	}

	*pos_x = (float)distance_ref_mm * sinf(angles_VL53L8[last_col_avec_planche]);
	*pos_y = (float)distance_ref_mm;

	return 0;
}

float VL53L8_get_old_min_distance(){
	return 	old_min_distance;
}