/**
  *
  * 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"

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


size_t _I2C_write(uint8_t *data, size_t quantity)
{
    for(size_t i = 0; i < quantity; ++i) {
        if(!Wire.write(data[i])) {
            Wire.endTransmission(true);

            Serial.printf("Erreur I2C : %d %d\n", i, quantity);
            return i;
        }
    }
    return quantity;

}

uint8_t I2C_write_register(int adresse_i2c, uint16_t adresse_registre, unsigned char * donnees, unsigned int taille_donnees){
  // Ecriture
  int status;

  
  
  Wire.beginTransmission(adresse_i2c);
  Wire.write((adresse_registre >> 8) & 0xFF);
  Wire.write(adresse_registre & 0xFF);
  _I2C_write(donnees, taille_donnees);
  status = Wire.endTransmission(true);
  return status;
}

uint8_t I2C_read_register(int adresse_i2c, uint16_t adresse_registre, unsigned char * donnees_recues, unsigned int taille_donnees){
  int status;
  Wire.beginTransmission(adresse_i2c);
  Wire.write((adresse_registre >> 8) & 0xFF);
  Wire.write(adresse_registre & 0xFF);
  status = Wire.endTransmission(true);
  //Wire.beginTransmission(adresse_i2c);
  Wire.requestFrom(adresse_i2c, taille_donnees);
  //status = Wire.endTransmission(true);

  Wire.readBytes(donnees_recues, taille_donnees);
  return 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);
  return I2C_read_register(p_platform->address, RegisterAdress, p_value, 1);
	
}

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 */
  

  Wire.beginTransmission(p_platform->address);
  a = Wire.write((RegisterAdress >> 8) & 0xFF);
  b = Wire.write(RegisterAdress & 0xFF);
  c = Wire.write(value);
  status = Wire.endTransmission(true);
  if(status != 0){
    Serial.printf("WrByte adresse :%x - ",p_platform->address);
    Serial.printf("%d %d %d %d\n", a, b ,c, status);
  }

  return status;

}

uint8_t WrMulti(
		VL53L8CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_values,
		uint32_t size)
{
	uint8_t status = 255;
	
	return I2C_write_register(p_platform->address, RegisterAdress, p_values, size);

}

uint8_t RdMulti(
		VL53L8CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_values,
		uint32_t size)
{
	uint8_t status = 255;
	
	return I2C_read_register(p_platform->address, RegisterAdress, p_values, size);
	
}

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