/**************************************************************************//**
 *  @file si117xhrm.c
 *  @brief Main HRM source file for si117xhrm library
 *  @version 1.0.4
 ******************************************************************************
 * @section License
 * <b>(C) Copyright 2017 Silicon Labs, http://www.silabs.com</b>
 ******************************************************************************
 *
 * This file is licensed under the Silabs License Agreement. See the file
 * "Silabs_License_Agreement.txt" for details. Before using this software for
 * any purpose, you must agree to the terms of that agreement.
 *
 *****************************************************************************/

#include <sihrmUser.h>
#include "si117xhrm_static.h"

#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "em_device.h"

#include "../si117xdrv/si117x_functions.h"
#include "si117x_types.h"


/**************************************************************************//**
 * Static Function Prototypes
 *****************************************************************************/
static void si117xhrm_AdjustLedCurrent(Si117xhrmHandle_t *handle, int32_t channel, int32_t direction);
static void si117xhrm_PerformAgc(Si117xhrmHandle_t *handle);
static void si117xhrm_PerformDCSensing(Si117xhrmHandle_t *handle, uint16_t *ppg);
static int32_t si117xhrm_InitializeBuffers(Si117xhrmHandle_t *handle);
static int32_t si117xhrm_BPF_filtering(Si117xhrmHandle_t *handle, int32_t PS_input, int16_t *pPS_output, Si117xBPF_t *pBPF);
static int32_t si117xhrm_FrameProcess(Si117xhrmHandle_t *handle, int16_t *heart_rate, int32_t *HeartRateInvalidation, Si117xhrmData_t *hrm_data);
static int32_t si117xhrm_InitMeasurementParameters (Si117xhrmHandle_t *handle, int16_t measurement_rate);
static int32_t si117xhrm_GetSample(Si117xhrmHandle_t *handle, Si117xhrmIrqSample_t *samples);
static int32_t si117xhrm_SampleProcess(Si117xhrmHandle_t *handle, uint16_t HRM_timestamp, uint16_t hrmPs, uint16_t SpO2_PS_RED, uint16_t SpO2_PS_IR, Si117xhrmData_t *hrm_data);

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)	// Compile In/Out SpO2
  static int32_t si117xhrm_SpO2FrameProcess(Si117xhrmHandle_t *handle, int16_t *SpO2, int32_t *HeartRateInvalidation, Si117xhrmData_t *hrm_data);
#endif
static int32_t si117xhrm_IdentifyPart(Si117xhrmHandle_t *handle, int16_t *part_id);

/**************************************************************************//**
 * Global Variables and Constants
 *****************************************************************************/
static Si117xhrmDeviceConfiguration_t defaultDeviceConfiguration = {
  0x07,                      // task_enable;
  FS_25HZ,                   // measrate;
  MSCNT_25Hz,	             // ppg_meascount;
  0,                         // bioz_meascount;
  1,                         // ecg_meascount;
  3*2*15,                    // fifo_int_level; 15 samples per task

  (0*ACCEL_SYNCH + 0*PPG_SW_AVG + 1*ECG_SZ + 0*FIFO_TEST),  // meas_cntl;

  0,                         // bioz_dc_value;
  0,                         // bioz_freq_sel;

  {{// PPG1
  //  Drive Current = LED_CURRENT_LEVEL(Bit2:0) * LED_CURRENT_STEP(Bit5:3) + Min
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg1_led1_config;
  1 *LED_EN + 2 *LED_CURRENT_STEP + 6 *LED_CURRENT_LEVEL,  // ppg1_led2_config;
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg1_led3_config;
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg1_led4_config;
  0xf *HRM_MODE,  // ppg1_mode; All quadrants
  3*ADC_AVG,      // ppg1_measconfig;
  0 *DECIM + 3 *ADC_RANGE + 0 *CLK_DIV},  // ppg1_adcconfig;

  {// PPG2
  //  Drive Current = LED_CURRENT_LEVEL(Bit2:0) * LED_CURRENT_STEP(Bit5:3) + Min
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg2_led1_config;
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg2_led2_config;
  1 *LED_EN + 2 *LED_CURRENT_STEP + 4 *LED_CURRENT_LEVEL,  // ppg2_led3_config;
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg2_led4_config;
  0xf *HRM_MODE,  // ppg2_mode; All quadrants
  3 *ADC_AVG,     // ppg2_measconfig;
  1 *DECIM + 3 *ADC_RANGE + 0 *CLK_DIV},  // ppg2_adcconfig;

  {// PPG3
  //  Drive Current = LED_CURRENT_LEVEL(Bit2:0) * LED_CURRENT_STEP(Bit5:3) + Min
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg3_led1_config;
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg3_led2_config;
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg3_led3_config;
  1 *LED_EN + 2 *LED_CURRENT_STEP + 4 *LED_CURRENT_LEVEL,  // ppg3_led4_config;
  0xf *HRM_MODE,  // ppg3_mode; All quadrants
  3 *ADC_AVG,     // ppg3_measconfig;
  1 *DECIM + 3 *ADC_RANGE + 0 *CLK_DIV},  // ppg3_adcconfig;

  {// PPG4
  //  Drive Current = LED_CURRENT_LEVEL(Bit2:0) * LED_CURRENT_STEP(Bit5:3) + Min
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg4_led1_config;
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg4_led2_config;
  1 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg4_led3_config;
  0 *LED_EN + 3 *LED_CURRENT_STEP + 0 *LED_CURRENT_LEVEL,  // ppg4_led4_config;
  0xf *HRM_MODE,  // ppg4_mode; All quadrants
  3 *ADC_AVG,     // ppg4_measconfig;
  0 *DECIM + 3 *ADC_RANGE + 0 *CLK_DIV}},  // ppg4_adcconfig;

  0x25,   // ecg_measconfig;
  0x70,   // ecg_adcconfig;

  (0x01 * SYNCH_MODE),   // synch_config;
};

static const int16_t HRM_Interpolator_Coefs_R4[]={  // In Q15. R=4, L=4, Alpha=0.5
         //0x0000, 0x0000, 0x0000, 0x7FFF, 0x0000, 0x0000, 0x0000, 0x0000,
         0xFF56, 0x03FE, 0xF061, 0x6F86, 0x253F, 0xF4C6, 0x034D, 0xFF6B,
         0xFF22, 0x050D, 0xEDBE, 0x4E0F, 0x4E0F, 0xEDBE, 0x050D, 0xFF22,
         0xFF6B, 0x034D, 0xF4C6, 0x253F, 0x6F86, 0xF061, 0x03FE, 0xFF56,
};

#if (SI117XHRM_ENABLE_MEASUREMENT_RATE_25Hz == 1)
static int32_t BPF_25Hz[]=	{	//Q16
							0x00002000, 0x00000000, 0xFFFFE000, 0x0000FFFF, 0xFFFEF5F9, 0x00002F38,
							0x00008000, 0x000098B7, 0x00007FFF, 0x0000FFFF, 0xFFFF954A, 0x000075C0,
							0x0001FFFE, 0xFFFC0239, 0x0001FFFE, 0x0000FFFF, 0xFFFE2787, 0x0000DFCA
						};
// This was used in the finger-tip algorithm -
static const short BPF_output_scaler_25Hz=0x6F6F; //Q15
// static const sihrmFloat_t BPF_output_scaler_float_25Hz=0.870593092509911;
#endif

#if (SI117XHRM_ENABLE_MEASUREMENT_RATE_60Hz == 1)
static int32_t BPF_60Hz[]={ //Q16. Fs=79; [BPF_b,BPF_a] = cheby2(3,35, [18, 500]/60/Fs*2); 0dB at 45BMP. -1dB at 160BPM, -6dB at 200BPM. No overflow.
								0x00001000, 0x00000000, 0xFFFFF000, 0x0000FFFF, 0xFFFE43CF, 0x0000BFDC,
								0x00004000, 0xFFFFA265, 0x00004000, 0x0000FFFF, 0xFFFE4604, 0x0000CF60,
								0x0000FFFF, 0xFFFE001E, 0x0000FFFF, 0x0000FFFF, 0xFFFE0A1A, 0x0000F69F
							};
static const short BPF_output_scaler_60Hz=0x7ebc; //Q15
#endif

#if (SI117XHRM_ENABLE_MEASUREMENT_RATE_95Hz == 1)
static int32_t BPF_95Hz[]={	//Q16
								0x00001000, 0x00000000, 0xFFFFF000, 0x0000FFFF, 0xFFFE37E6, 0x0000CA9D,
								0x00004000, 0xFFFF97C4, 0x00004000, 0x0000FFFF, 0xFFFE36EF, 0x0000D793,
								0x0000FFFF, 0xFFFE0015, 0x0000FFFF, 0x0000FFFF, 0xFFFE0832, 0x0000F84B
							};
static const short BPF_output_scaler_95Hz=0x679c; //Q15
#endif

#if (SI117XHRM_ENABLE_MEASUREMENT_RATE_185Hz == 1)
static int32_t BPF_185Hz[]={	//Q16
								0x00000CCD, 0x00000000, 0xFFFFF333, 0x0000FFFF, 0xFFFE1D35, 0x0000E37D,
								0x00003333, 0xFFFF9ED3, 0x00003333, 0x0000FFFF, 0xFFFE1961, 0x0000EA8B,
								0x0000CCCC, 0xFFFE666C, 0x0000CCCC, 0x0000FFFF, 0xFFFE041C, 0x0000FC06
							 };
static const short BPF_output_scaler_185Hz=0x6aac; //Q15
#endif

#if (SI117XHRM_ENABLE_MEASUREMENT_RATE_229Hz == 1)
static int32_t BPF_229Hz[]={	 //Q16
								0x00000CCD, 0x00000000, 0xFFFFF333, 0x0000FFFF, 0xFFFE17B5, 0x0000E8C1,
								0x00003333, 0xFFFF9D08, 0x00003333, 0x0000FFFF, 0xFFFE140B, 0x0000EE89,
								0x0000CCCC, 0xFFFE666B, 0x0000CCCC, 0x0000FFFF, 0xFFFE034E, 0x0000FCC8
							};
static const short BPF_output_scaler_229Hz=0x5727; //Q15
#endif

#if (SI117XHRM_ENABLE_MEASUREMENT_RATE_430Hz == 1)
static int32_t BPF_430Hz[]={	//Q16
								0x00000AAB, 0x00000000, 0xFFFFF555, 0x0000FFFF, 0xFFFE0CC9, 0x0000F359,
								0x00002AAB, 0xFFFFAB7D, 0x00002AAA, 0x0000FFFF, 0xFFFE0A36, 0x0000F689,
								0x0000AAAA, 0xFFFEAAAD, 0x0000AAAA, 0x0000FFFF, 0xFFFE01C1, 0x0000FE47
							};
static const short BPF_output_scaler_430Hz=0x52ab; //Q15
#endif

static const int16_t NumOfLowestHRCyclesMin=300;		// 3.00*60/45BPM=4.0s, Min(Initial) #(x100) of lowest-HR cycle. The longer the frame scale is, more accurate the heart rate is.
static const int16_t NumOfLowestHRCyclesMax=500;		// 5.00*60/45BPM=6.7s, Max(Final) #(x100) of lowest-HR cycle. The longer the frame scale is, more accurate the heart rate is.
static const int16_t f_Low=45;							// (bpm), min heart rate.
static const int16_t f_High=200;						// (bpm), max heart rate.
static const uint16_t HRM_PS_Raw_Min_Thresh=6000;		// HRM PS threshold for validation (this depends on the LED active current: 16000 for current=0xF(359mA))

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)	// Compile In/Out SpO2
static const int16_t SpO2_Crest_Factor_Thresh=12;				// Crest factor (C^2) threshold
static const uint16_t SpO2_DC_to_ACpp_Ratio_Low_Thresh=20;
static const uint16_t SpO2_DC_to_ACpp_Ratio_High_Thresh=400;   // 1/60=1.67%, 1/100=1%. DC to ACpp ratio threshold for validation. Normally, <250 for finger. <700 for wrist.
  #if SPO2_REFLECTIVE_MODE
    static const uint16_t SpO2_DC_Min_Thresh=4000;	// Red or IR PS threshold for validation.
  #else
    static const uint16_t SpO2_DC_Min_Thresh=9000;	// Red or IR PS threshold for validation.
  #endif
#endif

/* DC sensing */
static uint8_t led_register_address[4] = {0x0c, 0x14, 0x1c, 0x20};  // ppg1_led2, ppg2_led3, ppg3_led4, unused

/*****************************************************************************
*  Error Checking Macros
******************************************************************************/
#ifndef checkErr
#define checkErr(fCall)      if (error = (fCall), (error = (error < 0) ? error : SI117xHRM_SUCCESS)) \
								 {goto Error;}  else
#endif

#ifndef checkParamRange
#define checkParamRange(param_value, min, max, param_number)  if((param_value < min) || (param_value > max)) \
															{ error = SI117xHRM_ERROR_PARAM1_OUT_OF_RANGE - param_number - 1; \
															  goto Error; \
															}
#endif

/**************************************************************************//*
 * @brief
 *	Start the device's autonomous measurement operation.
 *  The device must be configured before calling this function.
 *
 * @param[in] _handle
 *	si117xhrm handle
 *
 * @param[in] reset
 *	Reset the internal parameters of the HRM algorithm.  If set to false the
 *	HRM algorithm will begin running using parameter values from the last time
 *	it was run.  If the users heart rate has changed significantly since the
 *	last time the algorithm has run and reset is set to false, it could take
 *	longer to converge on the correct heart rate.  It is recommended to set
 *	reset to true if the HRM algorithm has been stopped for greater than 15s.
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_Run(Si117xhrmHandle_t *handle, int8_t reset)
{
  int32_t error = SI117xHRM_SUCCESS;
  int16_t part_id;


  if (handle->flag_samples == 0 &&  !si117xhrm_IdentifyPart(handle, &part_id))
  {
    error = SI117xHRM_ERROR_INVALID_PART_ID;
    goto Error;
  }

  if(reset & 1)
    si117xhrm_InitMeasurementParameters (handle, handle->measurement_rate);

  if (handle->flag_samples != 1)    // if taking fresh samples
  {
    if (handle->HRM_DC_Sensing_Count < DC_SENSING_AGC_START_CNT)  // DC sensing?
    {
      SI117XDRV_StartLegacyPPG(handle->deviceID, 1, 800); //25Hz
      handle->hrmActiveDC=SI117xHRM_TRUE;    // reinitialize for DC sensing
    }
    else
    {
      SI117XDRV_Start(handle->deviceID);
    }
}

Error:
  return error;
}

/**************************************************************************//**
 * @brief
 *	Pause the device's autonomous measurement operation.
 *  HRM must be running before calling this function.
 *
 * @param[in] _handle
 *	si117xhrm handle
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_Pause(Si117xhrmHandle_t *handle)
{
  int32_t error = SI117xHRM_SUCCESS;
  int16_t part_id = 0;

  if (!si117xhrm_IdentifyPart(handle, &part_id))
  {
    error = SI117xHRM_ERROR_INVALID_PART_ID;
    goto Error;
  }

  // Stop Autonomous Operation
  if(handle->HRM_DC_Sensing_Flag == HRM_DC_SENSING_SENSE_FINISH)
    SI117XDRV_Stop(handle->deviceID);
  else
	SI117XDRV_StopLegacyPPG(handle->deviceID);

Error:
  return error;
}

/**************************************************************************//**
 * @brief
 *	Get low level i2c handle
 *
 * @param[in] _handle
 *	si117xhrm handle
 *
 * @param[out] deviceHandle
 *	Low level i2c handle
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_GetLowLevelHandle(Si117xhrmHandle_t *handle, HANDLE *si114x_handle)
{
  int32_t error = SI117xHRM_SUCCESS;

  *si114x_handle = handle->si117x_handle;

  return error;
}

/**************************************************************************//**
 * @brief
 *	Configure si117xhrm debugging mode.
 *
 * @param[in] handle
 *	Pointer to si117xhrm handle
 *
 * @param[in] enable
 *	Enable or Disable debug
 *
 * @param[in] debug
 *	Pointer to debug status
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_SetupDebug(Si117xhrmHandle_t *handle, int32_t enable, void *debug)
{
  int32_t error = SI117xHRM_SUCCESS;
  error = sihrmUser_SetupDebug(handle->si117x_handle, enable, debug);

  if (enable & SI117xHRM_DEBUG_CHANNEL_ENABLE)
    handle->algorithmStatusControlFlags |= SIHRM_ALGORITHM_STATUS_CONTROL_FLAG_DEBUG_ENABLED;
  else
    handle->algorithmStatusControlFlags &= ~SIHRM_ALGORITHM_STATUS_CONTROL_FLAG_DEBUG_ENABLED;

  return error;
}

/**************************************************************************//**
 * @brief
 *	Output debug message
 *
 * @param[in] handle
 *	Pointer to si117xhrm handle
 *
 * @param[in] message
 *	Message data
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_OutputDebugMessage(Si117xhrmHandle_t *handle, int8_t * message)
{
  int32_t error = SI117xHRM_SUCCESS;

  if(handle->algorithmStatusControlFlags & SIHRM_ALGORITHM_STATUS_CONTROL_FLAG_DEBUG_ENABLED)
    error = sihrmUser_OutputDebugMessage(handle->si117x_handle, (const uint8_t *)message);
  else
    error = SI117xHRM_ERROR_DEBUG_DISABLED;

  return error;
}

/**************************************************************************//**
 * @brief
 *	Configure device and algorithm
 *
 * @param[in] _handle
 *	si117xhrm handle
 *
 * @param[in] configuration
 *	Pointer to a configuration structure of type Si117xhrmConfiguration_t
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_Configure(Si117xhrmHandle_t *handle, Si117xhrmUserConfiguration_t *configuration)
{
  int16_t retval=0;
  uint8_t i=0;
  int32_t error = SI117xHRM_SUCCESS;

  int16_t part_id=0;
  SI117XDRV_PPGCfg_t ppgCfg = SI117XDRV_DEFAULT_PPG_CFG;

  if (!si117xhrm_IdentifyPart(handle, &part_id))
  {
    error = SI117xHRM_ERROR_INVALID_PART_ID;
    goto Error;
  }

  if (handle == NULL)
  {
    error = SI117xHRM_ERROR_BAD_POINTER;
    goto Error;
  }

  /*
   * For normal usage the application should pass a NULL pointer to the
   * deviceConfiguration in which case the algorithm will use the default config.
   * The application can override the default config by passing a pointer to the
   * new device configuration.  This is not recommended except for engineering
   * purposes
   */
  if(configuration->deviceConfiguration == NULL)
  { configuration->deviceConfiguration = &defaultDeviceConfiguration;
  }

  // Force hrm to use ppg1(green led), spo2 to use ppg2(ir led) and ppg3(red led)
  handle->hrm_ps_select = 0;
  handle->spo2_ir_ps_select = 1;
  handle->spo2_red_ps_select = 2;

  // Not sure if need for Si117x Static HRM
  handle->configuration = configuration;
  handle->timestampPeriod100ns = configuration->timestampPeriod100ns;

  si117xhrm_SetupDebug(handle, configuration->debugEnable, configuration->debugData);

  /*  The FIFO Test Debug Mode can only be enabled/disabled through the configure function  */
  if (configuration->debugEnable & SI117xHRM_DEBUG_FIFO_TEST_MODE_ENABLE)
  { handle->algorithmStatusControlFlags |= SIHRM_ALGORITHM_STATUS_CONTROL_FLAG_FIFO_TEST_MODE;
    handle->configuration->deviceConfiguration->meas_cntl |= FIFO_TEST;   // Enable FIFO test mode
  }
  else
  { handle->algorithmStatusControlFlags &= ~SIHRM_ALGORITHM_STATUS_CONTROL_FLAG_FIFO_TEST_MODE;
    handle->configuration->deviceConfiguration->meas_cntl &= ~FIFO_TEST;  // Disable FIFO test mode
  }

  for(i=0; i<4;i++)
  {
  	ppgCfg.ppgCfg[i].ppg_led1_config = configuration->deviceConfiguration->ppgConfig[i].ppg_led1_config;
  	ppgCfg.ppgCfg[i].ppg_led2_config = configuration->deviceConfiguration->ppgConfig[i].ppg_led2_config;
  	ppgCfg.ppgCfg[i].ppg_led3_config = configuration->deviceConfiguration->ppgConfig[i].ppg_led3_config;
  	ppgCfg.ppgCfg[i].ppg_led4_config = configuration->deviceConfiguration->ppgConfig[i].ppg_led4_config;
  	ppgCfg.ppgCfg[i].ppg_mode = configuration->deviceConfiguration->ppgConfig[i].ppg_mode;
  	ppgCfg.ppgCfg[i].ppg_measconfig = configuration->deviceConfiguration->ppgConfig[i].ppg_measconfig;
  	ppgCfg.ppgCfg[i].ppg_adcconfig = configuration->deviceConfiguration->ppgConfig[i].ppg_adcconfig;
  }

  // Restore measrate to 25Hz with no over-sampling before re-initialization
  configuration->deviceConfiguration->measrate = FS_25HZ;

  retval += Si117xWriteToRegister(handle->si117x_handle, REG_IRQ_ENABLE, (PPG1_IRQ+FIFO_IRQ));   // Enable PPG interrupt
  SI117XDRV_InitPPG (handle->deviceID, &ppgCfg);
  SI117XDRV_SetPPGTaskEnable (handle->deviceID, configuration->taskEnable);

  handle->measurement_rate = configuration->deviceConfiguration->measrate;

  handle->timestampClockFreq = 8192;

  si117xhrm_InitMeasurementParameters(handle, handle->measurement_rate);  // 25Hz

  return retval;
Error:
  return error;
}

/**************************************************************************//**
 * @brief
 *	Process Si117x samples and compute HRM/SpO2 results
 *
 * @param[in] _handle
 *	si117xhrm handle
 *
 * @param[out] heart_rate
 *  Pointer to a location where this function will return the heart rate result
 *
 * @param[out] SpO2
 *  Pointer to a location where this function will return the SpO2 result
 *
 * @param[out] hrm_status
 *  Pointer to a integer where this function will report status flags
 *
 * @param[out] hrm_data
 *  Optional pointer to a si117xhrmData_t structure where this function will return
 *  auxiliary data useful for the application.  If the application is not
 *  interested in this data it may pass NULL to this parameter.
 *
 * @param[in] samples
 *  Si117x samples
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_ProcessExternalSample(Si117xhrmHandle_t *handle, int16_t *heart_rate, int16_t *SpO2, int32_t *hrm_status, Si117xhrmData_t *hrm_data, Si117xhrmIrqSample_t *samples)
{
  int32_t error = SI117xHRM_SUCCESS;
  uint16_t *ppg_ptr[4];  // array of pointers used to select the ppg value used for the measurements

  ppg_ptr[0] = &samples->ppg[0];
  ppg_ptr[1] = &samples->ppg[1];
  ppg_ptr[2] = &samples->ppg[2];
  ppg_ptr[3] = &samples->ppg[3];

  checkErr(si117xhrm_SampleProcess(handle, (uint16_t)samples->timestamp, *ppg_ptr[handle->hrm_ps_select], *ppg_ptr[handle->spo2_red_ps_select], *ppg_ptr[handle->spo2_ir_ps_select], hrm_data));

  checkErr(si117xhrm_FrameProcess(handle, heart_rate, hrm_status, hrm_data));

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
  if (handle->spo2 != NULL)  // Call SpO2 Frame process
  {  checkErr(si117xhrm_SpO2FrameProcess(handle, SpO2, hrm_status, hrm_data));
  }
#endif

Error:
  return error;
}

/**************************************************************************//**
 * @brief
 *	HRM process engine.  This function should be called at least once per sample
 *
 * @param[in] _handle
 *	si117xhrm handle
 *
 * @param[out] heartRate
 *	Pointer to a location where this function will return the heart rate result
 *
 * @param[out] SpO2
 *  Pointer to a location where this function will return the SpO2 result
 *
 * @param[in] numSamples
 *
 * @param[out] numSamplesProcessed
 *
 * @param[out] hrmStatus
 *	Pointer to a integer where this function will report status flags
 *
 * @param[out] hrmData
 *	Optional pointer to a si117xhrmData_t structure where this function will return
 *  auxiliary data useful for the application.  If the application is not
 *  interested in this data it may pass NULL to this parameter.
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_Process(Si117xhrmHandle_t *handle, int16_t *heart_rate, int16_t *SpO2, int16_t numSamples, int16_t *numSamplesProcessed, int32_t *hrm_status, Si117xhrmData_t *hrm_data)
{
  int32_t error = SI117xHRM_SUCCESS;
  Si117xhrmIrqSample_t samples;
  int32_t i, channel;

  for(i=0; i<numSamples; i++)
  {
	  checkErr(si117xhrm_GetSample(handle, &samples));
#if (UART_DEBUG == 1)
	  sihrmUser_OutputRawSampleDebugMessage(handle, &samples, NULL);
#endif
	  if(handle->HRM_DC_Sensing_Flag == HRM_DC_SENSING_START || handle->HRM_DC_Sensing_Flag == HRM_DC_SENSING_RESTART)
	  {
		*hrm_status |= SI117xHRM_STATUS_FINGER_OFF;
		si117xhrm_PerformDCSensing(handle, (uint16_t *)&samples.ppg);
	  }
	  else if(handle->HRM_DC_Sensing_Flag == HRM_DC_SENSING_CHANGE_PARAMETERS)
	  {
		SI117XDRV_StopLegacyPPG(handle->deviceID);
		sihrmUser_SampleQueue_Clear(handle->si117x_handle);
		sihrmUser_PostSkinSensingCallback(handle->si117x_handle);
		handle->HRM_DC_Sensing_Flag = HRM_DC_SENSING_SENSE_FINISH;
		SI117XDRV_Start(handle->deviceID);
	  }
	  else if(handle->HRM_DC_Sensing_Flag == HRM_DC_SENSING_SENSE_FINISH)
	  {
#if HRM_PS_AGC
		if ((handle->HeartRateInvalidation_Previous & SI117xHRM_STATUS_HRM_MASK)==SI117xHRM_STATUS_SUCCESS)
		{
		  for(channel = 0; channel < 4; channel++)
	      {
			handle->HRM_AGC[channel].raw_ppg_sum += samples.ppg[channel];   //sum the raw data for later averaging.
			handle->HRM_AGC[channel].raw_ppg_count++;
		  }
		}
		si117xhrm_PerformAgc(handle);
#endif
	    error = si117xhrm_ProcessExternalSample(handle, heart_rate, SpO2, hrm_status, hrm_data, &samples);
	  }
	  // Cannot check ambient light for si117x, but can use PPG4 wrist detection if needed
  }

Error:
  *numSamplesProcessed = i;
  return error;
}

/**************************************************************************//**
 * @brief
 *	Initialize the optical sensor device and the HRM algorithm
 *
 * @param[in] portName
 *	Platform specific data to specify the i2c port information.
 *
 * @param[in] options
 *	Initialization options flags.
 *
 * @param[in] data
 *  Pointer to data storage structure
 *
 * @param[in] handle
 *	Pointer to si117xhrm handle
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_Initialize(void *port_name, int32_t options, Si117xDataStorage_t *data, Si117xhrmHandle_t **handle)
{
  int32_t error = SI117xHRM_SUCCESS;
  int32_t device;
  int16_t part_id = 0;

  Si117xhrmHandle_t *_handle;

#if (SIHRM_USE_DYNAMIC_DATA_STRUCTURE == 0)
  _handle = (Si117xhrmHandle_t*)(data->hrm);
  _handle->spo2 = (Si117xSpO2Handle_t*)(data->spo2);
#else
  _handle = (Si117xhrmHandle_t *)malloc(sizeof(Si117xhrmHandle_t));
  _handle->spo2 = (Si117xSpO2Handle_t *)malloc(sizeof(Si117xSpO2Handle_t));
#endif

  device = ((options & 0xE)>>1) + 1;

  if ((options & 0x01) == 0)	// if taking new samples
  {
    sihrmUser_Initialize(port_name, device, &(_handle->si117x_handle), &(_handle->deviceID));

	if(si117xhrm_IdentifyPart(_handle, &part_id) == 0)
	{
	  error = SI117xHRM_ERROR_INVALID_PART_ID;
	  sihrmUser_Close(_handle->si117x_handle);
#if (SIHRM_USE_DYNAMIC_DATA_STRUCTURE != 0)
	  if(_handle != 0)
	  {
		free(_handle);
		_handle = 0;
	  }
#endif
	}
	else
	{
	  _handle->flag_samples = 0;
	}
  }
  else
  {
	_handle->flag_samples = 1;
  }

  (*handle) = (Si117xhrmHandle_t *)_handle;

  return error;
}

/**************************************************************************//**
 * @brief
 *	Close the Si117x device
 *
 * @param[in] handle
 *	Pointer to si117xhrm handle
 *
 * @return
 *	Returns error status
 *****************************************************************************/
int32_t si117xhrm_Close(Si117xhrmHandle_t *handle)
{
  int32_t error = SI117xHRM_SUCCESS;

  if (handle != 0)
  {
    if (handle->flag_samples == 0)	// If we are passing samples then do not attempt to write to registers
    {
      sihrmUser_Close(handle->si117x_handle);
    }
#if (SIHRM_USE_DYNAMIC_DATA_STRUCTURE != 0)
    free(handle);
#endif
  }

  return error;
}

/**************************************************************************//**
 * @brief
 *	Returns algorithm version
 *
 * @param[out] revision
 *	String representing the si117xhrm library version.  The version string has
 *  a maximum size of 32 bytes.
 *
 * @return
 *	Returns error status.
 *****************************************************************************/
int32_t si117xhrm_QuerySoftwareRevision(int8_t *revision)
{
  int32_t error = SI117xHRM_SUCCESS;

  strcpy((char *)revision, SI117xHRM_VERSION);

  return error;
}

/**************************************************************************//**
 * @brief Adjust the LED current setting based on AGC result
 *****************************************************************************/
static void si117xhrm_AdjustLedCurrent(Si117xhrmHandle_t *handle, int32_t channel, int32_t direction)
{
  if(direction == 1)
  {
    if(handle->DC_Sensing_LED[channel] != 63)
      handle->DC_Sensing_LED[channel] = handle->DC_Sensing_LED[channel] + 1;
  }
  else
  {
    if(handle->DC_Sensing_LED[channel] != 0)
	  handle->DC_Sensing_LED[channel] = handle->DC_Sensing_LED[channel] - 1;
  }
  handle->HRM_AGC[channel].led_current_value = handle->DC_Sensing_LED[channel];
  Si117xParamSet(handle->si117x_handle, led_register_address[channel], handle->DC_Sensing_LED[channel]+LED_EN);
}

/**************************************************************************//**
 * @brief Perform the AGC (automatic gain control)
 *****************************************************************************/
static void si117xhrm_PerformAgc(Si117xhrmHandle_t *handle)
{
  uint32_t average_ppg;
  uint16_t agc_threshold_percent;
  int32_t direction = 0;
  int16_t channel;

  for(channel = 0; channel < 4; channel++)
  {
    if((handle->HRM_AGC[channel].raw_ppg_count >= HRM_NUM_SAMPLES_IN_FRAME) && (handle->configuration->taskEnable & (0x1 << channel)))
    {
      average_ppg = handle->HRM_AGC[channel].raw_ppg_sum/handle->HRM_AGC[channel].raw_ppg_count;  // Average the raw PS value for the frame
      handle->HRM_AGC[channel].raw_ppg_sum=0;
      handle->HRM_AGC[channel].raw_ppg_count=0;

      // Ideally the relative threshold should be set according to the integration time and sensitivity
      // Set agc_threshold_percent (%). When PPG is out of the range, AGC increase/decrease the LED current by 1.
      agc_threshold_percent = 20;

      if ((average_ppg<(int32_t)HRM_DC_WORKING_LEVEL*(100-agc_threshold_percent)/100) || (average_ppg>(int32_t)HRM_DC_WORKING_LEVEL*(100+agc_threshold_percent)/100))
      { // Increase or decrease the LED current by 1
        if(average_ppg<(int32_t)HRM_DC_WORKING_LEVEL*(100-agc_threshold_percent)/100)
          direction = 1;
        else
          direction = 0;

        si117xhrm_AdjustLedCurrent(handle, channel, direction);

        handle->HRM_AGC[channel].agc_flag = 1;  // Signal that the AGC made a change to the current or gain.  This flag is cleared in sample process.
      }
    }
  }
}

/**************************************************************************//**
 * @brief Perform the DC Sensing
 *****************************************************************************/
static void si117xhrm_PerformDCSensing(Si117xhrmHandle_t *handle, uint16_t *ppg)
{
  int16_t j;
  int16_t channel;
  uint8_t range_changed = 0;
  uint8_t clkdiv_changed = 0;

  for(channel = 0; channel < 4; channel++)
  {
	if ((((handle->ppg_led_local>>1)<<1)==handle->ppg_led_local)
			&& ((handle->ppg_led_local>=1) && (handle->ppg_led_local<63)) && (handle->configuration->taskEnable & (0x1 << channel)))
    {
      Si117xParamSet(handle->si117x_handle, led_register_address[channel], handle->ppg_led_local+LED_EN);
      if(ppg[channel] < HRM_PS_RAW_SKIN_CONTACT_THRESHOLD)
      {
    	handle->HRM_DC_Sensing_Flag = HRM_DC_SENSING_SEEK_NO_CONTACT;
        handle->ppg_led_local=0;   // Restart DC sensing for another try
        break;
      }
      else
        handle->DC_Sensing_Level[channel][(handle->ppg_led_local>>1)-1]=ppg[channel];
    }
  }

  if (handle->ppg_led_local==63)  // If we have tried all the LED currents
  { // DC sensing decision
    for (channel=0; channel<4; channel++)
    {
      handle->DC_Sensing_LED[channel]=0;
      for (j=1; j<32; j++)
      {
    	if (handle->DC_Sensing_Level[channel][j] > HRM_DC_WORKING_LEVEL)
        {
    	  handle->DC_Sensing_LED[channel]=j;  // Found the working level.
          break;
        }
      }
      if ((handle->DC_Sensing_LED[channel]==0) && (handle->configuration->taskEnable & (0x1 << channel)))
      {
    	handle->DC_Sensing_LED[channel]=31;  // Set it to the max level

        // Set to change CLK_DIV
        if (handle->CLK_DIV_value[channel]!=2)  // Do nothing if clkdiv is already equal to 2
        {
          handle->CLK_DIV_value[channel]=2;
          clkdiv_changed = 1;
        }
      }

      handle->DC_Sensing_LED[channel] = handle->DC_Sensing_LED[channel]*2;  // As DC_Sensing_Level[i][1] has LED current=1*2 and scan LED current at every other current.

      // If LED current is < 10mA then increase the range.
      if(handle->DC_Sensing_LED[channel] < 20)
      {
    	if((handle->RANGE_value[channel] < 3) && (handle->configuration->taskEnable & (0x1 << channel))) // If not max range
        {
    	  handle->RANGE_value[channel]++;
          range_changed = 1;
        }
      }
    }
    handle->HRM_DC_Sensing_Flag = HRM_DC_SENSING_CHANGE_PARAMETERS;

    // If needed, change CLK_DIV or range for specific channel and restore LED currents to the configuration's.
    if ((handle->DC_sensing_changes_CLK_DIV && (clkdiv_changed == 1)) || (range_changed == 1))
    {
      for (channel=0; channel<4; channel++)
      {
  	    if (handle->configuration->taskEnable & (0x1 << channel))
  		  Si117xParamSet(handle->si117x_handle, PARAM_PPG1_ADCCONFIG+channel*PPG_OFFSET, (handle->configuration->deviceConfiguration->ppgConfig[channel].ppg_adcconfig & 0xC0)| handle->CLK_DIV_value[channel] | ((handle->RANGE_value[channel] << 4) & 0x30)); //Change CLK_DIV
      }
      handle->ppg_led_local=0;   // Restart DC sensing for another try
      handle->HRM_DC_Sensing_Flag = HRM_DC_SENSING_RESTART;
      return;
    }


    // DC Sensing successful.  Set the working LED currents based on DC-sensing results.
    for(channel = 0; channel < 4; channel++)
    {
      if (handle->configuration->taskEnable & (0x1 << channel))
        Si117xParamSet(handle->si117x_handle, led_register_address[channel], handle->DC_Sensing_LED[channel]+LED_EN);
    }
  }

  if(handle->HRM_DC_Sensing_Flag == HRM_DC_SENSING_SEEK_NO_CONTACT)
  {
	handle->ppg_led_local=0;   // Restart DC sensing for another try
	handle->HRM_DC_Sensing_Flag = HRM_DC_SENSING_RESTART;
  }
  else
    handle->ppg_led_local = handle->ppg_led_local >= 63 ? 64 :  handle->ppg_led_local+1;
}

/**************************************************************************//**
 * @brief Initialize HRM/SpO2 buffers and other variables
 *****************************************************************************/
static int32_t si117xhrm_InitializeBuffers(Si117xhrmHandle_t *handle)
{
  int32_t i, j;
  int32_t error = SI117xHRM_SUCCESS;

  // Buffer initialization
  for(i=0; i<MAX_FRAME_SAMPLES; i++)
  {
	handle->HRM_sample_buffer[i]=0;

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)	// Compile In/Out SpO2
	if (handle->spo2 != NULL)
	{
	  handle->spo2->SpO2_RED_AC_sample_buffer[i]=0;
	  handle->spo2->SpO2_RED_DC_sample_buffer[i]=0;
	  handle->spo2->SpO2_IR_AC_sample_buffer[i]=0;
	  handle->spo2->SpO2_IR_DC_sample_buffer[i]=0;
	}
#endif
  }

  for(i=0; i<handle->BPF_biquads; i++)	// zero-out the input and output buffer
  {
	for(j=0; j<3; j++)
	{
	  handle->HRM_BPF.x[i][j]=0;
	  handle->HRM_BPF.yi[i][j]=0;
	  handle->HRM_BPF.yf[i][j]=0;
#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)	// Compile In/Out SpO2
	  if (handle->spo2 != NULL)  //SpO2 BPF buffer initialization
	  {
		handle->spo2->SpO2_Red_BPF.x[i][j]=0;
		handle->spo2->SpO2_Red_BPF.yi[i][j]=0;
		handle->spo2->SpO2_Red_BPF.yf[i][j]=0;

		handle->spo2->SpO2_IR_BPF.x[i][j]=0;
		handle->spo2->SpO2_IR_BPF.yi[i][j]=0;
		handle->spo2->SpO2_IR_BPF.yf[i][j]=0;
	  }
#endif
	}
  }

  for (i=0; i<INTERPOLATOR_L*2; i++) handle->HRM_interpolator_buf[i]=0;  // Initialized the whole HRM interpolator buffer to 0.

  handle->sample_count=0;
  handle->HRM_PS_raw_level_count=0;
  handle->HRM_Buffer_In_Index=0;

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
  if (handle->spo2 != NULL)  // Variable Initialization
  {
	for (i=0; i<INTERPOLATOR_L*2; i++)
	{
	  handle->spo2->SpO2_Red_interpolator_buf[i]=0;  // Initialized the whole SpO2 Red interpolator buffer to 0.
	  handle->spo2->SpO2_IR_interpolator_buf[i]=0;   // Initialized the whole SpO2 Red interpolator buffer to 0.
	}
	handle->spo2->SpO2_Buffer_In_index=0;
	handle->spo2->SpO2_raw_level_count=0;
	handle->spo2->SpO2_Red_BPF_Ripple_Count=0;
	handle->spo2->SpO2_Red_PS_Input_old=0;
	handle->spo2->SpO2_IR_BPF_Ripple_Count=0;
	handle->spo2->SpO2_IR_PS_Input_old=0;
	handle->spo2->TimeSinceFingerOff=0;
  }
#endif

  handle->HRM_DC_Sensing_Count=0;
  handle->HRM_Raw_PS_old=0;
  handle->HRM_DC_Sensing_Flag=HRM_DC_SENSING_START;
  handle->BPF_Active_Flag=(0xff-HRM_BPF_ACTIVE-SpO2_RED_BPF_ACTIVE-SpO2_IR_BPF_ACTIVE);	 // BPF-ripple-reduction flags. Clear these bits.
  handle->HRM_BPF_Ripple_Count=0;
  handle->HRM_PS_Input_old=0;
  handle->HeartRateInvalidation_Previous=SI117xHRM_STATUS_FINGER_OFF;
  handle->HRM_PS_DC=0;

  // DC sensing initialization
  handle->ppg_led_local = 0;
  handle->DC_sensing_changes_CLK_DIV = 1;
  for(i=0; i<4; i++)
  {
    handle->CLK_DIV_value[i] = 0;
    handle->RANGE_value[i] = (handle->configuration->deviceConfiguration->ppgConfig[i].ppg_adcconfig & 0x30) >> 4;
  }

  // AGC initialization
  for(i=0; i<4; i++)
  {
    handle->HRM_AGC[i].raw_ppg_sum = 0;
    handle->HRM_AGC[i].raw_ppg_count = 0;
    handle->HRM_AGC[i].led_current_value = 0;
    handle->HRM_AGC[i].agc_flag = 0;
    handle->HRM_AGC[i].saved_ppg = 0;
  }

  if (handle->flag_samples == 0)
    sihrmUser_SampleQueue_Clear(handle->si117x_handle);

  return error;
}

/**************************************************************************//**
 * @brief Perform band-pass filtering on PPG samples
 *****************************************************************************/
static int32_t si117xhrm_BPF_filtering(Si117xhrmHandle_t *handle, int32_t PS_input, int16_t *pPS_output, Si117xBPF_t *pBPF)
{
  // Band-pass filter: 32-bit integer-point precision.
  const int16_t BPF_Q15=(1<<15)-1;
  int32_t error = SI117xHRM_SUCCESS;
  int16_t i,j;
  int32_t BPF_new_sample;
  int32_t *pBPF_a, *pBPF_b;	 // Pointers for the BPF calculations
  int32_t New_Sample_i1, New_Sample_i2;  // May use __int64 for debug
  int32_t New_Sample_i=0, New_Sample_f=0;

  // BPF-Ripple Reduction: After finger-On is detected, check the PS DC after 1s and clear BPF data buffers with pBPF->x[0][:]=current PS.
  uint8_t BPF_Current_Flag;
  uint16_t PS_Normlized_Thresh, PS_Normalization_Scaler, *pBPF_Ripple_Count, *pPS_Input_old;

  if (pBPF == &handle->HRM_BPF)
  {
    PS_Normlized_Thresh=HRM_PS_Raw_Min_Thresh;
    BPF_Current_Flag = HRM_BPF_ACTIVE;
    pBPF_Ripple_Count = &handle->HRM_BPF_Ripple_Count;
    pPS_Input_old = &handle->HRM_PS_Input_old;
    PS_Normalization_Scaler=handle->HRM_Normalization_Scaler;
  }
#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
  // SpO2 BPF-ripple reduction
  else if ((handle->spo2 != NULL) && (pBPF == &handle->spo2->SpO2_Red_BPF))
  {
    if (handle->spo2 != NULL)
    {
      PS_Normlized_Thresh=SpO2_DC_Min_Thresh;
      BPF_Current_Flag = SpO2_RED_BPF_ACTIVE;
      pBPF_Ripple_Count = &handle->spo2->SpO2_Red_BPF_Ripple_Count;
      pPS_Input_old = &handle->spo2->SpO2_Red_PS_Input_old;
      PS_Normalization_Scaler=handle->RED_Normalization_Scaler;
    }
  }
  else if ((handle->spo2 != NULL) && (pBPF == &handle->spo2->SpO2_IR_BPF))
  {
    if (handle->spo2 != NULL)
    {
      PS_Normlized_Thresh=SpO2_DC_Min_Thresh;
      BPF_Current_Flag = SpO2_IR_BPF_ACTIVE;
      pBPF_Ripple_Count = &handle->spo2->SpO2_IR_BPF_Ripple_Count;
      pPS_Input_old = &handle->spo2->SpO2_IR_PS_Input_old;
      PS_Normalization_Scaler=handle->IR_Normalization_Scaler;
    }
  }
#endif
  else
  {
    error = SI117xHRM_ERROR_BAD_POINTER;
    goto Error;
  }

  if (PS_input<PS_Normlized_Thresh)  // Look for the finger Off-to-On transition and then initialize the BPF variables
  {
    handle->BPF_Active_Flag &=(0xff-BPF_Current_Flag);  // Skip HRM BPF filter
    *pBPF_Ripple_Count=0;
  }
  else
  {
    // When Finger On is detected, check diff(PS) and Ripple Count before initialize the BPF buffer.
    if ((handle->BPF_Active_Flag & BPF_Current_Flag)==0)
    {
      (*pBPF_Ripple_Count)++;
      i = PS_input>*pPS_Input_old ? PS_input-*pPS_Input_old : *pPS_Input_old-PS_input;  // i=abs(PS-PS_old)
      if ((i<((10*PS_Normalization_Scaler)>>QF_SCALER)) && (*pBPF_Ripple_Count>(10*handle->Fs/100)))  // delay 1s
      {
        handle->BPF_Active_Flag |= BPF_Current_Flag;
        for(i=0; i<handle->BPF_biquads; i++)  // zero-out the input and output buffer
        {
          for(j=0; j<3; j++)
          {
            pBPF->x[i][j] =0;
            pBPF->yi[i][j]=0;
            pBPF->yf[i][j]=0;
          }
        }
        // Use the current level to initialize these BPF variables to significantly reduce the BPF ripple.
        for(j=0; j<3; j++)
          pBPF->x[0][j]=PS_input;  // Initialized to the HRM PS DC.
      }
    }
  }

  if ((handle->BPF_Active_Flag & BPF_Current_Flag)==0)
  {
    *pPS_Input_old=PS_input;
    *pPS_output=0;
    return error;
  }

  pBPF_b=handle->pBPF_b_0;			// Initialize to the start of b coefs.
  pBPF_a=handle->pBPF_b_0+3;		// Initialize to the start of a coefs.
  BPF_new_sample=PS_input;          // Normalized input

  for(i=0; i<handle->BPF_biquads; i++)
  { // filter biquad process
    for(j=3-1; j>0; j--)  // Shift the BPF in/out data buffers and add the new input sample
    {
      pBPF->x[i][j]  = pBPF->x[i][j-1];
      pBPF->yi[i][j] = pBPF->yi[i][j-1];
      pBPF->yf[i][j] = pBPF->yf[i][j-1];
    }
    pBPF->x[i][0]=(int32_t)BPF_new_sample;	// Add new sample for the current biquad
    New_Sample_i1=0;					// BPF_new_sample is used for the next biquad
    New_Sample_i2=0;
    for(j=0; j<3; j++)
    {
      if (abs(pBPF->x[i][j])<(1<<14))  // If |x| is large, scale down first before the 32-bit-int32_t multiplication.
        New_Sample_i1 += ((((pBPF_b[j] * pBPF->x[i][j]))>>(SCALE_I_SHIFT-1))+1)>>1;  // +1 is round-up
      else
        New_Sample_i1 += ((((pBPF_b[j] * (pBPF->x[i][j]>>2)))>>(SCALE_I_SHIFT-2-1))+1)>>1;  // +1 is round-up
    }
    for(j=1; j<3; j++)  // Shift the BPF in/out data buffers and add the new input sample
    {
      if (abs(pBPF->yi[i][j])<(1<<14))  // If |y| is large, scale down first before the 32-bit-int32_t multiplication.
        New_Sample_i1 -= ((((pBPF_a[j] * (int32_t)pBPF->yi[i][j]))>>(SCALE_I_SHIFT-1))+1)>>1;  // +1 is round-up
      else
        New_Sample_i1 -= ((((pBPF_a[j] * (int32_t)(pBPF->yi[i][j]>>2)))>>(SCALE_I_SHIFT-2-1))+1)>>1;  // +1 is round-up
      New_Sample_i2 -= ((pBPF_a[j] * pBPF->yf[i][j]));
    }
    New_Sample_i  = ((New_Sample_i1>>(SOS_SCALE_SHIFT-SCALE_I_SHIFT-1))+1)>>1;  // +1 is round-up
    New_Sample_i += ((New_Sample_i2>>(SOS_SCALE_SHIFT+SCALE_F_SHIFT-1))+1)>>1;  // +1 is round-up

    New_Sample_f  = ((New_Sample_i1>>(SOS_SCALE_SHIFT-SCALE_I_SHIFT-SCALE_F_SHIFT-1))+1)>>1;  // +1 is round-up
    New_Sample_f += ((New_Sample_i2>>(SOS_SCALE_SHIFT-1))+1)>>1;  // +1 is round-up
    New_Sample_f -= New_Sample_i<<SCALE_F_SHIFT;

    BPF_new_sample= New_Sample_i;

    pBPF->yi[i][0]=New_Sample_i;
    pBPF->yf[i][0]=New_Sample_f;

    // Update new input sample for next 2nd-order stage. point pBPF_b/_a to the next 2nd-order stage
    pBPF_b +=6;
    pBPF_a +=6;
  }
  *pPS_output=(int16_t)(New_Sample_i*handle->BPF_output_scaler/BPF_Q15);  // 0.5=roundup

Error:
  return error;
}

/**************************************************************************//**
 * @brief Heart rate frame process
 *****************************************************************************/
static int32_t si117xhrm_FrameProcess(Si117xhrmHandle_t *handle, int16_t *heart_rate, int32_t *HeartRateInvalidation, Si117xhrmData_t *hrm_data)
{
  int32_t error = SI117xHRM_SUCCESS;
  int16_t i, j, k, m;
  int16_t ZC_count, ZC_HR;					    // Zero-crossing count and the heart rate estimated from ZC_count
  int16_t hrmPsVppLow=0, hrmPsVppHigh=0;	    // Variables for BPF(PS) min and max. Initializations are needed to avoid the usage before set.
  int16_t T_Low, T_High;				        // (sample), times for min and max heart rates based on Zero-crossing count.
  int32_t CF_abs_max=0, CF_abs_energy=1000;	    // Initializations are needed to avoid the usage before set.

  int32_t HRM_PS_AutoCorr_max, HRM_PS_AutoCorr_current, HRM_PS_AutoCorr_max_index;  // Auto correlation variables
  const int32_t HRM_PS_AutoCorrMax_Thresh=100000/2880/4;	// Auto correlation threshold for validation (may need for the wrist-band as the pulse is usually weaker than that on fingertip))
  int16_t HeartRateX10;
  int16_t HRM_Buffer_Out_Index;				// Output index to the circular buffer of the frame sample buffer.

  T_Low=handle->T_Low0;
  T_High=handle->T_High0;					// (sample), times for min and max heart rates based on Zero-crossing count.

  handle->HRM_PS_DC += handle->Normalized_HRM_PS*handle->HRM_Interpolator_Factor;	// HRM_PS_DC is the PS DC accumulator. PS is the normalized PS and also input to BPF.

  // Frame process (Background job)
  if (handle->sample_count>=handle->hrUpdateInterval)
  {
	// Received new hrUpdateInterval samples
    handle->sample_count -= handle->hrUpdateInterval;		// Adjust the count for the next block of samples
    *HeartRateInvalidation = SI117xHRM_STATUS_SUCCESS;		// Clears all HRM and SpO2 status bits.

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
    if (handle->spo2 != NULL)
    {
      handle->spo2->SpO2_Percent=SPO2_STATUS_PROCESS_SPO2_FRAME;	// Notify SpO2 to process the SpO2 frame.
    }
#endif

    // Re-initialize hrIframe to the min length when "Finger-off", so to speed up the next HR reporting.
    if ((handle->HeartRateInvalidation_Previous & (SI117xHRM_STATUS_FINGER_OFF|SI117xHRM_STATUS_FINGER_ON|SI117xHRM_STATUS_BPF_PS_VPP_OFF_RANGE|SI117xHRM_STATUS_AUTO_CORR_MAX_INVALID)))
    {
      handle->NumOfLowestHRCycles=NumOfLowestHRCyclesMin;
      handle->hrIframe=handle->T_Low0*handle->NumOfLowestHRCycles/100;  // (sample)
    }
    // After the first valid HR is detected, hrIframe is increased by hrUpdateInterval up to the max(final) length.
    if (((handle->HeartRateInvalidation_Previous & SI117xHRM_STATUS_HRM_MASK)==SI117xHRM_STATUS_SUCCESS) &&
    		(handle->NumOfLowestHRCycles < NumOfLowestHRCyclesMax) && (handle->hrIframe+handle->hrUpdateInterval)<MAX_FRAME_SAMPLES)  // Make sure hrIframe<MAX_FRAME_SAMPLES.
    {
      handle->hrIframe +=handle->hrUpdateInterval;  // (sample)
      handle->NumOfLowestHRCycles += handle->hrUpdateInterval*100/handle->T_Low0;  // cycles(x100)
    }

    HRM_Buffer_Out_Index=handle->HRM_Buffer_In_Index-handle->hrIframe;     // Set the output pointer based on the Input pointer.
    if (HRM_Buffer_Out_Index<0) HRM_Buffer_Out_Index +=MAX_FRAME_SAMPLES;  // Wrap around

    // Validation: Invalidate if any sample in the current frame is below the level threshold
    if (handle->HRM_PS_raw_level_count < handle->hrIframe)
    {
      if(handle->HRM_PS_raw_level_count<=handle->hrUpdateInterval)
        *HeartRateInvalidation |= SI117xHRM_STATUS_FINGER_OFF;
      else
        *HeartRateInvalidation |= SI117xHRM_STATUS_FINGER_ON;
    }
    else if ((handle->HeartRateInvalidation_Previous & SI117xHRM_STATUS_HRM_MASK) == SI117xHRM_STATUS_FINGER_ON)
    {
      // Validation: Still Finger-On mode if the last frame is Finger-On and the leading zeros in the current frame are 5% or more of iFrame samples.
      i=HRM_Buffer_Out_Index;
      for(k=0; k<handle->hrIframe*5/100; k++)
      {
        if (handle->HRM_sample_buffer[i]!=0) break;  // Break if non-zero
        if (++i==MAX_FRAME_SAMPLES) i=0;  // Wrap around
      }
      if (k==handle->hrIframe*5/100) *HeartRateInvalidation|=SI117xHRM_STATUS_FINGER_ON;
    }

    if (*HeartRateInvalidation==0)  // Skip below if the frame is invalid for heart-rate detection
    {
      // a) Calculate the max/min values and zero-crossing counts
      ZC_count=0;
      hrmPsVppHigh=-20000;
      hrmPsVppLow =+20000;
      i=HRM_Buffer_Out_Index;
      handle->HRM_PS_DC /= handle->hrUpdateInterval;	// Now, HRM_PS_DC is the averaged normalized PS over hrUpdateInterval sample (1s)
      for(k=0; k<handle->hrIframe; k++)  // Scaled HRM PS AC, max/min values
      {
        // Scaled HRM PS AC based on HRM PS DC with reference of HRM_PS_DC_REFERENCE
        m=(handle->HRM_sample_buffer[i]*HRM_PS_DC_REFERENCE+(uint16_t)handle->HRM_PS_DC/2)/(uint16_t)handle->HRM_PS_DC;  // Round-up
        handle->HRM_sample_buffer[i]=m;  // Scaled HRM_PS_AC
        if (hrmPsVppHigh<m) hrmPsVppHigh=m;
        if (hrmPsVppLow >m) hrmPsVppLow =m;
        if (++i==MAX_FRAME_SAMPLES) i=0;  // Wrap around
      }
      // Validation: Invalidate if the HRM_PS_Vpp is out of the range
      if (hrmPsVppLow<handle->HRM_PS_Vpp_min || hrmPsVppHigh>handle->HRM_PS_Vpp_max)
        *HeartRateInvalidation |= SI117xHRM_STATUS_BPF_PS_VPP_OFF_RANGE;

      i=HRM_Buffer_Out_Index;
      for(k=0; k<handle->hrIframe-1; k++)  // zero-crossing counts
      {	// Find zero-crossing counts for BPF(PS)-hrmPsVppHigh/ZR_BIAS_SCALE as BPF(PS) is asymmetric.
        m=i+1;
        if (m==MAX_FRAME_SAMPLES) m=0;  // Wrap around
        if (((int32_t)(handle->HRM_sample_buffer[i]-hrmPsVppHigh*(ZR_BIAS_SCALE))*2+1)*((int32_t)(handle->HRM_sample_buffer[m]-hrmPsVppHigh*(ZR_BIAS_SCALE))*2+1)<0)
          ZC_count++;
        if (++i==MAX_FRAME_SAMPLES)	i=0;  // Wrap around
      }

      // ZC_HR=ZC_count./2.*60./(iFrame/Fs); %(bmp), rough heart-rate estimate from the zero-cross count
      ZC_HR=ZC_count*60*(handle->Fs+5)/10/handle->hrIframe/2;

      // Validation: Invalidate if the zero-crossing HR is out of the HR range
      if (ZC_HR<f_Low || ZC_HR>f_High)
        *HeartRateInvalidation |= SI117xHRM_STATUS_ZERO_CROSSING_INVALID;
      else
      {
        // Modify T_High and T_Low based on rough heart-rate estimate that is from the zero-cross count.
        T_Low  = ZC_HR*(100-ZC_HR_TOLERANCE)< f_Low*100  ? handle->T_Low0  : 60*(handle->Fs+5)/10*100/(ZC_HR*(100-ZC_HR_TOLERANCE));
        T_High = ZC_HR*(100+ZC_HR_TOLERANCE)> f_High*100 ? handle->T_High0 : 60*(handle->Fs+5)/10*100/(ZC_HR*(100+ZC_HR_TOLERANCE));

        // b) Compute Crest factor of PS
        CF_abs_max= hrmPsVppHigh > abs(hrmPsVppLow) ? hrmPsVppHigh : abs(hrmPsVppLow);
        CF_abs_energy=0;
        i=HRM_Buffer_Out_Index;
        for(k=0; k<handle->hrIframe; k++)  // zero-crossing counts
        { // Find zero-crossing counts for BPF(PS)-hrmPsVppHigh/ZR_BIAS_SCALE as BPF(PS) is asymmetric.
          if (abs(handle->HRM_sample_buffer[i]) < handle->HRM_PS_Vpp_max )
          {
            CF_abs_energy += handle->HRM_sample_buffer[i]*handle->HRM_sample_buffer[i];
          }
          else
          {
            CF_abs_energy += handle->HRM_PS_Vpp_max*handle->HRM_PS_Vpp_max;
          }
          if (++i==MAX_FRAME_SAMPLES) i=0;  // Wrap around
        }
      }
      // Validation 1: Invalidate the frame if CF > CF_threshold
      if (CF_abs_max*CF_abs_max > handle->HRM_PS_CrestFactor_Thresh*(CF_abs_energy/handle->hrIframe))
        *HeartRateInvalidation |= SI117xHRM_STATUS_CREST_FACTOR_TOO_HIGH;

      if (*HeartRateInvalidation!=0)
      { //Scaled back HRM PS AC, so that HRM sample buffer is same as before
        i=HRM_Buffer_Out_Index;
        for(k=0; k<handle->hrIframe; k++)
        {
          handle->HRM_sample_buffer[i]=(handle->HRM_sample_buffer[i]*(uint16_t)handle->HRM_PS_DC+HRM_PS_DC_REFERENCE/2)/HRM_PS_DC_REFERENCE; //Round up
          if (++i==MAX_FRAME_SAMPLES) i=0;  // Wrap around
        }
      }
    }  // HeartRateInvalidation

    if (*HeartRateInvalidation==0) //Skip if the frame is invalid for heart-rate detection
    {
      // Compute the auto correlation on BPF(PS)
      HRM_PS_AutoCorr_max=-100000000;			// A large negative number
      HRM_PS_AutoCorr_max_index=T_High;
      for(i=T_High; i<=T_Low; i++)
      {
        HRM_PS_AutoCorr_current=0;
        j=HRM_Buffer_Out_Index;
        for(k=0; k<handle->hrIframe-i; k++)
        {
          m=j+i;
          if (m>=MAX_FRAME_SAMPLES) m -= MAX_FRAME_SAMPLES;  // Wrap around
          HRM_PS_AutoCorr_current += handle->HRM_sample_buffer[j]*handle->HRM_sample_buffer[m];
          if (++j==MAX_FRAME_SAMPLES) j=0;  // Wrap around
        }
        // Find the max
        if (HRM_PS_AutoCorr_current > HRM_PS_AutoCorr_max)
        {
          HRM_PS_AutoCorr_max = HRM_PS_AutoCorr_current;
          HRM_PS_AutoCorr_max_index = i;
        }
      }

      // Validation: Invalidate if the auto-correlation max is too small.
      if (HRM_PS_AutoCorr_max < HRM_PS_AutoCorrMax_Thresh*handle->hrIframe)
      {
        *HeartRateInvalidation |= SI117xHRM_STATUS_AUTO_CORR_TOO_LOW;
      }
      else if (HRM_PS_AutoCorr_max_index == T_Low || HRM_PS_AutoCorr_max_index == T_High)
      {
    	// Validation: Invalidate if the auto-correlation max occurs at the search bounday of [T_Low, T_High].
        *HeartRateInvalidation |=SI117xHRM_STATUS_AUTO_CORR_MAX_INVALID;
      }
      else
      {
        HeartRateX10=(600*(handle->Fs+5)/10*2/HRM_PS_AutoCorr_max_index+1)/2;  // +1 for Roundup
        *heart_rate = (HeartRateX10/5+1)/2;
      }

      // Scaled back HRM PS AC, so that HRM sample buffer is same as before
      i=HRM_Buffer_Out_Index;
      for(k=0; k<handle->hrIframe; k++)
      {
        handle->HRM_sample_buffer[i]=(handle->HRM_sample_buffer[i]*(uint16_t)handle->HRM_PS_DC+HRM_PS_DC_REFERENCE/2)/HRM_PS_DC_REFERENCE;  // Round up
        if (++i==MAX_FRAME_SAMPLES) i=0;  // Wrap around
      }
    }  //HeartRateInvalidation

    handle->HeartRateInvalidation_Previous=*HeartRateInvalidation & SI117xHRM_STATUS_HRM_MASK;  // Copy all HRM bits.
    *HeartRateInvalidation |=SI117xHRM_STATUS_FRAME_PROCESSED;
    if(hrm_data != 0)  // Copy these variables to hrm_data for display.
    {
      hrm_data->hrmPsVppLow  = (hrmPsVppLow*(uint16_t)handle->HRM_PS_DC+HRM_PS_DC_REFERENCE/2)/HRM_PS_DC_REFERENCE;   // Round up
      hrm_data->hrmPsVppHigh = (hrmPsVppHigh*(uint16_t)handle->HRM_PS_DC+HRM_PS_DC_REFERENCE/2)/HRM_PS_DC_REFERENCE;  // Round up
      if ((CF_abs_energy/handle->hrIframe)==0)
        hrm_data->hrmCrestFactor=-1;  // Invalid Crest factor
      else
        hrm_data->hrmCrestFactor=(CF_abs_max*CF_abs_max)/(CF_abs_energy/handle->hrIframe);
      if (hrm_data->hrmPs!=0)
        handle->hrmPerfusionIndex=10000*(hrm_data->hrmPsVppHigh - hrm_data->hrmPsVppLow)/hrm_data->hrmPs;
      else
        handle->hrmPerfusionIndex=0;
      hrm_data->hrmPerfusionIndex=handle->hrmPerfusionIndex;
      for(i=0; i<4; i++) hrm_data->DC_Sensing_LED[i]=handle->DC_Sensing_LED[i];
    }
    handle->HRM_PS_DC=0;	// Reset HRM PS DC accumulator
  }  // Frame process, sample_count==hrUpdateInterval

  return error;
}

/**************************************************************************//**
 * @brief SpO2 frame process
 *****************************************************************************/
#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
static int32_t si117xhrm_SpO2FrameProcess(Si117xhrmHandle_t *handle, int16_t *SpO2, int32_t *HeartRateInvalidation, Si117xhrmData_t *hrm_data)
{
  int32_t error = SI117xHRM_SUCCESS;
  int32_t SpO2_RED_DC, SpO2_IR_DC=1;
  int32_t SpO2_RED_AC2, SpO2_IR_AC2;
  int16_t SpO2_RED_AC_min, SpO2_RED_AC_max, SpO2_IR_AC_min=0, SpO2_IR_AC_max=0;
  int32_t SpO2_R;
  uint16_t SpO2_R_Root, SpO2_R_Root_Shift, SpO2_R_Root_TMP;
  uint16_t SpO2_AC2_Scaler;
  int16_t SpO2_RED_Crest_Factor, SpO2_IR_Crest_Factor;
  int16_t SpO2_Buffer_Out_index;	// Output index to the frame sample circular buffer.
  uint16_t spo2DcToAcRatio=0;		// Ratio of DC to ACpp. For debug reporting
  int16_t spo2CrestFactor=0;		// Crest factor (C^2)
  int16_t i, k;

  if (handle->spo2 == NULL)
    return SI117xHRM_STATUS_SPO2_EXCEPTION;

  // Frame process (Background job)
  if (handle->spo2->SpO2_Percent==SPO2_STATUS_PROCESS_SPO2_FRAME)  // HRM_frame_process sets the flag value when a new frame of samples is available.
  {
    handle->spo2->SpO2_Percent ^= SPO2_STATUS_PROCESS_SPO2_FRAME;	// Clear the flag. SpO2_Percent=0;
    // Count how many samples are Finger-On since the last Finger-Off.
    i=handle->spo2->SpO2_Buffer_In_index-handle->hrUpdateInterval;  // Set the output buffer index based on the Input index.
    if (i<0) i +=MAX_FRAME_SAMPLES;  // Wrap around
    for(k=0; k<handle->hrUpdateInterval; k++)  // Append the new block of samples
    {
      handle->spo2->SpO2_raw_level_count = handle->spo2->SpO2_raw_level_count>10000 ? handle->spo2->SpO2_raw_level_count:handle->spo2->SpO2_raw_level_count+1;  // limit the count to 10000
#if SPO2_REFLECTIVE_MODE
      if ((handle->spo2->SpO2_RED_DC_sample_buffer[i]<SpO2_DC_Min_Thresh) || (handle->spo2->SpO2_IR_DC_sample_buffer[i]<SpO2_DC_Min_Thresh))
#else
      if ((handle->spo2->SpO2_RED_DC_sample_buffer[i]>SpO2_DC_Min_Thresh) || (handle->spo2->SpO2_IR_DC_sample_buffer[i]>SpO2_DC_Min_Thresh))
#endif
      handle->spo2->SpO2_raw_level_count=0;		// Reset the count to 0
      if (++i==MAX_FRAME_SAMPLES) i=0; 	// Wrap around
    }

    SpO2_Buffer_Out_index=handle->spo2->SpO2_Buffer_In_index-handle->hrIframe;  // Set the output buffer index based on the Input index.
    if (SpO2_Buffer_Out_index<0) SpO2_Buffer_Out_index +=MAX_FRAME_SAMPLES;  // Wrap around

    handle->spo2->TimeSinceFingerOff = handle->spo2->TimeSinceFingerOff <60000 ? handle->spo2->TimeSinceFingerOff+1*handle->Fs/10 : handle->spo2->TimeSinceFingerOff;  // Limit to 60000

    if (handle->spo2->SpO2_raw_level_count<handle->hrIframe)
    {
      // Validation 1: Finger Off or On
      if (handle->spo2->SpO2_raw_level_count < handle->hrUpdateInterval)
      {
        *HeartRateInvalidation |= SI117xHRM_STATUS_SPO2_FINGER_OFF;
        handle->spo2->TimeSinceFingerOff=0;		// Reset if Finger off
      }
      else
        *HeartRateInvalidation |= SI117xHRM_STATUS_SPO2_FINGER_ON;
    }
    else
    {
      SpO2_RED_DC=0;
      SpO2_IR_DC=0;
      SpO2_RED_AC2=0;		// sum(Red AC^2)
      SpO2_IR_AC2=0;		// sum(IR AC^2)
      SpO2_RED_AC_min=0x7fff;
      SpO2_IR_AC_min=0x7fff;
      SpO2_RED_AC_max=0x8000;
      SpO2_IR_AC_max=0x8000;
      SpO2_AC2_Scaler=1;
      i=SpO2_Buffer_Out_index;
      for(k=0; k<handle->hrIframe; k++)
      {
        SpO2_RED_DC += handle->spo2->SpO2_RED_DC_sample_buffer[i];
        SpO2_IR_DC  += handle->spo2->SpO2_IR_DC_sample_buffer[i];
        if (SpO2_RED_AC2>0x20000000 || SpO2_IR_AC2>0x20000000)  // AC_sample_buffer[i]<0x7fff
        {
          SpO2_AC2_Scaler++; SpO2_RED_AC2 >>=1; SpO2_IR_AC2 >>=1;  // Change the scaler shift and scale sum(AC^2).
        }
        SpO2_RED_AC2 +=(handle->spo2->SpO2_RED_AC_sample_buffer[i]*handle->spo2->SpO2_RED_AC_sample_buffer[i])>>SpO2_AC2_Scaler;  // This is sum(AC^2)
        SpO2_IR_AC2  +=(handle->spo2->SpO2_IR_AC_sample_buffer[i] *handle->spo2->SpO2_IR_AC_sample_buffer[i] )>>SpO2_AC2_Scaler;  // This is sum(AC^2)

        // Find the AC min and max.
        SpO2_RED_AC_min=handle->spo2->SpO2_RED_AC_sample_buffer[i]<SpO2_RED_AC_min ? handle->spo2->SpO2_RED_AC_sample_buffer[i]:SpO2_RED_AC_min;
        SpO2_IR_AC_min =handle->spo2->SpO2_IR_AC_sample_buffer[i] <SpO2_IR_AC_min  ? handle->spo2->SpO2_IR_AC_sample_buffer[i] :SpO2_IR_AC_min;
        SpO2_RED_AC_max=handle->spo2->SpO2_RED_AC_sample_buffer[i]>SpO2_RED_AC_max ? handle->spo2->SpO2_RED_AC_sample_buffer[i]:SpO2_RED_AC_max;
        SpO2_IR_AC_max =handle->spo2->SpO2_IR_AC_sample_buffer[i] >SpO2_IR_AC_max  ? handle->spo2->SpO2_IR_AC_sample_buffer[i] :SpO2_IR_AC_max;

        if (++i==MAX_FRAME_SAMPLES)
          i=0;  // Wrap around
      }

      // Calculate Crest factor: C^2=|Vpeak|^2/Vrms^2.
      i= -SpO2_RED_AC_min > SpO2_RED_AC_max ?  -SpO2_RED_AC_min : SpO2_RED_AC_max;  // i=|Vpeak|
      SpO2_RED_Crest_Factor = SpO2_RED_AC2>0 ? ((((i*i)>>SpO2_AC2_Scaler)*handle->hrIframe)/SpO2_RED_AC2) : 9999;

      i= -SpO2_IR_AC_min  > SpO2_IR_AC_max  ?  -SpO2_IR_AC_min  : SpO2_IR_AC_max;   // i=|Vpeak|
      SpO2_IR_Crest_Factor  = SpO2_IR_AC2 >0 ? ((((i*i)>>SpO2_AC2_Scaler)*handle->hrIframe)/SpO2_IR_AC2) : 9999;

      spo2CrestFactor= SpO2_RED_Crest_Factor>SpO2_IR_Crest_Factor ? SpO2_RED_Crest_Factor : SpO2_IR_Crest_Factor;  // Take the larger one.

      // Validation 2: Check RED/IR AC with reference to its DC (to exclude the case on a non-finger surface or BPF transition).
      spo2DcToAcRatio=9999;
      if ((SpO2_RED_AC_max-SpO2_RED_AC_min)>0 && (SpO2_IR_AC_max-SpO2_IR_AC_min)>0)
      {
        spo2DcToAcRatio= (SpO2_RED_DC/handle->hrIframe)/(SpO2_RED_AC_max-SpO2_RED_AC_min);
        spo2DcToAcRatio = spo2DcToAcRatio>((SpO2_IR_DC/handle->hrIframe)/(SpO2_IR_AC_max-SpO2_IR_AC_min)) ? spo2DcToAcRatio:((SpO2_IR_DC/handle->hrIframe)/(SpO2_IR_AC_max-SpO2_IR_AC_min));
      }

      if (((SpO2_RED_DC/handle->hrIframe))>((SpO2_RED_AC_max-SpO2_RED_AC_min)*SpO2_DC_to_ACpp_Ratio_High_Thresh) || \
         ((SpO2_IR_DC/handle->hrIframe ))>((SpO2_IR_AC_max -SpO2_IR_AC_min )*SpO2_DC_to_ACpp_Ratio_High_Thresh))
      {
        *HeartRateInvalidation |= SI117xHRM_STATUS_SPO2_TOO_LOW_AC;		// Exclude the case on a non-finger surface
      }
      else if (((SpO2_RED_DC/handle->hrIframe))<((SpO2_RED_AC_max-SpO2_RED_AC_min)*SpO2_DC_to_ACpp_Ratio_Low_Thresh) || \
              ((SpO2_IR_DC/handle->hrIframe ))<((SpO2_IR_AC_max -SpO2_IR_AC_min )*SpO2_DC_to_ACpp_Ratio_Low_Thresh))
      {
        *HeartRateInvalidation |= SI117xHRM_STATUS_SPO2_TOO_HIGH_AC;	// Exclude possible BPF transition.
      }
      else if (spo2CrestFactor>SpO2_Crest_Factor_Thresh)
      {
    	// Validation 3: Crest factor check to exclude the finger moving and BPF transition.
        *HeartRateInvalidation |= SI117xHRM_STATUS_SPO2_CREST_FACTOR_OFF;
      }
      else
      {
        // Average DC and AC
        SpO2_RED_DC /= handle->hrIframe;	// Averaged RED DC
        SpO2_IR_DC	/= handle->hrIframe;	// Averaged IR DC
        SpO2_RED_AC2/= handle->hrIframe;	// Averaged RED AC^2
        SpO2_IR_AC2	/= handle->hrIframe;	// Averaged IR AC^2

        if (SpO2_RED_AC2>0 && SpO2_IR_AC2>0)
        {
          // SpO2(%)= Coeff_A - Coeff_B*R, where R=(ACred/DCred)/(ACir/DCir)
          // 1. Calculate RMS(R)^2 as AC2=RMS^2.
          SpO2_R=(R_SCALER*R_SCALER*((SpO2_IR_DC*SpO2_IR_DC)/SpO2_RED_DC))/SpO2_RED_DC;  // (R_SCALER*IR_DC/RED_DC)^2
          SpO2_R=(SpO2_R*SpO2_RED_AC2)/SpO2_IR_AC2;

          // 2. Calculate 32-bit square root(The input is 32-bit (max<=0x3fffffff). The output is the square root in 16-bit SpO2_R_Root)
          SpO2_R_Root=1<<(SQRT_SHIFT-1);
          SpO2_R_Root_Shift=1<<(SQRT_SHIFT-1);
          SpO2_R_Root_TMP=0;
          for (i=0; i<SQRT_SHIFT; i++)
          {
            if (SpO2_R>=(SpO2_R_Root*SpO2_R_Root))
            {
              SpO2_R_Root_TMP=SpO2_R_Root;
            }
            SpO2_R_Root_Shift >>= 1;  // Divided by 2
            SpO2_R_Root=SpO2_R_Root_Shift+SpO2_R_Root_TMP;
          }

          // 3. SpO2(%)=114 - 32*R (Coeff_A and Coeff_B require calibration on different hardware platforms)
          handle->spo2->SpO2_Percent=(int16_t)(114-(32*SpO2_R_Root+R_SCALER/2)/R_SCALER);  // R_SCALER/2 due to the round-off.
          // 4. Limit SpO2(%) to 75~99%
          handle->spo2->SpO2_Percent= handle->spo2->SpO2_Percent>99 ? 99 : handle->spo2->SpO2_Percent;  // Upper limit=99%
          if (handle->spo2->SpO2_Percent<75)
            *HeartRateInvalidation |= SI117xHRM_STATUS_SPO2_EXCEPTION;  // If SpO2<75%, SpO2 is invalid.

          if (handle->spo2->TimeSinceFingerOff < 51*handle->Fs/10/10)	// Delay 5.1(s) to report SpO2 after the last Finger off.
            *HeartRateInvalidation |= SI117xHRM_STATUS_SPO2_EXCEPTION;
        }
        else
        {
          // Exception: Can occur if the finger is a little distance above the sensors without contact.
          *HeartRateInvalidation |= SI117xHRM_STATUS_SPO2_EXCEPTION;
        }
      }
    }

    *SpO2=handle->spo2->SpO2_Percent;
    if(hrm_data != 0)  // Copy these varibles to hrm_data for display.
    {
      i=handle->spo2->SpO2_Buffer_In_index-1;  // i points to the newest sample.
      if (i<0) i += MAX_FRAME_SAMPLES;  // Wrap around
      hrm_data->spo2RedDcSample=handle->spo2->SpO2_RED_DC_sample_buffer[i];
      hrm_data->spo2IrDcSample=handle->spo2->SpO2_IR_DC_sample_buffer[i];
      hrm_data->spo2DcToAcRatio=spo2DcToAcRatio;
      hrm_data->spo2CrestFactor=spo2CrestFactor;
      hrm_data->spo2IrPerfusionIndex=10000*(SpO2_IR_AC_max-SpO2_IR_AC_min)/SpO2_IR_DC;
      for(i=0; i<4; i++) hrm_data->DC_Sensing_LED[i]=handle->DC_Sensing_LED[i];
    }
  }

  return error;
}
#endif

/**************************************************************************//**
 * @brief Initialize HRM/SpO2 measurement parameters
 *****************************************************************************/
static int32_t si117xhrm_InitMeasurementParameters (Si117xhrmHandle_t *handle, int16_t measurement_rate)
{
  int32_t error = SI117xHRM_SUCCESS;

  // Fs=21~25Hz. Interpolate by a factor of 4 to Fs=84~100Hz
  handle->HRM_Interpolator_Factor=4;
  handle->pHRM_Interpolator_Coefs=(int16_t *)HRM_Interpolator_Coefs_R4;


  // Set Fs and the BPF coefficients per measurementRate
  switch(measurement_rate)
  {
    case FS_25HZ:  // 25Hz
      handle->Fs=1000;	// (Hz*10)
      handle->BPF_biquads=sizeof(BPF_95Hz)/6/4;
      handle->pBPF_b_0=BPF_95Hz;
      handle->BPF_output_scaler=BPF_output_scaler_95Hz;
      break;
    default:
      error = SI117xHRM_ERROR_INVALID_MEASUREMENT_RATE;
      goto Error;
      // break not needed because of goto statement above
      // break;
  }

  handle->hrUpdateInterval=1*(handle->Fs+5)/10;	  // (sample) in 1s

  handle->T_Low0 =60*(handle->Fs+5)/10/f_Low-1;	  // (sample), -1 for the auto-corr max validation.
  handle->T_High0=60*(handle->Fs+5)/10/f_High+1;  // (sample), +1 for the auto-corr max validation.
  handle->NumOfLowestHRCycles=NumOfLowestHRCyclesMin;
  handle->hrIframe=handle->T_Low0*handle->NumOfLowestHRCycles/100;  // (sample)

  si117xhrm_InitializeBuffers(handle);

  // Normalize HRM PS to around 20000 for the HRM algorithm
  handle->HRM_Normalization_Scaler=(1<<QF_SCALER);	 // Scaler=1.0 in Qf
  #if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
    handle->RED_Normalization_Scaler=(1<<QF_SCALER);	// Scaler=1.0 in Qf
    handle->IR_Normalization_Scaler=(1<<QF_SCALER);	    // Scaler=1.0 in Qf
  #endif

  // Seems we can use same values for 3 cases.
  handle->HRM_PS_Vpp_max=HRM_PS_VPP_MAX_GREEN_FINGERTIP;
  handle->HRM_PS_CrestFactor_Thresh=HRM_CREST_FACTOR_THRESH_FINGERTIP;
  handle->HRM_PS_Vpp_min=handle->HRM_PS_Vpp_max * (-1);

Error:
  return error;
}

/**************************************************************************//**
 * @brief Get si117x sample from the sample queue
 *****************************************************************************/
static int32_t si117xhrm_GetSample(Si117xhrmHandle_t *handle, Si117xhrmIrqSample_t *samples)
{
  int32_t error = SI117xHRM_SUCCESS;

  error = sihrmUser_SampleQueue_Get(handle->si117x_handle, 0, samples);  // Set getAccelSamples to 0

  return error;
}

/**************************************************************************//**
 * @brief Identify si117x parts
 *****************************************************************************/
static int32_t si117xhrm_IdentifyPart(Si117xhrmHandle_t *handle, int16_t *part_id)
{
  int32_t valid_part = 0;

  *part_id=Si117xReadFromRegister(handle->si117x_handle, REG_PART_ID);

  switch(*part_id)
  { // Static HRM/SpO2 supports all Si117x parts
    case 0x71:
    case 0x72:
    case 0x73:
    case 0x74:
    case 0x75:
      valid_part = 1;
      break;
    default:
      valid_part = 0;
      break;
  }
  return valid_part;
}

/**************************************************************************//**
 * @brief Si117x HRM/SpO2 sample process
 *****************************************************************************/
static int32_t si117xhrm_SampleProcess(Si117xhrmHandle_t *handle, uint16_t HRM_timestamp, uint16_t hrmPs, uint16_t SpO2_PS_RED, uint16_t SpO2_PS_IR, Si117xhrmData_t *hrm_data)
{
  int32_t error = SI117xHRM_SUCCESS;
  uint16_t PS_Local;
  uint16_t Raw_PS_Local;
  uint16_t i, j, iInterpolator;
#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
  uint16_t SpO2_PS_RED_Scaled, SpO2_PS_IR_Scaled;
#endif
  int32_t Interpolator_PS;

  PS_Local=hrmPs;			// From Si117x EVB or the captured file
  Raw_PS_Local=hrmPs;		// Use for display purpose

#if HRM_PS_AGC  // Change the HRM normalized scaler, so that the HRM BPF will not see the disturbance due to LED current change.
  if(handle->HRM_AGC[0].agc_flag == 1)
  {
	handle->HRM_Normalization_Scaler = handle->HRM_Normalization_Scaler * handle->HRM_AGC[0].saved_ppg / hrmPs;  // Modify the normalization scaler based on the current and previous samples.
	for (i=0; i<(INTERPOLATOR_L*2); i++) handle->HRM_interpolator_buf[i] = handle->HRM_interpolator_buf[i]*hrmPs / handle->HRM_AGC[0].saved_ppg;  // Scale the HRM interpolator data buffer
	handle->HRM_AGC[0].agc_flag = 0;
  }
#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
  if(handle->HRM_AGC[1].agc_flag == 1)
  {
	handle->IR_Normalization_Scaler = handle->IR_Normalization_Scaler * handle->HRM_AGC[1].saved_ppg / SpO2_PS_IR;  // Modify the normalization scaler based on the current and previous samples.
	for (i=0; i<(INTERPOLATOR_L*2); i++) handle->spo2->SpO2_IR_interpolator_buf[i] = handle->spo2->SpO2_IR_interpolator_buf[i]*SpO2_PS_IR / handle->HRM_AGC[1].saved_ppg;  // Scale the SpO2 interpolator data buffer
	handle->HRM_AGC[1].agc_flag = 0;
  }
  if(handle->HRM_AGC[2].agc_flag == 1)
  {
	handle->RED_Normalization_Scaler = handle->RED_Normalization_Scaler * handle->HRM_AGC[2].saved_ppg / SpO2_PS_RED;  // Modify the normalization scaler based on the current and previous samples.
	for (i=0; i<(INTERPOLATOR_L*2); i++) handle->spo2->SpO2_Red_interpolator_buf[i] = handle->spo2->SpO2_Red_interpolator_buf[i]*SpO2_PS_IR / handle->HRM_AGC[2].saved_ppg;  // Scale the SpO2 interpolator data buffer
	handle->HRM_AGC[2].agc_flag = 0;
  }
#endif
#endif

  handle->HRM_AGC[0].saved_ppg = hrmPs;        // ppg1
  handle->HRM_AGC[1].saved_ppg = SpO2_PS_IR;   // ppg2
  handle->HRM_AGC[2].saved_ppg = SpO2_PS_RED;  // ppg3

  handle->hrmRawPs=Raw_PS_Local;    // Save the raw PS for next sample or frame process.

  // Interpolator causes a delay of INTERPOLATOR_L-1 samples due to the its filter.
  for (i=0; i<(INTERPOLATOR_L*2-1); i++) handle->HRM_interpolator_buf[i]=handle->HRM_interpolator_buf[i+1];  // Use memmove() instead for efficiency
  handle->HRM_interpolator_buf[i]=PS_Local;  // Add the new HRM sample to the interpolator buffer

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
  if (handle->spo2 != NULL)  // SpO2 interpolator buffer shift and add new samples.
  {
    for (i=0; i<(INTERPOLATOR_L*2-1); i++)  // Shift the buffer
    {
      handle->spo2->SpO2_Red_interpolator_buf[i]=handle->spo2->SpO2_Red_interpolator_buf[i+1];		// Use memmove() instead for efficiency
      handle->spo2->SpO2_IR_interpolator_buf[i] =handle->spo2->SpO2_IR_interpolator_buf[i+1];		// Use memmove() instead for efficiency
    }
    handle->spo2->SpO2_Red_interpolator_buf[i]=SpO2_PS_RED;  // Add the new SpO2 Red sample to the interpolator buffer
    handle->spo2->SpO2_IR_interpolator_buf[i] =SpO2_PS_IR;   // Add the new SpO2 IR sample to the interpolator buffer
  }
#endif

  for (iInterpolator=0; iInterpolator<handle->HRM_Interpolator_Factor; iInterpolator++)
  {
    if (iInterpolator==0)
    {
      if (handle->HRM_Interpolator_Factor>1)  // Don't touch hrmPs and SpO2_PS_RED/IR if handle->HRM_Interpolator_Factor==1;
      {
        PS_Local=handle->HRM_interpolator_buf[INTERPOLATOR_L-1];
#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
        if (handle->spo2 != NULL)  // SpO2 interpolating
        {
          SpO2_PS_RED=handle->spo2->SpO2_Red_interpolator_buf[INTERPOLATOR_L-1];
          SpO2_PS_IR =handle->spo2->SpO2_IR_interpolator_buf[INTERPOLATOR_L-1];
        }
#endif
      }
    }
    else
    {
      Interpolator_PS=0;
      for (j=0; j<INTERPOLATOR_L*2; j++)
        Interpolator_PS += handle->HRM_interpolator_buf[j]*(handle->pHRM_Interpolator_Coefs[(iInterpolator-1)*INTERPOLATOR_L*2+j]);  // -1 because of no 1st interpolator
      PS_Local= Interpolator_PS<0 ? 0: (Interpolator_PS>>15);	// Interpolator_Coefs are in Q15

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
      if (handle->spo2 != NULL)  // SpO2 interpolating
      {
        Interpolator_PS=0;
        for (j=0; j<INTERPOLATOR_L*2; j++)
          Interpolator_PS += handle->spo2->SpO2_Red_interpolator_buf[j]*(handle->pHRM_Interpolator_Coefs[(iInterpolator-1)*INTERPOLATOR_L*2+j]);  // -1 because of no 1st interpolator
        SpO2_PS_RED= Interpolator_PS<0 ? 0: (Interpolator_PS>>15);	// Interpolator_Coefs are in Q15
        Interpolator_PS=0;
        for (j=0; j<INTERPOLATOR_L*2; j++)
          Interpolator_PS += handle->spo2->SpO2_IR_interpolator_buf[j] *(handle->pHRM_Interpolator_Coefs[(iInterpolator-1)*INTERPOLATOR_L*2+j]);  // -1 because of no 1st interpolator
        SpO2_PS_IR = Interpolator_PS<0 ? 0: (Interpolator_PS>>15);	// Interpolator_Coefs are in Q15
      }
#endif
    }

    Raw_PS_Local=PS_Local;  // Raw_PS_Local after interpolator if handle->HRM_Interpolator_Factor>1.

	// 0) Normalize PS
	PS_Local=(uint16_t)(((int32_t)PS_Local*(int32_t)handle->HRM_Normalization_Scaler)>>QF_SCALER);	// HRM_Normalization_Scaler in Q8

	si117xhrm_BPF_filtering(handle, (int32_t)PS_Local, &handle->HRM_sample_buffer[handle->HRM_Buffer_In_Index], &handle->HRM_BPF);  // Add the BPF(PS) in the framed sample circular buffer.

#if (SI117XHRM_BUILD_GECKO_SPO2 == 1)  // Compile In/Out SpO2
	if (handle->spo2 != NULL)  // SpO2 BPF process and compute Red_DC, Red_AC, IR_DC and IR_AC
	{
	  // Normalize SpO2 PSs, removing the DC floor of 256
	  SpO2_PS_RED_Scaled=(uint16_t)(((int32_t)(SpO2_PS_RED-256)*(int32_t)handle->RED_Normalization_Scaler)>>QF_SCALER);	 // RED_Normalization_Scaler in Q8
	  SpO2_PS_IR_Scaled=(uint16_t)(((int32_t)(SpO2_PS_IR-256)*(int32_t)handle->IR_Normalization_Scaler)>>QF_SCALER);	 // IR_Normalization_Scaler in Q8

	  si117xhrm_BPF_filtering(handle, (int32_t)SpO2_PS_RED_Scaled, &handle->spo2->SpO2_RED_AC_sample_buffer[handle->spo2->SpO2_Buffer_In_index], &handle->spo2->SpO2_Red_BPF);  // Add the BPF(PS2) in the framed sample circular buffer.
	  si117xhrm_BPF_filtering(handle, (int32_t)SpO2_PS_IR_Scaled,  &handle->spo2->SpO2_IR_AC_sample_buffer[handle->spo2->SpO2_Buffer_In_index],  &handle->spo2->SpO2_IR_BPF);   // Add the BPF(PS3) in the framed sample circular buffer.
	  handle->spo2->SpO2_RED_DC_sample_buffer[handle->spo2->SpO2_Buffer_In_index]=(uint16_t)SpO2_PS_RED_Scaled;
	  handle->spo2->SpO2_IR_DC_sample_buffer[handle->spo2->SpO2_Buffer_In_index] =(uint16_t)SpO2_PS_IR_Scaled;
	  if (++handle->spo2->SpO2_Buffer_In_index==MAX_FRAME_SAMPLES) handle->spo2->SpO2_Buffer_In_index=0;  // Wrap around
	}
#endif

    // Count how many samples are valid since the last Finger-Off. Finger-On detection uses the raw PS and its detection threshold.
    if (++handle->HRM_PS_raw_level_count>10000) handle->HRM_PS_raw_level_count--;		// limit the count

    handle->sample_count++;

    if (hrm_data != 0)  // Copy these variables to hrm_data for display.
    {
      hrm_data->Fs = handle->Fs/handle->HRM_Interpolator_Factor;
      hrm_data->hrIframe = handle->hrIframe;
      hrm_data->hrUpdateInterval = handle->hrUpdateInterval/handle->HRM_Interpolator_Factor;
      hrm_data->hrmRawPs = Raw_PS_Local;
      hrm_data->hrmPs = PS_Local;
      hrm_data->hrmNumSamples = handle->HRM_Interpolator_Factor;
      hrm_data->hrmInterpolatedPs[iInterpolator] = Raw_PS_Local;
      hrm_data->hrmSamples[iInterpolator] = handle->HRM_sample_buffer[handle->HRM_Buffer_In_Index];
    }

    if (++handle->HRM_Buffer_In_Index==MAX_FRAME_SAMPLES) handle->HRM_Buffer_In_Index=0;  // Wrap around
  }
  handle->Normalized_HRM_PS=PS_Local;  // Save the HRM raw PS for next sample or frame process.

  return error;
}
