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



VL53L8CX_Configuration 	dev;
int status;
char SPI_buffer[10];

void setup()
{ 
  char value; 
  dev.platform.address = 0x29;
  Serial.begin(115200);
  pinMode(D0, OUTPUT);
  digitalWrite(D0, HIGH); 

  delay(500);
  Serial.print("MOSI: ");
  Serial.println(MOSI);
  Serial.print("MISO: ");
  Serial.println(MISO);
  Serial.print("SCK: ");
  Serial.println(SCK);
  Serial.print("SS: ");
  Serial.println(SS); 

  delay(1000);

  SPI.begin();

  
  SPI.beginTransaction(SPISettings(400000, MSBFIRST, SPI_MODE3));
  digitalWrite(D0, LOW); 
  SPI_buffer[0] = 0xff;
  SPI_buffer[1] = 0xff;
  SPI_buffer[2] = 0x00;
  SPI.transfer(SPI_buffer, 3); // WrByte(&(p_dev->platform), 0x7fff, 0x00);
  digitalWrite(D0, HIGH); 
  SPI.endTransaction();
  
  
  SPI.beginTransaction(SPISettings(400000, MSBFIRST, SPI_MODE3));
  digitalWrite(D0, LOW); 
  /*SPI.transfer(0);
  SPI.transfer(0);
  value = SPI.transfer(0);*/
   SPI_buffer[0] = 0x00;
   SPI_buffer[1] = 0x00;
   SPI.transfer(SPI_buffer, 3);
  SPI.endTransaction();
  
  digitalWrite(D0, HIGH);
  Serial.printf("device id:%d\n", SPI_buffer[2]);
  
  
  

  SPI.beginTransaction(SPISettings(400000, MSBFIRST, SPI_MODE3));
  digitalWrite(D0, LOW); 
  SPI_buffer[0] = 0x00;
  SPI_buffer[1] = 0x01;
  SPI.transfer(SPI_buffer, 3);
  digitalWrite(D0, HIGH);
  SPI.endTransaction();
  Serial.printf("revision id:%d\n", SPI_buffer[2]);

  uint8_t my_buffer[10];
  RdMulti(NULL, 0x00, my_buffer, 2);
  Serial.printf("id:%d, rev:%d", my_buffer[0], my_buffer[1]);
  

  

  /*
	status |= RdByte(&(p_dev->platform), 0, &device_id);
	status |= RdByte(&(p_dev->platform), 1, &revision_id);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);*/

  //while(1);



  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[row]);
      }
      Serial.printf("\n");
    }
    Serial.printf("\n");
  }else{
    //Serial.printf(">r:0\n");
  }
  

}