/**
  *
  * 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 <stdlib.h>
#include <string.h>
#include "vl53l8cx_api.h"
#include "vl53l8cx_buffers.h"

/**
 * @brief Inner function, not available outside this file. This function is used
 * to wait for an answer from VL53L8CX sensor.
 */

static uint8_t _vl53l8cx_poll_for_answer(
		VL53L8CX_Configuration	*p_dev,
		uint8_t					size,
		uint8_t					pos,
		uint16_t				address,
		uint8_t					mask,
		uint8_t					expected_value)
{
	uint8_t status = VL53L8CX_STATUS_OK;
	uint8_t timeout = 0;

	do {
		status |= RdMulti(&(p_dev->platform), address,
				p_dev->temp_buffer, size);
		status |= WaitMs(&(p_dev->platform), 10);

		if(timeout >= (uint8_t)200)	/* 2s timeout */
		{
			status |= (uint8_t)VL53L8CX_STATUS_TIMEOUT_ERROR;
			break;
		}else if((size >= (uint8_t)4) 
                         && (p_dev->temp_buffer[2] >= (uint8_t)0x7f))
		{
			status |= VL53L8CX_MCU_ERROR;
			break;
		}
		else
		{
			timeout++;
		}
	}while ((p_dev->temp_buffer[pos] & mask) != expected_value);

	return status;
}

/*
 * Inner function, not available outside this file. This function is used to
 * wait for the MCU to boot.
 */
static uint8_t _vl53l8cx_poll_for_mcu_boot(
              VL53L8CX_Configuration      *p_dev)
{
   uint8_t go2_status0, go2_status1, status = VL53L8CX_STATUS_OK;
   uint16_t timeout = 0;

   do {
		status |= RdByte(&(p_dev->platform), 0x06, &go2_status0);
		if((go2_status0 & (uint8_t)0x80) != (uint8_t)0){
			status |= RdByte(&(p_dev->platform), 0x07, &go2_status1);
            if(go2_status1 & (uint8_t)0x01)
            {
            	status |= VL53L8CX_STATUS_OK;
    			break;
            }
		}
		(void)WaitMs(&(p_dev->platform), 1);
		timeout++;

		if((go2_status0 & (uint8_t)0x1) != (uint8_t)0){
			break;
		}
	}while (timeout < (uint16_t)500);

   return status;
}

/**
 * @brief Inner function, not available outside this file. This function is used
 * to set the offset data gathered from NVM.
 */

static uint8_t _vl53l8cx_send_offset_data(
		VL53L8CX_Configuration		*p_dev,
		uint8_t						resolution)
{
	uint8_t status = VL53L8CX_STATUS_OK;
	uint32_t signal_grid[64];
	int16_t range_grid[64];
	uint8_t dss_4x4[] = {0x0F, 0x04, 0x04, 0x00, 0x08, 0x10, 0x10, 0x07};
	uint8_t footer[] = {0x00, 0x00, 0x00, 0x0F, 0x03, 0x01, 0x01, 0xE4};
	int8_t i, j;
	uint16_t k;

	(void)memcpy(p_dev->temp_buffer,
               p_dev->offset_data, VL53L8CX_OFFSET_BUFFER_SIZE);

	/* Data extrapolation is required for 4X4 offset */
	if(resolution == (uint8_t)VL53L8CX_RESOLUTION_4X4){
		(void)memcpy(&(p_dev->temp_buffer[0x10]), dss_4x4, sizeof(dss_4x4));
		SwapBuffer(p_dev->temp_buffer, VL53L8CX_OFFSET_BUFFER_SIZE);
		(void)memcpy(signal_grid,&(p_dev->temp_buffer[0x3C]),
			sizeof(signal_grid));
		(void)memcpy(range_grid,&(p_dev->temp_buffer[0x140]),
			sizeof(range_grid));

		for (j = 0; j < (int8_t)4; j++)
		{
			for (i = 0; i < (int8_t)4 ; i++)
			{
				signal_grid[i+(4*j)] =
				(signal_grid[(2*i)+(16*j)+ (int8_t)0]
				+ signal_grid[(2*i)+(16*j)+(int8_t)1]
				+ signal_grid[(2*i)+(16*j)+(int8_t)8]
				+ signal_grid[(2*i)+(16*j)+(int8_t)9])
                                  /(uint32_t)4;
				range_grid[i+(4*j)] =
				(range_grid[(2*i)+(16*j)]
				+ range_grid[(2*i)+(16*j)+1]
				+ range_grid[(2*i)+(16*j)+8]
				+ range_grid[(2*i)+(16*j)+9])
                                  /(int16_t)4;
			}
		}
	    (void)memset(&range_grid[0x10], 0, (uint16_t)96);
	    (void)memset(&signal_grid[0x10], 0, (uint16_t)192);
            (void)memcpy(&(p_dev->temp_buffer[0x3C]),
		signal_grid, sizeof(signal_grid));
            (void)memcpy(&(p_dev->temp_buffer[0x140]),
		range_grid, sizeof(range_grid));
            SwapBuffer(p_dev->temp_buffer, VL53L8CX_OFFSET_BUFFER_SIZE);
	}

	for(k = 0; k < (VL53L8CX_OFFSET_BUFFER_SIZE - (uint16_t)4); k++)
	{
		p_dev->temp_buffer[k] = p_dev->temp_buffer[k + (uint16_t)8];
	}

	(void)memcpy(&(p_dev->temp_buffer[0x1E0]), footer, 8);
	status |= WrMulti(&(p_dev->platform), 0x2e18, p_dev->temp_buffer,
		VL53L8CX_OFFSET_BUFFER_SIZE);
	status |=_vl53l8cx_poll_for_answer(p_dev, 4, 1,
		VL53L8CX_UI_CMD_STATUS, 0xff, 0x03);

	return status;
}

/**
 * @brief Inner function, not available outside this file. This function is used
 * to set the Xtalk data from generic configuration, or user's calibration.
 */

static uint8_t _vl53l8cx_send_xtalk_data(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				resolution)
{
	uint8_t status = VL53L8CX_STATUS_OK;
	uint8_t res4x4[] = {0x0F, 0x04, 0x04, 0x17, 0x08, 0x10, 0x10, 0x07};
	uint8_t dss_4x4[] = {0x00, 0x78, 0x00, 0x08, 0x00, 0x00, 0x00, 0x08};
	uint8_t profile_4x4[] = {0xA0, 0xFC, 0x01, 0x00};
	uint32_t signal_grid[64];
	int8_t i, j;

	(void)memcpy(p_dev->temp_buffer, &(p_dev->xtalk_data[0]),
		VL53L8CX_XTALK_BUFFER_SIZE);

	/* Data extrapolation is required for 4X4 Xtalk */
	if(resolution == (uint8_t)VL53L8CX_RESOLUTION_4X4)
	{
		(void)memcpy(&(p_dev->temp_buffer[0x8]),
			res4x4, sizeof(res4x4));
		(void)memcpy(&(p_dev->temp_buffer[0x020]),
			dss_4x4, sizeof(dss_4x4));

		SwapBuffer(p_dev->temp_buffer, VL53L8CX_XTALK_BUFFER_SIZE);
		(void)memcpy(signal_grid, &(p_dev->temp_buffer[0x34]),
			sizeof(signal_grid));

		for (j = 0; j < (int8_t)4; j++)
		{
			for (i = 0; i < (int8_t)4 ; i++)
			{
				signal_grid[i+(4*j)] =
				(signal_grid[(2*i)+(16*j)+0]
				+ signal_grid[(2*i)+(16*j)+1]
				+ signal_grid[(2*i)+(16*j)+8]
				+ signal_grid[(2*i)+(16*j)+9])/(uint32_t)4;
			}
		}
	    (void)memset(&signal_grid[0x10], 0, (uint32_t)192);
	    (void)memcpy(&(p_dev->temp_buffer[0x34]),
                  signal_grid, sizeof(signal_grid));
	    SwapBuffer(p_dev->temp_buffer, VL53L8CX_XTALK_BUFFER_SIZE);
	    (void)memcpy(&(p_dev->temp_buffer[0x134]),
	    profile_4x4, sizeof(profile_4x4));
	    (void)memset(&(p_dev->temp_buffer[0x078]),0 ,
                         (uint32_t)4*sizeof(uint8_t));
	}

	status |= WrMulti(&(p_dev->platform), 0x2cf8,
			p_dev->temp_buffer, VL53L8CX_XTALK_BUFFER_SIZE);
	status |=_vl53l8cx_poll_for_answer(p_dev, 4, 1,
			VL53L8CX_UI_CMD_STATUS, 0xff, 0x03);

	return status;
}

uint8_t vl53l8cx_is_alive(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_is_alive)
{
	uint8_t status = VL53L8CX_STATUS_OK;
	uint8_t device_id, revision_id;

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

	if (!status) return status;

	if((device_id == (uint8_t)0xF0) && (revision_id == (uint8_t)0x0C))
	{
		*p_is_alive = 1;
	}
	else
	{
		*p_is_alive = 0;
	}

	return status;
}

uint8_t vl53l8cx_init(
		VL53L8CX_Configuration		*p_dev)
{
	uint8_t tmp, status = VL53L8CX_STATUS_OK;
	uint8_t pipe_ctrl[] = {VL53L8CX_NB_TARGET_PER_ZONE, 0x00, 0x01, 0x00};
	uint32_t single_range = 0x01;
	uint32_t crc_checksum = 0x00;

	p_dev->default_xtalk = (uint8_t*)VL53L8CX_DEFAULT_XTALK;
	p_dev->default_configuration = (uint8_t*)VL53L8CX_DEFAULT_CONFIGURATION;
	p_dev->is_auto_stop_enabled = (uint8_t)0x0;

	/* SW reboot sequence */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
	status |= WrByte(&(p_dev->platform), 0x0009, 0x04);
	status |= WrByte(&(p_dev->platform), 0x000F, 0x40);
	status |= WrByte(&(p_dev->platform), 0x000A, 0x03);
	status |= RdByte(&(p_dev->platform), 0x7FFF, &tmp);
	status |= WrByte(&(p_dev->platform), 0x000C, 0x01);

	status |= WrByte(&(p_dev->platform), 0x0101, 0x00);
	status |= WrByte(&(p_dev->platform), 0x0102, 0x00);
	status |= WrByte(&(p_dev->platform), 0x010A, 0x01);
	status |= WrByte(&(p_dev->platform), 0x4002, 0x01);
	status |= WrByte(&(p_dev->platform), 0x4002, 0x00);
	status |= WrByte(&(p_dev->platform), 0x010A, 0x03);
	status |= WrByte(&(p_dev->platform), 0x0103, 0x01);
	status |= WrByte(&(p_dev->platform), 0x000C, 0x00);
	status |= WrByte(&(p_dev->platform), 0x000F, 0x43);
	status |= WaitMs(&(p_dev->platform), 1);

	status |= WrByte(&(p_dev->platform), 0x000F, 0x40);
	status |= WrByte(&(p_dev->platform), 0x000A, 0x01);
	status |= WaitMs(&(p_dev->platform), 100);

	/* Wait for sensor booted (several ms required to get sensor ready ) */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
	status |= _vl53l8cx_poll_for_answer(p_dev, 1, 0, 0x06, 0xff, 1);
	if(status != (uint8_t)0){
		goto exit;
	}
	
	status |= WrByte(&(p_dev->platform), 0x000E, 0x01);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

	/* Enable FW access */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
	status |= WrByte(&(p_dev->platform), 0x06, 0x01);
	status |= _vl53l8cx_poll_for_answer(p_dev, 1, 0, 0x21, 0xFF, 0x4);

	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);

	/* Enable host access to GO1 */
	status |= RdByte(&(p_dev->platform), 0x7fff, &tmp);
	status |= WrByte(&(p_dev->platform), 0x0C, 0x01);

	/* Power ON status */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
	status |= WrByte(&(p_dev->platform), 0x101, 0x00);
	status |= WrByte(&(p_dev->platform), 0x102, 0x00);
	status |= WrByte(&(p_dev->platform), 0x010A, 0x01);
	status |= WrByte(&(p_dev->platform), 0x4002, 0x01);
	status |= WrByte(&(p_dev->platform), 0x4002, 0x00);
	status |= WrByte(&(p_dev->platform), 0x010A, 0x03);
	status |= WrByte(&(p_dev->platform), 0x103, 0x01);
	status |= WrByte(&(p_dev->platform), 0x400F, 0x00);
	status |= WrByte(&(p_dev->platform), 0x21A, 0x43);
	status |= WrByte(&(p_dev->platform), 0x21A, 0x03);
	status |= WrByte(&(p_dev->platform), 0x21A, 0x01);
	status |= WrByte(&(p_dev->platform), 0x21A, 0x00);
	status |= WrByte(&(p_dev->platform), 0x219, 0x00);
	status |= WrByte(&(p_dev->platform), 0x21B, 0x00);

	/* Wake up MCU */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
	status |= RdByte(&(p_dev->platform), 0x7fff, &tmp);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
	status |= WrByte(&(p_dev->platform), 0x20, 0x07);
	status |= WrByte(&(p_dev->platform), 0x20, 0x06);

	/* Download FW into VL53L8CX */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x09);
	status |= WrMulti(&(p_dev->platform),0,
		(uint8_t*)&VL53L8CX_FIRMWARE[0],0x8000);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x0a);
	status |= WrMulti(&(p_dev->platform),0,
		(uint8_t*)&VL53L8CX_FIRMWARE[0x8000],0x8000);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x0b);
	status |= WrMulti(&(p_dev->platform),0,
		(uint8_t*)&VL53L8CX_FIRMWARE[0x10000],0x5000);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);

	/* Check if FW correctly downloaded */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x01);
	status |= WrByte(&(p_dev->platform), 0x06, 0x03);

	status |= WaitMs(&(p_dev->platform), 5);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
	status |= RdByte(&(p_dev->platform), 0x7fff, &tmp);
	status |= WrByte(&(p_dev->platform), 0x0C, 0x01);

	/* Reset MCU and wait boot */
	status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
	status |= WrByte(&(p_dev->platform), 0x114, 0x00);
	status |= WrByte(&(p_dev->platform), 0x115, 0x00);
	status |= WrByte(&(p_dev->platform), 0x116, 0x42);
	status |= WrByte(&(p_dev->platform), 0x117, 0x00);
	status |= WrByte(&(p_dev->platform), 0x0B, 0x00);
	status |= RdByte(&(p_dev->platform), 0x7fff, &tmp);
	status |= WrByte(&(p_dev->platform), 0x0C, 0x00);
	status |= WrByte(&(p_dev->platform), 0x0B, 0x01);

	status |= _vl53l8cx_poll_for_mcu_boot(p_dev);
	if(status != (uint8_t)0){
		goto exit;
	}

	status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

	/* Firmware checksum */
	status |= RdMulti(&(p_dev->platform), (uint16_t)(0x812FFC & 0xFFFF),
			p_dev->temp_buffer, 4);
	SwapBuffer(p_dev->temp_buffer, 4);
	memcpy((uint8_t*)&crc_checksum, &(p_dev->temp_buffer[0]), 4);
	if (crc_checksum != (uint32_t)0xee922b77)
	{
		status |= VL53L8CX_STATUS_FW_CHECKSUM_FAIL;
	}
	//crc_checksum = *((uint32_t *)&p_dev->temp_buffer[0]);

	/* Get offset NVM data and store them into the offset buffer */
	status |= WrMulti(&(p_dev->platform), 0x2fd8,
		(uint8_t*)VL53L8CX_GET_NVM_CMD, sizeof(VL53L8CX_GET_NVM_CMD));
	status |= _vl53l8cx_poll_for_answer(p_dev, 4, 0,
		VL53L8CX_UI_CMD_STATUS, 0xff, 2);
	status |= RdMulti(&(p_dev->platform), VL53L8CX_UI_CMD_START,
		p_dev->temp_buffer, VL53L8CX_NVM_DATA_SIZE);
	(void)memcpy(p_dev->offset_data, p_dev->temp_buffer,
		VL53L8CX_OFFSET_BUFFER_SIZE);
	status |= _vl53l8cx_send_offset_data(p_dev, VL53L8CX_RESOLUTION_4X4);

	/* Set default Xtalk shape. Send Xtalk to sensor */
	(void)memcpy(p_dev->xtalk_data, (uint8_t*)VL53L8CX_DEFAULT_XTALK,
		VL53L8CX_XTALK_BUFFER_SIZE);
	status |= _vl53l8cx_send_xtalk_data(p_dev, VL53L8CX_RESOLUTION_4X4);

	/* Send default configuration to VL53L8CX firmware */
	status |= WrMulti(&(p_dev->platform), 0x2c34,
		p_dev->default_configuration,
		sizeof(VL53L8CX_DEFAULT_CONFIGURATION));
	status |= _vl53l8cx_poll_for_answer(p_dev, 4, 1,
		VL53L8CX_UI_CMD_STATUS, 0xff, 0x03);

	status |= vl53l8cx_dci_write_data(p_dev, (uint8_t*)&pipe_ctrl,
		VL53L8CX_DCI_PIPE_CONTROL, (uint16_t)sizeof(pipe_ctrl));

#if VL53L8CX_NB_TARGET_PER_ZONE != 1
	tmp = VL53L8CX_NB_TARGET_PER_ZONE;
	status |= vl53l8cx_dci_replace_data(p_dev, p_dev->temp_buffer,
		VL53L8CX_DCI_FW_NB_TARGET, 16,
	(uint8_t*)&tmp, 1, 0x0C);
#endif

	status |= vl53l8cx_dci_write_data(p_dev, (uint8_t*)&single_range,
			VL53L8CX_DCI_SINGLE_RANGE,
			(uint16_t)sizeof(single_range));

exit:
	return status;
}

uint8_t vl53l8cx_set_i2c_address(
		VL53L8CX_Configuration		*p_dev,
		uint16_t		        i2c_address)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
	status |= WrByte(&(p_dev->platform), 0x4, (uint8_t)(i2c_address >> 1));
	p_dev->platform.address = i2c_address;
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

	return status;
}

uint8_t vl53l8cx_get_power_mode(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_power_mode)
{
	uint8_t tmp, status = VL53L8CX_STATUS_OK;

	status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
	status |= RdByte(&(p_dev->platform), 0x009, &tmp);

	switch(tmp)
	{
		case 0x4:
			*p_power_mode = VL53L8CX_POWER_MODE_WAKEUP;
			break;
		case 0x2:
			*p_power_mode = VL53L8CX_POWER_MODE_SLEEP;

			break;
		default:
			*p_power_mode = 0;
			status = VL53L8CX_STATUS_ERROR;
			break;
	}

	status |= WrByte(&(p_dev->platform), 0x7FFF, 0x02);

	return status;
}

uint8_t vl53l8cx_set_power_mode(
		VL53L8CX_Configuration		*p_dev,
		uint8_t			        power_mode)
{
	uint8_t current_power_mode, status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_get_power_mode(p_dev, &current_power_mode);
	if(power_mode != current_power_mode)
	{
	switch(power_mode)
	{
		case VL53L8CX_POWER_MODE_WAKEUP:
			status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
			status |= WrByte(&(p_dev->platform), 0x09, 0x04);
			status |= _vl53l8cx_poll_for_answer(
						p_dev, 1, 0, 0x06, 0x01, 1);
			break;

		case VL53L8CX_POWER_MODE_SLEEP:
			status |= WrByte(&(p_dev->platform), 0x7FFF, 0x00);
			status |= WrByte(&(p_dev->platform), 0x09, 0x02);
			status |= _vl53l8cx_poll_for_answer(
						p_dev, 1, 0, 0x06, 0x01, 0);
			break;

		default:
			status = VL53L8CX_STATUS_ERROR;
			break;
		}
		status |= WrByte(&(p_dev->platform), 0x7FFF, 0x02);
	}

	return status;
}

uint8_t vl53l8cx_start_ranging(
		VL53L8CX_Configuration		*p_dev)
{
	uint8_t resolution, status = VL53L8CX_STATUS_OK;
	uint16_t tmp;
	uint32_t i;
	uint32_t header_config[2] = {0, 0};

	union Block_header *bh_ptr;
	uint8_t cmd[] = {0x00, 0x03, 0x00, 0x00};

	status |= vl53l8cx_get_resolution(p_dev, &resolution);
	p_dev->data_read_size = 0;
	p_dev->streamcount = 255;

	/* Enable mandatory output (meta and common data) */
	uint32_t output_bh_enable[] = {
		0x00000007U,
		0x00000000U,
		0x00000000U,
		0xC0000000U};

	/* Send addresses of possible output */
	uint32_t output[] ={VL53L8CX_START_BH,
		VL53L8CX_METADATA_BH,
		VL53L8CX_COMMONDATA_BH,
		VL53L8CX_AMBIENT_RATE_BH,
		VL53L8CX_SPAD_COUNT_BH,
		VL53L8CX_NB_TARGET_DETECTED_BH,
		VL53L8CX_SIGNAL_RATE_BH,
		VL53L8CX_RANGE_SIGMA_MM_BH,
		VL53L8CX_DISTANCE_BH,
		VL53L8CX_REFLECTANCE_BH,
		VL53L8CX_TARGET_STATUS_BH,
		VL53L8CX_MOTION_DETECT_BH};

	/* Enable selected outputs in the 'platform.h' file */
#ifndef VL53L8CX_DISABLE_AMBIENT_PER_SPAD
	output_bh_enable[0] += (uint32_t)8;
#endif
#ifndef VL53L8CX_DISABLE_NB_SPADS_ENABLED
	output_bh_enable[0] += (uint32_t)16;
#endif
#ifndef VL53L8CX_DISABLE_NB_TARGET_DETECTED
	output_bh_enable[0] += (uint32_t)32;
#endif
#ifndef VL53L8CX_DISABLE_SIGNAL_PER_SPAD
	output_bh_enable[0] += (uint32_t)64;
#endif
#ifndef VL53L8CX_DISABLE_RANGE_SIGMA_MM
	output_bh_enable[0] += (uint32_t)128;
#endif
#ifndef VL53L8CX_DISABLE_DISTANCE_MM
	output_bh_enable[0] += (uint32_t)256;
#endif
#ifndef VL53L8CX_DISABLE_REFLECTANCE_PERCENT
	output_bh_enable[0] += (uint32_t)512;
#endif
#ifndef VL53L8CX_DISABLE_TARGET_STATUS
	output_bh_enable[0] += (uint32_t)1024;
#endif
#ifndef VL53L8CX_DISABLE_MOTION_INDICATOR
	output_bh_enable[0] += (uint32_t)2048;
#endif

	/* Update data size */
	for (i = 0; i < (uint32_t)(sizeof(output)/sizeof(uint32_t)); i++)
	{
		if ((output[i] == (uint8_t)0) 
                    || ((output_bh_enable[i/(uint32_t)32]
                         &((uint32_t)1 << (i%(uint32_t)32))) == (uint32_t)0))
		{
			continue;
		}

		bh_ptr = (union Block_header *)&(output[i]);
		if (((uint8_t)bh_ptr->type >= (uint8_t)0x1) 
                    && ((uint8_t)bh_ptr->type < (uint8_t)0x0d))
		{
			if ((bh_ptr->idx >= (uint16_t)0x54d0) 
                            && (bh_ptr->idx < (uint16_t)(0x54d0 + 960)))
			{
				bh_ptr->size = resolution;
			}
			else
			{
				bh_ptr->size = (uint16_t)((uint16_t)resolution
                                  * (uint16_t)VL53L8CX_NB_TARGET_PER_ZONE);
			}
			p_dev->data_read_size += bh_ptr->type * bh_ptr->size;
		}
		else
		{
			p_dev->data_read_size += bh_ptr->size;
		}
		p_dev->data_read_size += (uint32_t)4;
	}
	p_dev->data_read_size += (uint32_t)24;

	status |= vl53l8cx_dci_write_data(p_dev,
			(uint8_t*)&(output), VL53L8CX_DCI_OUTPUT_LIST,
			(uint16_t)sizeof(output));

	header_config[0] = p_dev->data_read_size;
	header_config[1] = i + (uint32_t)1;

	status |= vl53l8cx_dci_write_data(p_dev,
			(uint8_t*)&(header_config), VL53L8CX_DCI_OUTPUT_CONFIG,
			(uint16_t)sizeof(header_config));

	status |= vl53l8cx_dci_write_data(p_dev,
			(uint8_t*)&(output_bh_enable), VL53L8CX_DCI_OUTPUT_ENABLES,
			(uint16_t)sizeof(output_bh_enable));

	/* Start xshut bypass (interrupt mode) */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
	status |= WrByte(&(p_dev->platform), 0x09, 0x05);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

	/* Start ranging session */
	status |= WrMulti(&(p_dev->platform), VL53L8CX_UI_CMD_END -
			(uint16_t)(4 - 1), (uint8_t*)cmd, sizeof(cmd));
	status |= _vl53l8cx_poll_for_answer(p_dev, 4, 1,
			VL53L8CX_UI_CMD_STATUS, 0xff, 0x03);

	/* Read ui range data content and compare if data size is the correct one */
	status |= vl53l8cx_dci_read_data(p_dev,
			(uint8_t*)p_dev->temp_buffer, 0x5440, 12);
	(void)memcpy(&tmp, &(p_dev->temp_buffer[0x8]), sizeof(tmp));
	if(tmp != p_dev->data_read_size)
	{
		status |= VL53L8CX_STATUS_ERROR;
	}

	/* Ensure that there is no laser safety fault */
	status |= vl53l8cx_dci_read_data(p_dev,
			(uint8_t*)p_dev->temp_buffer, 0xE0C4, 8);
	if((uint8_t)p_dev->temp_buffer[0x6] != (uint8_t)0)
	{
		status |= VL53L8CX_STATUS_LASER_SAFETY;
	}

	return status;
}

uint8_t vl53l8cx_stop_ranging(
		VL53L8CX_Configuration		*p_dev)
{
	uint8_t tmp = 0, status = VL53L8CX_STATUS_OK;
	uint16_t timeout = 0;
	uint32_t auto_stop_flag = 0;

	status |= RdMulti(&(p_dev->platform),
                          0x2FFC, (uint8_t*)&auto_stop_flag, 4);
	if((auto_stop_flag != (uint32_t)0x4FF)
			&& (p_dev->is_auto_stop_enabled == (uint8_t)0))
	{
	        status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);

	        /* Provoke MCU stop */
	        status |= WrByte(&(p_dev->platform), 0x15, 0x16);
	        status |= WrByte(&(p_dev->platform), 0x14, 0x01);

	        /* Poll for G02 status 0 MCU stop */
	        while(((tmp & (uint8_t)0x80) >> 7) == (uint8_t)0x00)
	        {
	        	status |= RdByte(&(p_dev->platform), 0x6, &tmp);
	        	status |= WaitMs(&(p_dev->platform), 10);
	        	timeout++;	/* Timeout reached after 5 seconds */

	        	if(timeout > (uint16_t)500)
				{
					status |= tmp;
					break;
				}
        	}
        }

	/* Check GO2 status 1 if status is still OK */
	status |= RdByte(&(p_dev->platform), 0x6, &tmp);
	if((tmp & (uint8_t)0x80) != (uint8_t)0){
		status |= RdByte(&(p_dev->platform), 0x7, &tmp);
		if((tmp != (uint8_t)0x84) && (tmp != (uint8_t)0x85)){
		   status |= tmp;
		}
	}

	/* Undo MCU stop */
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x00);
	status |= WrByte(&(p_dev->platform), 0x14, 0x00);
	status |= WrByte(&(p_dev->platform), 0x15, 0x00);

	/* Stop xshut bypass */
	status |= WrByte(&(p_dev->platform), 0x09, 0x04);
	status |= WrByte(&(p_dev->platform), 0x7fff, 0x02);

	return status;
}

uint8_t vl53l8cx_check_data_ready(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_isReady)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= RdMulti(&(p_dev->platform), 0x0, p_dev->temp_buffer, 4);

	if((p_dev->temp_buffer[0] != p_dev->streamcount)
			&& (p_dev->temp_buffer[0] != (uint8_t)255)
			&& (p_dev->temp_buffer[1] == (uint8_t)0x5)
			&& ((p_dev->temp_buffer[2] & (uint8_t)0x5) == (uint8_t)0x5)
			&& ((p_dev->temp_buffer[3] & (uint8_t)0x10) ==(uint8_t)0x10)
			)
	{
		*p_isReady = (uint8_t)1;
		 p_dev->streamcount = p_dev->temp_buffer[0];
	}
	else
	{
        if ((p_dev->temp_buffer[3] & (uint8_t)0x80) != (uint8_t)0)
        {
        	status |= p_dev->temp_buffer[2];	/* Return GO2 error status */
        }

		*p_isReady = 0;
	}

	return status;
}

uint8_t vl53l8cx_get_ranging_data(
		VL53L8CX_Configuration		*p_dev,
		VL53L8CX_ResultsData		*p_results)
{
	uint8_t status = VL53L8CX_STATUS_OK;
	uint16_t header_id, footer_id;
	union Block_header *bh_ptr;
	uint32_t i, j, msize;
	status |= RdMulti(&(p_dev->platform), 0x0,
			p_dev->temp_buffer, p_dev->data_read_size);
	p_dev->streamcount = p_dev->temp_buffer[0];
	SwapBuffer(p_dev->temp_buffer, (uint16_t)p_dev->data_read_size);

	/* Start conversion at position 16 to avoid headers */
	for (i = (uint32_t)16; i 
             < (uint32_t)p_dev->data_read_size; i+=(uint32_t)4)
	{
		bh_ptr = (union Block_header *)&(p_dev->temp_buffer[i]);
		if ((bh_ptr->type > (uint32_t)0x1) 
                    && (bh_ptr->type < (uint32_t)0xd))
		{
			msize = bh_ptr->type * bh_ptr->size;
		}
		else
		{
			msize = bh_ptr->size;
		}

		switch(bh_ptr->idx){
			case VL53L8CX_METADATA_IDX:
				p_results->silicon_temp_degc =
						(int8_t)p_dev->temp_buffer[i + (uint32_t)12];
				break;

#ifndef VL53L8CX_DISABLE_AMBIENT_PER_SPAD
			case VL53L8CX_AMBIENT_RATE_IDX:
				(void)memcpy(p_results->ambient_per_spad,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
#ifndef VL53L8CX_DISABLE_NB_SPADS_ENABLED
			case VL53L8CX_SPAD_COUNT_IDX:
				(void)memcpy(p_results->nb_spads_enabled,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
#ifndef VL53L8CX_DISABLE_NB_TARGET_DETECTED
			case VL53L8CX_NB_TARGET_DETECTED_IDX:
				(void)memcpy(p_results->nb_target_detected,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
#ifndef VL53L8CX_DISABLE_SIGNAL_PER_SPAD
			case VL53L8CX_SIGNAL_RATE_IDX:
				(void)memcpy(p_results->signal_per_spad,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
#ifndef VL53L8CX_DISABLE_RANGE_SIGMA_MM
			case VL53L8CX_RANGE_SIGMA_MM_IDX:
				(void)memcpy(p_results->range_sigma_mm,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
#ifndef VL53L8CX_DISABLE_DISTANCE_MM
			case VL53L8CX_DISTANCE_IDX:
				(void)memcpy(p_results->distance_mm,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
#ifndef VL53L8CX_DISABLE_REFLECTANCE_PERCENT
			case VL53L8CX_REFLECTANCE_EST_PC_IDX:
				(void)memcpy(p_results->reflectance,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
#ifndef VL53L8CX_DISABLE_TARGET_STATUS
			case VL53L8CX_TARGET_STATUS_IDX:
				(void)memcpy(p_results->target_status,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
#ifndef VL53L8CX_DISABLE_MOTION_INDICATOR
			case VL53L8CX_MOTION_DETEC_IDX:
				(void)memcpy(&p_results->motion_indicator,
				&(p_dev->temp_buffer[i + (uint32_t)4]), msize);
				break;
#endif
			default:
				break;
		}
		i += msize;
	}

#ifndef VL53L8CX_USE_RAW_FORMAT

	/* Convert data into their real format */
#ifndef VL53L8CX_DISABLE_AMBIENT_PER_SPAD
	for(i = 0; i < (uint32_t)VL53L8CX_RESOLUTION_8X8; i++)
	{
		p_results->ambient_per_spad[i] /= (uint32_t)2048;
	}
#endif

	for(i = 0; i < (uint32_t)(VL53L8CX_RESOLUTION_8X8
			*VL53L8CX_NB_TARGET_PER_ZONE); i++)
	{
#ifndef VL53L8CX_DISABLE_DISTANCE_MM
		p_results->distance_mm[i] /= 4;
#endif
#ifndef VL53L8CX_DISABLE_REFLECTANCE_PERCENT
		p_results->reflectance[i] /= (uint8_t)2;
#endif
#ifndef VL53L8CX_DISABLE_RANGE_SIGMA_MM
		p_results->range_sigma_mm[i] /= (uint16_t)128;
#endif
#ifndef VL53L8CX_DISABLE_SIGNAL_PER_SPAD
		p_results->signal_per_spad[i] /= (uint32_t)2048;
#endif
	}

	/* Set target status to 255 if no target is detected for this zone */
#ifndef VL53L8CX_DISABLE_NB_TARGET_DETECTED
	for(i = 0; i < (uint32_t)VL53L8CX_RESOLUTION_8X8; i++)
	{
		if(p_results->nb_target_detected[i] == (uint8_t)0){
			for(j = 0; j < (uint32_t)
				VL53L8CX_NB_TARGET_PER_ZONE; j++)
			{
#ifndef VL53L8CX_DISABLE_TARGET_STATUS
				p_results->target_status
				[((uint32_t)VL53L8CX_NB_TARGET_PER_ZONE
					*(uint32_t)i) + j]=(uint8_t)255;
#endif
			}
		}
	}
#endif

#ifndef VL53L8CX_DISABLE_MOTION_INDICATOR
	for(i = 0; i < (uint32_t)32; i++)
	{
		p_results->motion_indicator.motion[i] /= (uint32_t)65535;
	}
#endif

#endif

	/* Check if footer id and header id are matching. This allows to detect
	 * corrupted frames */
	header_id = ((uint16_t)(p_dev->temp_buffer[0x8])<<8) & 0xFF00U;
	header_id |= ((uint16_t)(p_dev->temp_buffer[0x9])) & 0x00FFU;

	footer_id = ((uint16_t)(p_dev->temp_buffer[p_dev->data_read_size
		- (uint32_t)4]) << 8) & 0xFF00U;
	footer_id |= ((uint16_t)(p_dev->temp_buffer[p_dev->data_read_size
		- (uint32_t)3])) & 0xFFU;

	if(header_id != footer_id)
	{
		status |= VL53L8CX_STATUS_CORRUPTED_FRAME;
	}

	return status;
}

uint8_t vl53l8cx_get_resolution(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_resolution)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_read_data(p_dev, p_dev->temp_buffer,
			VL53L8CX_DCI_ZONE_CONFIG, 8);
	*p_resolution = p_dev->temp_buffer[0x00]*p_dev->temp_buffer[0x01];

	return status;
}



uint8_t vl53l8cx_set_resolution(
		VL53L8CX_Configuration 		 *p_dev,
		uint8_t				resolution)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	switch(resolution){
		case VL53L8CX_RESOLUTION_4X4:
			status |= vl53l8cx_dci_read_data(p_dev,
					p_dev->temp_buffer,
					VL53L8CX_DCI_DSS_CONFIG, 16);
			p_dev->temp_buffer[0x04] = 64;
			p_dev->temp_buffer[0x06] = 64;
			p_dev->temp_buffer[0x09] = 4;
			status |= vl53l8cx_dci_write_data(p_dev,
					p_dev->temp_buffer,
					VL53L8CX_DCI_DSS_CONFIG, 16);

			status |= vl53l8cx_dci_read_data(p_dev,
					p_dev->temp_buffer,
					VL53L8CX_DCI_ZONE_CONFIG, 8);
			p_dev->temp_buffer[0x00] = 4;
			p_dev->temp_buffer[0x01] = 4;
			p_dev->temp_buffer[0x04] = 8;
			p_dev->temp_buffer[0x05] = 8;
			status |= vl53l8cx_dci_write_data(p_dev,
					p_dev->temp_buffer,
					VL53L8CX_DCI_ZONE_CONFIG, 8);
			break;

		case VL53L8CX_RESOLUTION_8X8:
			status |= vl53l8cx_dci_read_data(p_dev,
					p_dev->temp_buffer,
					VL53L8CX_DCI_DSS_CONFIG, 16);
			p_dev->temp_buffer[0x04] = 16;
			p_dev->temp_buffer[0x06] = 16;
			p_dev->temp_buffer[0x09] = 1;
			status |= vl53l8cx_dci_write_data(p_dev,
					p_dev->temp_buffer,
					VL53L8CX_DCI_DSS_CONFIG, 16);

			status |= vl53l8cx_dci_read_data(p_dev,
					p_dev->temp_buffer,
					VL53L8CX_DCI_ZONE_CONFIG, 8);
			p_dev->temp_buffer[0x00] = 8;
			p_dev->temp_buffer[0x01] = 8;
			p_dev->temp_buffer[0x04] = 4;
			p_dev->temp_buffer[0x05] = 4;
			status |= vl53l8cx_dci_write_data(p_dev,
					p_dev->temp_buffer,
					VL53L8CX_DCI_ZONE_CONFIG, 8);

			break;

		default:
			status = VL53L8CX_STATUS_INVALID_PARAM;
			break;
		}

	status |= _vl53l8cx_send_offset_data(p_dev, resolution);
	status |= _vl53l8cx_send_xtalk_data(p_dev, resolution);

	return status;
}

uint8_t vl53l8cx_get_ranging_frequency_hz(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_frequency_hz)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_read_data(p_dev, (uint8_t*)p_dev->temp_buffer,
			VL53L8CX_DCI_FREQ_HZ, 4);
	*p_frequency_hz = p_dev->temp_buffer[0x01];

	return status;
}

uint8_t vl53l8cx_set_ranging_frequency_hz(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				frequency_hz)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_replace_data(p_dev, p_dev->temp_buffer,
					VL53L8CX_DCI_FREQ_HZ, 4,
					(uint8_t*)&frequency_hz, 1, 0x01);

	return status;
}

uint8_t vl53l8cx_get_integration_time_ms(
		VL53L8CX_Configuration		*p_dev,
		uint32_t			*p_time_ms)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_read_data(p_dev, (uint8_t*)p_dev->temp_buffer,
			VL53L8CX_DCI_INT_TIME, 20);

	(void)memcpy(p_time_ms, &(p_dev->temp_buffer[0x0]), 4);
	*p_time_ms /= (uint32_t)1000;

	return status;
}

uint8_t vl53l8cx_set_integration_time_ms(
		VL53L8CX_Configuration		*p_dev,
		uint32_t			integration_time_ms)
{
	uint8_t status = VL53L8CX_STATUS_OK;
        uint32_t integration = integration_time_ms;

	/* Integration time must be between 2ms and 1000ms */
	if((integration < (uint32_t)2)
           || (integration > (uint32_t)1000))
	{
		status |= VL53L8CX_STATUS_INVALID_PARAM;
	}else
	{
		integration *= (uint32_t)1000;

		status |= vl53l8cx_dci_replace_data(p_dev, p_dev->temp_buffer,
				VL53L8CX_DCI_INT_TIME, 20,
				(uint8_t*)&integration, 4, 0x00);
	}

	return status;
}

uint8_t vl53l8cx_get_sharpener_percent(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_sharpener_percent)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_read_data(p_dev,p_dev->temp_buffer,
			VL53L8CX_DCI_SHARPENER, 16);

	*p_sharpener_percent = (p_dev->temp_buffer[0xD]
                                *(uint8_t)100)/(uint8_t)255;

	return status;
}

uint8_t vl53l8cx_set_sharpener_percent(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				sharpener_percent)
{
	uint8_t status = VL53L8CX_STATUS_OK;
        uint8_t sharpener;

	if(sharpener_percent >= (uint8_t)100)
	{
		status |= VL53L8CX_STATUS_INVALID_PARAM;
	}
	else
	{
		sharpener = (sharpener_percent*(uint8_t)255)/(uint8_t)100;
		status |= vl53l8cx_dci_replace_data(p_dev, p_dev->temp_buffer,
				VL53L8CX_DCI_SHARPENER, 16,
                                (uint8_t*)&sharpener, 1, 0xD);
	}

	return status;
}

uint8_t vl53l8cx_get_target_order(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_target_order)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_read_data(p_dev, (uint8_t*)p_dev->temp_buffer,
			VL53L8CX_DCI_TARGET_ORDER, 4);
	*p_target_order = (uint8_t)p_dev->temp_buffer[0x0];

	return status;
}

uint8_t vl53l8cx_set_target_order(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				target_order)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	if((target_order == (uint8_t)VL53L8CX_TARGET_ORDER_CLOSEST)
		|| (target_order == (uint8_t)VL53L8CX_TARGET_ORDER_STRONGEST))
	{
		status |= vl53l8cx_dci_replace_data(p_dev, p_dev->temp_buffer,
				VL53L8CX_DCI_TARGET_ORDER, 4,
                                (uint8_t*)&target_order, 1, 0x0);
	}else
	{
		status |= VL53L8CX_STATUS_INVALID_PARAM;
	}

	return status;
}

uint8_t vl53l8cx_get_ranging_mode(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_ranging_mode)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_read_data(p_dev, p_dev->temp_buffer,
			VL53L8CX_DCI_RANGING_MODE, 8);

	if(p_dev->temp_buffer[0x01] == (uint8_t)0x1)
	{
		*p_ranging_mode = VL53L8CX_RANGING_MODE_CONTINUOUS;
	}
	else
	{
		*p_ranging_mode = VL53L8CX_RANGING_MODE_AUTONOMOUS;
	}

	return status;
}

uint8_t vl53l8cx_set_ranging_mode(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				ranging_mode)
{
	uint8_t status = VL53L8CX_STATUS_OK;
	uint32_t single_range = 0x00;

	status |= vl53l8cx_dci_read_data(p_dev, p_dev->temp_buffer,
			VL53L8CX_DCI_RANGING_MODE, 8);

	switch(ranging_mode)
	{
		case VL53L8CX_RANGING_MODE_CONTINUOUS:
			p_dev->temp_buffer[0x01] = 0x1;
			p_dev->temp_buffer[0x03] = 0x3;
			single_range = 0x00;
			break;

		case VL53L8CX_RANGING_MODE_AUTONOMOUS:
			p_dev->temp_buffer[0x01] = 0x3;
			p_dev->temp_buffer[0x03] = 0x2;
			single_range = 0x01;
			break;

		default:
			status = VL53L8CX_STATUS_INVALID_PARAM;
			break;
	}

	status |= vl53l8cx_dci_write_data(p_dev, p_dev->temp_buffer,
			VL53L8CX_DCI_RANGING_MODE, (uint16_t)8);

	status |= vl53l8cx_dci_write_data(p_dev, (uint8_t*)&single_range,
			VL53L8CX_DCI_SINGLE_RANGE,
                        (uint16_t)sizeof(single_range));

	return status;
}

uint8_t vl53l8cx_get_external_sync_pin_enable(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*p_is_sync_pin_enabled)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_read_data(p_dev, p_dev->temp_buffer,
			VL53L8CX_DCI_SYNC_PIN, 4);

	/* Check bit 1 value (get sync pause bit) */
	if((p_dev->temp_buffer[3] & (uint8_t)0x2) != (uint8_t)0)
	{
		*p_is_sync_pin_enabled = (uint8_t)1;
	}
	else
	{
		*p_is_sync_pin_enabled = (uint8_t)0;
	}

	return status;
}

uint8_t vl53l8cx_set_external_sync_pin_enable(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				enable_sync_pin)
{
	uint8_t status = VL53L8CX_STATUS_OK;
        uint32_t tmp;

	status |= vl53l8cx_dci_read_data(p_dev, p_dev->temp_buffer,
			VL53L8CX_DCI_SYNC_PIN, 4);
        tmp = (uint32_t)p_dev->temp_buffer[3];

	/* Update bit 1 with mask (set sync pause bit) */
	if(enable_sync_pin == (uint8_t)0)
	{
                tmp &= ~(1UL << 1);

	}
	else
	{
                tmp |= 1UL << 1;
	}

        p_dev->temp_buffer[3] = (uint8_t)tmp;
	status |= vl53l8cx_dci_write_data(p_dev, p_dev->temp_buffer,
			VL53L8CX_DCI_SYNC_PIN, 4);

	return status;
}

uint8_t vl53l8cx_dci_read_data(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*data,
		uint32_t			index,
		uint16_t			data_size)
{
	int16_t i;
	uint8_t status = VL53L8CX_STATUS_OK;
        uint32_t rd_size = (uint32_t) data_size + (uint32_t)12;
	uint8_t cmd[] = {0x00, 0x00, 0x00, 0x00,
			0x00, 0x00, 0x00, 0x0f,
			0x00, 0x02, 0x00, 0x08};

	/* Check if tmp buffer is large enough */
	if((data_size + (uint16_t)12)>(uint16_t)VL53L8CX_TEMPORARY_BUFFER_SIZE)
	{
		status |= VL53L8CX_STATUS_ERROR;
	}
	else
	{
		cmd[0] = (uint8_t)(index >> 8);	
		cmd[1] = (uint8_t)(index & (uint32_t)0xff);			
		cmd[2] = (uint8_t)((data_size & (uint16_t)0xff0) >> 4);
		cmd[3] = (uint8_t)((data_size & (uint16_t)0xf) << 4);

	/* Request data reading from FW */
		status |= WrMulti(&(p_dev->platform),
			(VL53L8CX_UI_CMD_END-(uint16_t)11),cmd, sizeof(cmd));
		status |= _vl53l8cx_poll_for_answer(p_dev, 4, 1,
			VL53L8CX_UI_CMD_STATUS,
			0xff, 0x03);

	/* Read new data sent (4 bytes header + data_size + 8 bytes footer) */
		status |= RdMulti(&(p_dev->platform), VL53L8CX_UI_CMD_START,
			p_dev->temp_buffer, rd_size);
		SwapBuffer(p_dev->temp_buffer, data_size + (uint16_t)12);

	/* Copy data from FW into input structure (-4 bytes to remove header) */
		for(i = 0 ; i < (int16_t)data_size;i++){
			data[i] = p_dev->temp_buffer[i + 4];
		}
	}

	return status;
}

uint8_t vl53l8cx_dci_write_data(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*data,
		uint32_t			index,
		uint16_t			data_size)
{
	uint8_t status = VL53L8CX_STATUS_OK;
	int16_t i;

	uint8_t headers[] = {0x00, 0x00, 0x00, 0x00};
	uint8_t footer[] = {0x00, 0x00, 0x00, 0x0f, 0x05, 0x01,
			(uint8_t)((data_size + (uint16_t)8) >> 8), 
			(uint8_t)((data_size + (uint16_t)8) & (uint8_t)0xFF)};

	uint16_t address = (uint16_t)VL53L8CX_UI_CMD_END -
		(data_size + (uint16_t)12) + (uint16_t)1;

	/* Check if cmd buffer is large enough */
	if((data_size + (uint16_t)12) 
           > (uint16_t)VL53L8CX_TEMPORARY_BUFFER_SIZE)
	{
		status |= VL53L8CX_STATUS_ERROR;
	}
	else
	{
		headers[0] = (uint8_t)(index >> 8);
		headers[1] = (uint8_t)(index & (uint32_t)0xff);
		headers[2] = (uint8_t)(((data_size & (uint16_t)0xff0) >> 4));
		headers[3] = (uint8_t)((data_size & (uint16_t)0xf) << 4);

	/* Copy data from structure to FW format (+4 bytes to add header) */
		SwapBuffer(data, data_size);
		for(i = (int16_t)data_size - (int16_t)1 ; i >= 0; i--)
		{
			p_dev->temp_buffer[i + 4] = data[i];
		}

	/* Add headers and footer */
		(void)memcpy(&p_dev->temp_buffer[0], headers, sizeof(headers));
		(void)memcpy(&p_dev->temp_buffer[data_size + (uint16_t)4],
			footer, sizeof(footer));

	/* Send data to FW */
		status |= WrMulti(&(p_dev->platform),address,
			p_dev->temp_buffer,
			(uint32_t)((uint32_t)data_size + (uint32_t)12));
		status |= _vl53l8cx_poll_for_answer(p_dev, 4, 1,
			VL53L8CX_UI_CMD_STATUS, 0xff, 0x03);

		SwapBuffer(data, data_size);
	}

	return status;
}

uint8_t vl53l8cx_dci_replace_data(
		VL53L8CX_Configuration		*p_dev,
		uint8_t				*data,
		uint32_t			index,
		uint16_t			data_size,
		uint8_t				*new_data,
		uint16_t			new_data_size,
		uint16_t			new_data_pos)
{
	uint8_t status = VL53L8CX_STATUS_OK;

	status |= vl53l8cx_dci_read_data(p_dev, data, index, data_size);
	(void)memcpy(&(data[new_data_pos]), new_data, new_data_size);
	status |= vl53l8cx_dci_write_data(p_dev, data, index, data_size);

	return status;
}
