/**
  *
  * 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/spi.h"
#include "hardware/gpio.h"
#include <stdio.h>
#include "vl53l8cx_api.h"

#define BUFFER_SIZE 0x1000
#define ERR_OK 0

spi_inst_t * my_spi;

void SPI_beginTransaction(char my_ss_pin){
  /* switch(my_ss_pin){
    case D0: SPI.begin(SCK, MISO, MOSI, SS); break;
    case D1: SPI.begin(SCK, D3, MOSI, SS); break;
  }*/
  // 
  gpio_put(my_ss_pin, 0);
  if(my_ss_pin == 1){
    my_spi = spi0;
  }else{
    my_spi = spi1;
  }
  
}

void SPI_endTransaction(char my_ss_pin){
  //SPI.endTransaction();
  gpio_put(my_ss_pin, 1); 
  //SPI.end();
}

uint8_t RdByte(
		VL53L8CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t *p_value)
{
	uint8_t status = 255;
  uint8_t my_ss_pin;
  uint8_t tampon[2];
  my_ss_pin = p_platform->address;
	
	/* Need to be implemented by customer. This function returns 0 if OK */
  //Serial.printf("RdByte adresse :%x\n",p_platform->address);
  SPI_beginTransaction(my_ss_pin);

  tampon[0] = RegisterAdress >> 8 & 0x7F;
  tampon[1] = RegisterAdress & 0xFF;

  spi_write_blocking(my_spi, tampon, 2);
  spi_read_blocking(my_spi, 0x00, p_value, 1 );

  //printf("RdByte 0x%02X%02X 0x%02X\n", tampon[0], tampon[1], *p_value);

  SPI_endTransaction(my_ss_pin);
  
  return ERR_OK;
	
}

uint8_t WrByte(
		VL53L8CX_Platform *p_platform,
		uint16_t RegisterAdress,
		uint8_t value)
{
	uint8_t status = 255;
  uint8_t my_ss_pin;
  uint8_t tampon[3];
  
  my_ss_pin = p_platform->address;

	/* Need to be implemented by customer. This function returns 0 if OK */
  SPI_beginTransaction(my_ss_pin);

  tampon[0] = RegisterAdress >> 8 | 0x80;
  tampon[1] = RegisterAdress & 0xFF;
  tampon[2] = value;

  //printf("WrByte 0x%02X%02X 0x%02X\n", tampon[0], tampon[1], tampon[2]);

  spi_write_blocking(my_spi, tampon, 3);
   
  SPI_endTransaction(my_ss_pin);
  

  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];
  uint8_t tampon[2];
  uint8_t my_ss_pin;
  my_ss_pin = p_platform->address;

  SPI_beginTransaction(my_ss_pin);
  
  tampon[0] = RegisterAdress >> 8 | 0x80;
  tampon[1] = RegisterAdress & 0xFF;
  spi_write_blocking(my_spi, tampon, 2);

  while(size > BUFFER_SIZE){
    memcpy(my_buffer, (uint8_t*)&p_values[index], BUFFER_SIZE);
    spi_write_blocking(my_spi, my_buffer, BUFFER_SIZE);
    size = size - BUFFER_SIZE;
    index = index + BUFFER_SIZE;
  }
  if(size > 0){
	memcpy(my_buffer, (uint8_t*)&p_values[index], size);
	spi_write_blocking(my_spi, my_buffer, size);
  }
  
  SPI_endTransaction(my_ss_pin);

  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;
  uint8_t my_ss_pin;
  uint8_t tampon[2];
  my_ss_pin = p_platform->address;

  SPI_beginTransaction(my_ss_pin);
  
  tampon[0] = RegisterAdress >> 8 & 0x7F;
  tampon[1] = RegisterAdress & 0xFF;

  spi_write_blocking(my_spi, tampon, 2);
  spi_read_blocking(my_spi, 0, p_values, size);

  //printf("RdMulti 0x%02X%02X size : 0x%02X\n", tampon[0], tampon[1], size);

/*
  while(size > buffer_size){
    spi_read_blocking(my_spi, 0, &(p_values[index]), buffer_size);
    index += buffer_size;
    size -= buffer_size;
  }
  if(size > 0){
  	spi_read_blocking(my_spi, 0, &(p_values[index]), size);
  }
  */
  
  SPI_endTransaction(my_ss_pin);
  
  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;
}