/**
  *
  * 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 "platform.h"
#include "pico/stdlib.h"
#include "hardware/i2c.h"
#include "hardware/gpio.h"
#include <stdio.h>

#define I2C_SUCCESS 0
#define I2C_FAILED 1
#define I2C_BUFFER_EXCEEDED 2

#define I2C_DEVICE i2c1

#define MAX_I2C_BUFFER 0x8100


/// @brief Blocking function allowing to write a register on an I2C device
/// @param address_7_bits 
/// @param index : register to write
/// @param values : values to write
/// @param count : number of byte to send
/// @return 0: Success, -1 or -2: Failed 
int8_t i2c_write_register(char adresse_7_bits, uint16_t index, uint8_t * values, uint32_t count){
    int statu;
	uint8_t buffer[MAX_I2C_BUFFER];
    uint8_t index_to_unint8[2];
	absolute_time_t timeout_time;

	if(count > MAX_I2C_BUFFER - 2){
		return I2C_BUFFER_EXCEEDED;
	}

	index_to_unint8[0] =  (index >> 8) & 0xFF;
	index_to_unint8[1] =  index & 0xFF;

	buffer[0] = index_to_unint8[0];
	buffer[1] = index_to_unint8[1];
	
	for(uint32_t i=0; i<count; i++){
		buffer[2+i] = values[i];
	}

	// Define timeout - now + 1s.
	timeout_time = time_us_64() + 5000000;
    statu = i2c_write_blocking_until (I2C_DEVICE, adresse_7_bits, buffer, count + 2, 0, timeout_time);
	//statu = i2c_write_blocking (I2C_DEVICE, adresse_7_bits, buffer, count + 2, 0);
    if(statu == PICO_ERROR_GENERIC){
        printf("I2C - Write - Envoi registre Echec %x\n", adresse_7_bits);
        return I2C_FAILED;
    }else if(statu == PICO_ERROR_TIMEOUT){
		printf("Erreur ecrire registre: timeout\n");
		printf("Adresse : %x\n",adresse_7_bits << 1 );
		return I2C_FAILED;
	}

    return I2C_SUCCESS;
}


/// @brief Blocking function allowing to write a register on an I2C device
/// @param address_7_bits 
/// @param index : register to write
/// @param values : values to write
/// @param count : number of byte to send
/// @return 0: Success, -1 or -2: Failed 
int8_t i2c_read_register(char adresse_7_bits, uint16_t index, uint8_t *pdata, uint32_t count){
    int statu;
	uint8_t buffer[MAX_I2C_BUFFER];
    uint8_t index_to_unint8[2];

	index_to_unint8[0] =  (index >> 8) & 0xFF;
	index_to_unint8[1] =  index & 0xFF;

    statu = i2c_write_blocking (I2C_DEVICE, adresse_7_bits, index_to_unint8, 2, 0);
    if(statu == PICO_ERROR_GENERIC){
        printf("I2C - Write - Envoi registre Echec %x\n", adresse_7_bits);
        return I2C_FAILED;
    }

    statu = i2c_read_blocking (I2C_DEVICE, adresse_7_bits, pdata, count, 0);
    if(statu == PICO_ERROR_GENERIC){
        printf("I2C - Lecture registre Echec\n");
        return I2C_FAILED;
    }

    return I2C_SUCCESS;
}


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 */
	return i2c_read_register(p_platform->address >> 1, RegisterAdress, p_value, 1);
}

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

	/* Need to be implemented by customer. This function returns 0 if OK */
	return i2c_write_register(p_platform->address >> 1, RegisterAdress, &value, 1);

}

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 >> 1, 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 >> 1, 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);
	
	return 0;
}