/**
  *
  * Copyright (c) 2023 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.
  *
  ******************************************************************************
  */

/**
 * @file  vl53l4ed_calibration.c
 * @brief Calibration functions implementation
 */

#include <math.h>
#include "vl53l4ed_api.h"
#include "vl53l4ed_calibration.h"

VL53L4ED_Error VL53L4ED_CalibrateOffset(
		Dev_t dev,
		int16_t TargetDistInMm,
		int16_t *p_measured_offset_mm,
		int16_t nb_samples)
{
	VL53L4ED_Error status = VL53L4ED_ERROR_NONE;
	uint8_t i, tmp, continue_loop;
	uint16_t j, tmpOff;
	int16_t AvgDistance = 0;
	VL53L4ED_ResultsData_t results;

	if(((nb_samples < (int16_t)5) || (nb_samples > (int16_t)255))
			|| ((TargetDistInMm < (int16_t)10)
				|| (TargetDistInMm > (int16_t)1000)))
	{
		status |= (uint8_t)VL53L4ED_ERROR_INVALID_ARGUMENT;
	}
	else
	{
		status |= VL53L4ED_WrWord(dev, VL53L4ED_RANGE_OFFSET_MM, 0x0);
		status |= VL53L4ED_WrWord(dev, VL53L4ED_INNER_OFFSET_MM, 0x0);
		status |= VL53L4ED_WrWord(dev, VL53L4ED_OUTER_OFFSET_MM, 0x0);

		/* Device heat loop (10 samples) */
		status |= VL53L4ED_StartRanging(dev);
		for (i = 0; i < (uint8_t)10; i++) {
			tmp = (uint8_t)0;
			j = (uint16_t)0;
			continue_loop = (uint8_t)1;
			do{
				status |= VL53L4ED_CheckForDataReady(dev, &tmp);
				if(tmp == (uint8_t)1) /* Data ready */
				{
					continue_loop = (uint8_t)0;
				}
				else if(j < (uint16_t)5000) /* Wait for answer*/
				{
					j++;
				}
				else /* Timeout 5000ms reached */
				{
					continue_loop = (uint8_t)0;
					status |= (uint8_t)VL53L4ED_ERROR_TIMEOUT;
				}
				VL53L4ED_WaitMs(dev, 1);
			}while(continue_loop == (uint8_t)1);
			status |= VL53L4ED_GetResult(dev, &results);
			status |= VL53L4ED_ClearInterrupt(dev);
		}
		status |= VL53L4ED_StopRanging(dev);

		/* Device ranging */
		status |= VL53L4ED_StartRanging(dev);
		for (i = 0; i < (uint8_t)nb_samples; i++) {
			tmp = (uint8_t)0;
			j = (uint16_t)0;
			continue_loop = (uint8_t)1;
			do{
				status |= VL53L4ED_CheckForDataReady(dev, &tmp);
				if(tmp == (uint8_t)1) /* Data ready */
				{
					continue_loop = (uint8_t)0;
				}
				else if(j < (uint16_t)5000) /* Wait for answer*/
				{
					j++;
				}
				else /* Timeout 5000ms reached */
				{
 					continue_loop = (uint8_t)0;
					status |= (uint8_t)VL53L4ED_ERROR_TIMEOUT;
				}
				VL53L4ED_WaitMs(dev, 1);
			}while(continue_loop == (uint8_t)1);

			status |= VL53L4ED_GetResult(dev, &results);
			status |= VL53L4ED_ClearInterrupt(dev);
			AvgDistance += (int16_t)results.distance_mm;
		}

		status |= VL53L4ED_StopRanging(dev);
		AvgDistance = AvgDistance / nb_samples;
		*p_measured_offset_mm = (int16_t)TargetDistInMm - AvgDistance;
		tmpOff = (uint16_t) *p_measured_offset_mm * (uint16_t)4;
		status |= VL53L4ED_WrWord(dev, VL53L4ED_RANGE_OFFSET_MM, tmpOff);
	}

	return status;
}

VL53L4ED_Error VL53L4ED_CalibrateXtalk(
		Dev_t dev,
		int16_t TargetDistInMm,
		uint16_t *p_measured_xtalk_kcps,
		int16_t nb_samples)
{
	VL53L4ED_Error status = VL53L4ED_ERROR_NONE;
	uint8_t i, tmp, continue_loop;
	float_t AverageSignal = (float_t)0.0;
	float_t AvgDistance = (float_t)0.0;
	float_t AverageSpadNb = (float_t)0.0;
	float_t TargetDistance = (float_t)TargetDistInMm;
	float_t tmp_xtalk, CounterNbSamples = (float_t)0.0;
	VL53L4ED_ResultsData_t results;

	uint16_t calXtalk, j;

	*p_measured_xtalk_kcps = 0;
	if(((nb_samples < (int16_t)5) || (nb_samples > (int16_t)255))
			|| ((TargetDistInMm < (int16_t)10)
				|| (TargetDistInMm > (int16_t)5000)))
	{
		status |= (uint8_t)VL53L4ED_ERROR_INVALID_ARGUMENT;
	}
	else
	{
		/* Disable Xtalk compensation */
		status |= VL53L4ED_WrWord(dev,
			VL53L4ED_XTALK_PLANE_OFFSET_KCPS, *p_measured_xtalk_kcps);

		/* Device heat loop (10 samples) */
		status |= VL53L4ED_StartRanging(dev);
		for (i = 0; i < (uint8_t)10; i++) {
			tmp = (uint8_t)0;
			j = (uint16_t)0;
			continue_loop = (uint8_t)1;
			do{
				status |= VL53L4ED_CheckForDataReady(dev, &tmp);
				if(tmp == (uint8_t)1) /* Data ready */
				{
					continue_loop = (uint8_t)0;
				}
				else if(j < (uint16_t)5000) /* Wait for answer*/
				{
					j++;
				}
				else /* Timeout 5000ms reached */
				{
					continue_loop = (uint8_t)0;
					status |= (uint8_t)VL53L4ED_ERROR_TIMEOUT;
				}
				VL53L4ED_WaitMs(dev, 1);
			}while(continue_loop == (uint8_t)1);
			status |= VL53L4ED_GetResult(dev, &results);
			status |= VL53L4ED_ClearInterrupt(dev);
		}
		status |= VL53L4ED_StopRanging(dev);

		/* Device ranging loop */
		status |= VL53L4ED_StartRanging(dev);
		for (i = 0; i < (uint8_t)nb_samples; i++)
			{
			tmp = (uint8_t)0;
			j = (uint16_t)0;
			continue_loop = (uint8_t)1;
			do{
				status |= VL53L4ED_CheckForDataReady(dev, &tmp);
				if(tmp == (uint8_t)1) /* Data ready */
				{
					continue_loop = (uint8_t)0;
				}
				else if(j < (uint16_t)5000) /* Wait for answer*/
				{
					j++;
				}
				else /* Timeout 5000ms reached */
				{
 					continue_loop = (uint8_t)0;
					status |= (uint8_t)VL53L4ED_ERROR_TIMEOUT;
				}
				VL53L4ED_WaitMs(dev, 1);
			}while(continue_loop == (uint8_t)1);

			status |= VL53L4ED_GetResult(dev, &results);
			status |= VL53L4ED_ClearInterrupt(dev);

			/* Discard invalid measurements and first frame */
			if ((results.range_status == (uint8_t)0)
					&& (i > (uint8_t)0) )
			{
				AvgDistance += (float_t)results.distance_mm;
				AverageSpadNb += (float_t)results.number_of_spad;
				AverageSignal += (float_t)results.signal_rate_kcps;
				CounterNbSamples++;
			}
		}
		status |= VL53L4ED_StopRanging(dev);

		if (CounterNbSamples == (float_t)0.0)
		{
			status = VL53L4ED_ERROR_XTALK_FAILED;
		}
		else
		{
			AvgDistance /= CounterNbSamples;
			AverageSpadNb /= CounterNbSamples;
			AverageSignal /= CounterNbSamples;

			tmp_xtalk = (float_t)1.0 - (AvgDistance/TargetDistance);
			tmp_xtalk *= (AverageSignal/AverageSpadNb);

			/* 127kcps is the max Xtalk value (65536/512) */
			if (tmp_xtalk > (float_t)127.0)
			{
				status = VL53L4ED_ERROR_XTALK_FAILED;
			}
			else
			{
				*p_measured_xtalk_kcps = (uint16_t)(round(tmp_xtalk));

				/* Send data to firmware */
				calXtalk = (uint16_t)(tmp_xtalk * (float_t)512.0);
				status |= VL53L4ED_WrWord(dev,
					VL53L4ED_XTALK_PLANE_OFFSET_KCPS, calXtalk);
			}
		}
	}

	return status;
}