#include <Wire.h>
#include "vl53l8cx_api.h"

VL53L8CX_Configuration 	dev;
int status;

void setup()
{  
  dev.platform.address = 0x29;
  Serial.begin(115200);

  delay(500);
  Wire.setBufferSize(0x8003);
  Wire.setTimeOut(8000);
  Wire.begin();
  
  Serial.println("debut init");
  status = vl53l8cx_init(&dev);
  if(status != 0){
    printf("Status init = %d\n", status);
    while(1);
  }
  delay(100);

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

  status = vl53l8cx_start_ranging(&dev);
  
  Serial.printf("Fin init: %d\n", status);
}

void loop()
{
  int row, col;
  uint8_t isReady;
  VL53L8CX_ResultsData 	Results;
  status = vl53l8cx_check_data_ready(&dev, &isReady);
  if(status){
			Serial.printf("erreur:%d\n", status);
		}
  if(isReady)
  {
    Serial.printf(">r:1\n");
    vl53l8cx_get_ranging_data(&dev, &Results);
    for(col=0; col<8; col++){
      for(row=0; row<8; row++){
        Serial.printf("%4d ", Results.distance_mm[col + 8*row]);
      }
      Serial.printf("\n");
    }
    Serial.printf("\n");
  }else{
    //Serial.printf(">r:0\n");
  }
  

}