#include <SPI.h>
#include "lwip/err.h"
/**
  *
  * Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
#include <stdint.h>
#include <Arduino.h>
#include <Wire.h>
#include "vl53l8cx_api.h"
#include "driver/i2c.h"
#include "esp32-hal-i2c.h"

#define SS D0
#define BUFFER_SIZE 0x1000

void print_status(int status){
  if(status)
    Serial.printf("Status : %d\n",status);
}


uint8_t RdByte(
		VL53L8CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_value)
{
	uint8_t status = 255;
	
	/* Need to be implemented by customer. This function returns 0 if OK */
  //Serial.printf("RdByte adresse :%x\n",p_platform->address);
  SPI.beginTransaction(SPISettings(2000000, MSBFIRST, SPI_MODE3));
  digitalWrite(SS, LOW); 


  SPI.transfer(RegisterAdress >> 8 & 0x7F);
  SPI.transfer(RegisterAdress & 0xFF);
  *p_value = SPI.transfer(0x00);
  digitalWrite(SS, HIGH); 
  SPI.endTransaction();
  return ERR_OK;
	
}

uint8_t WrByte(
		VL53L8CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t value)
{
	uint8_t status = 255;
  int a,b,c;

	/* Need to be implemented by customer. This function returns 0 if OK */
  
  SPI.beginTransaction(SPISettings(2000000, MSBFIRST, SPI_MODE3));
  digitalWrite(SS, LOW); 
  SPI.transfer(RegisterAdress >> 8 | 0x80);
  SPI.transfer(RegisterAdress & 0xFF);
  value = SPI.transfer(value);
  digitalWrite(SS, HIGH); 
  SPI.endTransaction();
  

  return ERR_OK;

}

uint8_t WrMulti(
		VL53L8CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_values,
		uint32_t size)
{
  uint32_t index =0;
  uint8_t my_buffer[BUFFER_SIZE];
  SPI.beginTransaction(SPISettings(2000000, MSBFIRST, SPI_MODE3));
  digitalWrite(SS, LOW); 


  SPI.transfer(RegisterAdress >> 8 | 0x80);
  SPI.transfer(RegisterAdress & 0xFF);
  while(size > BUFFER_SIZE){
    // Serial.printf("WrMulti size: %d, index: %d\n", size, index);
    // Serial.printf("WrMulti adresse: 0x%x, buffer_size: %d\n", (uint8_t*)&p_values[index], buffer_size);
    // Serial.printf("WrMulti adresse: 0x%x\n", my_buffer);
    memcpy(my_buffer, (uint8_t*)&p_values[index], BUFFER_SIZE);
    SPI.transfer(my_buffer, BUFFER_SIZE);
    // Serial.printf("Fin transfer\n", size, index);
    size = size - BUFFER_SIZE;
    index = index + BUFFER_SIZE;
  }
  // Serial.printf("WrMulti size: %d, index: %d\n", size, index);
  // Serial.printf("WrMulti adresse: 0x%x\n", my_buffer);
  memcpy(my_buffer, (uint8_t*)&p_values[index], size);
  SPI.transfer(my_buffer, size);
  digitalWrite(SS, HIGH); 
  SPI.endTransaction();
	return ERR_OK;

}

uint8_t RdMulti(
		VL53L8CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_values,
		uint32_t size)
{
	uint8_t status = 255;
  uint32_t index =0;
  uint32_t buffer_size = 32;
  SPI.beginTransaction(SPISettings(2000000, MSBFIRST, SPI_MODE3));
  digitalWrite(SS, LOW); 

  //Serial.printf("RdMulti adress 0x%x%x\n",RegisterAdress >> 8 & 0x7F, RegisterAdress & 0xFF);
  SPI.transfer(RegisterAdress >> 8 & 0x7F);
  SPI.transfer(RegisterAdress & 0xFF);
  while(size > buffer_size){
    //Serial.printf("RdMulti size: %d, index: %d\n", size, index);
    SPI.transfer(&(p_values[index]), buffer_size);
    index += buffer_size;
    size -= buffer_size;
  }
  //Serial.printf("RdMulti size: %d, index: %d\n", size, index);
  SPI.transfer(&(p_values[index]), size);
  digitalWrite(SS, HIGH); 
  SPI.endTransaction();

	
	return ERR_OK;
	
}

uint8_t Reset_Sensor(
		VL53L8CX_Platform *p_platform)
{
	uint8_t status = 0;
	
	/* (Optional) Need to be implemented by customer. This function returns 0 if OK */
	
	/* Set pin LPN to LOW */
	/* Set pin AVDD to LOW */
	/* Set pin VDDIO  to LOW */
	/* Set pin CORE_1V8 to LOW */
	WaitMs(p_platform, 100);

	/* Set pin LPN to HIGH */
	/* Set pin AVDD to HIGH */
	/* Set pin VDDIO to HIGH */
	/* Set pin CORE_1V8 to HIGH */
	WaitMs(p_platform, 100);

	return status;
}

void SwapBuffer(
		uint8_t 		*buffer,
		uint16_t 	 	 size)
{
	uint32_t i, tmp;
	
	/* Example of possible implementation using <string.h> */
	for(i = 0; i < size; i = i + 4) 
	{
		tmp = (
		  buffer[i]<<24)
		|(buffer[i+1]<<16)
		|(buffer[i+2]<<8)
		|(buffer[i+3]);
		
		memcpy(&(buffer[i]), &tmp, 4);
	}
}	

uint8_t WaitMs(
		VL53L8CX_Platform *p_platform,
		uint32_t TimeMs)
{
	/* Need to be implemented by customer. This function returns 0 if OK */
	//sleep_ms(TimeMs);
	delay(TimeMs);
	return 0;
}