/**
 *
 * 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 "vl53l5cx_platform.h"

uint8_t VL53L5CX_RdByte(
		VL53L5CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_value)
{
	uint8_t reg_addr[2];
	uint8_t status = 255;

	reg_addr[0] = (RegisterAdress >> 8) & 0xFF;
	reg_addr[1] = (RegisterAdress) & 0xFF;
	if (HAL_OK != HAL_I2C_Master_Transmit(VL53L5CX_pI2cHandle, p_platform->address, reg_addr, 2, 100)) return status;
	if (HAL_OK != HAL_I2C_Master_Receive(VL53L5CX_pI2cHandle, p_platform->address| 0x01, p_value, 1, 10)) return status;
	status = 0;

	return status;
}

uint8_t VL53L5CX_WrByte(
		VL53L5CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t value)
{
	uint8_t status = 255;
	uint8_t buf[3];
	buf[0] = (RegisterAdress >> 8) & 0xFF;
	buf[1] = RegisterAdress & 0xFF;
	buf[2] = value;

	if (HAL_OK != HAL_I2C_Master_Transmit(VL53L5CX_pI2cHandle, p_platform->address, buf, 3, 10)) return status;
	status = 0;
	return status;
}

uint8_t VL53L5CX_WrMulti(
		VL53L5CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_values,
		uint32_t size)
{
	uint8_t status = 255;
	uint8_t *buf;
	buf = malloc(size + 2);
	buf[0] = (RegisterAdress >> 8) & 0xFF;
	buf[1] = RegisterAdress & 0xFF;
	memcpy(buf +2, p_values, size);

	if (HAL_OK != HAL_I2C_Master_Transmit(VL53L5CX_pI2cHandle, p_platform->address, buf, size, 10)) return status;
	status = 0;
	return status;
}

uint8_t VL53L5CX_RdMulti(
		VL53L5CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_values,
		uint32_t size)
{
	uint8_t reg_addr[2];
	uint8_t status = 255;

	reg_addr[0] = (RegisterAdress >> 8) & 0xFF;
	reg_addr[1] = (RegisterAdress) & 0xFF;

	if (HAL_OK != HAL_I2C_Master_Transmit(VL53L5CX_pI2cHandle, p_platform->address, reg_addr, 2, 10)) return status;
	if (HAL_OK != HAL_I2C_Master_Receive(VL53L5CX_pI2cHandle, p_platform->address | 0x01, p_values, size, 10)) return status;
	status = 0;
	return status;
}

uint8_t VL53L5CX_Reset_Sensor(
		VL53L5CX_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 */
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_RESET); // LPn
	/* Set pin AVDD to LOW */
	/* Set pin VDDIO  to LOW */
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_RESET); // VDD
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); //I2C_SPI pin HIGH
	VL53L5CX_WaitMs(p_platform, 100);

	/* Set pin LPN of to HIGH */
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_5, GPIO_PIN_SET); // LPn
	/* Set pin AVDD of to HIGH */
	/* Set pin VDDIO of  to HIGH */
	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_15, GPIO_PIN_SET); // VDD
	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET); //I2C_SPI pin LOW
	VL53L5CX_WaitMs(p_platform, 100);

	return status;
}

void VL53L5CX_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 VL53L5CX_WaitMs(
		VL53L5CX_Platform *p_platform,
		uint32_t TimeMs)
{
	uint8_t status = 255;
	HAL_Delay(TimeMs);
	status = 0;
	return status;
}