/***************************************************************************//**
 * @file si117xdrv.c
 * @brief SI117XDRV API implementation.
 * @version 5.2.2
 *******************************************************************************
 * # License
 * <b>Copyright 2017 Silicon Laboratories, Inc, 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 "si117xdrv.h"
#include <stdint.h>
#include <stddef.h>
#include "si117xdrv_config.h"
#include "si117x_functions.h"



#define MEASCOUNT_LSB  20000 //20khz or 50us
#define MEASRATE_MIN   3
#define MEASRATE_MAX   0xffff
#define MEASCOUNT_MAX  0xff

#define PPG_OFFSET 7

/***************************************************************************//**
 * Config/Control typedefs
 ******************************************************************************/

/// General callback function datatype
typedef struct
{
	SI117XDRV_Callback_t callback; ///< Callback function pointer
	void                 *user;    ///< User supplied data
	bool                 allocated;///< Flag to indicate callback data is set
}SI117XDRV_CallbackData_t;


/// Accel sync DLE callback function datatype
typedef struct
{
	SI117XDRV_AccelCallback_t callback; ///< Callback function pointer
	void                 *user;         ///< User supplied data
	bool                 allocated; ///< Flag to indicate callback data is set
}SI117XDRV_AccelCallbackData_t;

/// Data structure containing all callback info
typedef struct
{
	SI117XDRV_CallbackData_t preStartCallbacks[SI117XDRV_MAX_NUM_CALLBACKS];
	SI117XDRV_CallbackData_t postStartCallbacks[SI117XDRV_MAX_NUM_CALLBACKS];
	SI117XDRV_CallbackData_t preFifoFlushCallbacks[SI117XDRV_MAX_NUM_CALLBACKS];
	SI117XDRV_CallbackData_t postFifoFlushCallbacks[SI117XDRV_MAX_NUM_CALLBACKS];
	SI117XDRV_AccelCallbackData_t accelSyncRecvCallbacks[SI117XDRV_MAX_NUM_CALLBACKS];
}SI117XDRV_Callbacks_t;


typedef struct {
  uint8_t          taskEnable;             ///< The global task enable register
  uint16_t         measrate;
  uint16_t         fifo_int_level;
  uint8_t          meas_cntl;
  uint8_t          synch_config;
  uint8_t          input_freq_sel;
  uint8_t          irq_enable;
} SI117XDRV_GlobalRegisters_t;

typedef struct {
	uint8_t ecg_meascount;
	uint8_t ecg_measconfig;
	uint8_t ecg_adcconfig;
	uint8_t ecg_feconfig;
	uint8_t ecg_threshold;
	uint8_t ecg_ld_meascount;
	uint8_t ecg_ldconfig;
} SI117XDRV_ECGRegisters_t;

typedef struct
{
	uint8_t	bioz_control1;
	uint8_t bioz_control2;
	uint8_t bioz_fe_gain1;
	uint8_t bioz_fe_gain2;
	uint8_t bioz_fe_mux;
} SI117XDRV_BIOZChannelRegisters_t;

typedef struct
{
  uint8_t ppg_led1_config;
  uint8_t ppg_led2_config;
  uint8_t ppg_led3_config;
  uint8_t ppg_led4_config;
  uint8_t ppg_mode;
  uint8_t ppg_measconfig;
  uint8_t ppg_adcconfig;
} SI117XDRV_PPGChannelRegisters_t;

typedef struct
{
  uint8_t ppg_meascount;
  uint8_t ppg4_threshold;
  SI117XDRV_PPGChannelRegisters_t ppgCfg[4];
} SI117XDRV_PPGRegisters_t;


typedef struct
{
  SI117XDRV_BIOZChannelRegisters_t biozConfig[2];
  uint8_t bioz_meascount;
  uint8_t bioz_dc_value;
} SI117XDRV_BioZRegisters_t;

typedef struct
{
	SI117XDRV_GlobalRegisters_t globalCfg;
	SI117XDRV_ECGRegisters_t    ecgCfg;
	SI117XDRV_PPGRegisters_t    ppgCfg;
	SI117XDRV_BioZRegisters_t   biozCfg;
} SI117XDRV_DeviceRegisters_t;

typedef struct
{
  uint8_t partNumber;
  uint8_t hrmOversamplingRatio; //HRM expects 25Hz
  bool running;
  uint8_t savedTasks;
  uint8_t irqSave;
  uint16_t savedMeasrate;
  uint8_t  savedPPG_meascount;
  uint8_t  savedECG_meascount;
} SI117XDRV_DeviceStatus_t;

typedef struct
{
  HANDLE deviceHandle;
  SI117XDRV_DeviceRegisters_t   deviceCfg;
  SI117XDRV_DeviceStatus_t deviceStatus;
} SI117XDRV_Device_t;

/***************************************************************************//**
 * Static variables
 ******************************************************************************/

static SI117XDRV_Device_t                     devices[ SI117XDRV_MAX_NUM_DEVICES ];
static SI117XDRV_Callbacks_t                  callbacks[ SI117XDRV_MAX_NUM_DEVICES ];
static bool leadDetectionFlag [SI117XDRV_MAX_NUM_DEVICES];
static bool wristDetectionFlag [SI117XDRV_MAX_NUM_DEVICES];
static bool leadDetect[SI117XDRV_MAX_NUM_DEVICES];

static int initFlag[SI117XDRV_MAX_NUM_DEVICES] = { 0 }; // all elements 0, cannot put this in device struct because it must be initilaized

/***************************************************************************//**
 * @brief Call accel sync DLE received callback functions
 ******************************************************************************/
static void accelSyncCallbacks (SI117XDRV_DeviceSelect_t device, uint16_t ppg_count)
{
	int i;
		for (i=0;i<SI117XDRV_MAX_NUM_CALLBACKS;i++)
		{
			if (callbacks[device].accelSyncRecvCallbacks[i].allocated)
			{
	           callbacks[device].accelSyncRecvCallbacks[i].callback(device,ppg_count,callbacks[device].accelSyncRecvCallbacks[i].user);
			}
		}
}

static void preStartCallbacks (SI117XDRV_DeviceSelect_t device)
{
	int i;
		for (i=0;i<SI117XDRV_MAX_NUM_CALLBACKS;i++)
		{
			if (callbacks[device].preStartCallbacks[i].allocated)
			{
	           callbacks[device].preStartCallbacks[i].callback(device,callbacks[device].preStartCallbacks[i].user);
			}
		}
}

static void postStartCallbacks (SI117XDRV_DeviceSelect_t device)
{
	int i;
		for (i=0;i<SI117XDRV_MAX_NUM_CALLBACKS;i++)
		{
			if (callbacks[device].postStartCallbacks[i].allocated)
			{
	           callbacks[device].postStartCallbacks[i].callback(device,callbacks[device].postStartCallbacks[i].user);
			}
		}
}
static void preFifoFlushCallbacks (SI117XDRV_DeviceSelect_t device)
{
	int i;
		for (i=0;i<SI117XDRV_MAX_NUM_CALLBACKS;i++)
		{
			if (callbacks[device].preFifoFlushCallbacks[i].allocated)
			{
	           callbacks[device].preFifoFlushCallbacks[i].callback(device,callbacks[device].preFifoFlushCallbacks[i].user);
			}
		}
}
static void postFifoFlushCallbacks (SI117XDRV_DeviceSelect_t device)
{
	int i;
		for (i=0;i<SI117XDRV_MAX_NUM_CALLBACKS;i++)
		{
			if (callbacks[device].postFifoFlushCallbacks[i].allocated)
			{
	           callbacks[device].postFifoFlushCallbacks[i].callback(device,callbacks[device].postFifoFlushCallbacks[i].user);
			}
		}
}
/***************************************************************************//**
 * @brief
 *    Flushes the Si117x fifo
 *
 * @param[in] device select ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_FifoFlush (SI117XDRV_DeviceSelect_t device)
{
	preFifoFlushCallbacks(device);
	Si117xFlushFIFO(devices[device].deviceHandle);
	postFifoFlushCallbacks(device);
	return ECODE_SI117XDRV_OK;
}


/***************************************************************************//**
 * @brief
 *    Resets the Si117x
 *
 * @param[in] device select ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_Reset (SI117XDRV_DeviceSelect_t device)
{
	int i;
	uint8_t retval;
    //devices[device].deviceHandle = NULL;
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
    devices[device].deviceStatus.running = false;
    devices[device].deviceStatus.savedTasks = 0;
    devices[device].deviceStatus.hrmOversamplingRatio = 1;
    devices[device].deviceStatus.partNumber = 0x70;
    devices[device].deviceCfg.globalCfg.fifo_int_level  = 0x100;
    devices[device].deviceCfg.globalCfg.meas_cntl = 0x4;
    devices[device].deviceCfg.globalCfg.measrate = 0x320;
    devices[device].deviceCfg.globalCfg.synch_config = 0;
    devices[device].deviceCfg.globalCfg.input_freq_sel = 0;
    devices[device].deviceCfg.globalCfg.taskEnable = 0;
    devices[device].deviceCfg.ppgCfg.ppg4_threshold = 0;
    devices[device].deviceCfg.ppgCfg.ppg_meascount = 1;
    devices[device].deviceCfg.globalCfg.irq_enable = 0;
    for (i=0;i<4;i++)
    {
      devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_mode = 0xf;
      devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_adcconfig = 0x30;
      devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_led1_config = 0;
      devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_led2_config = 0;
      devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_led3_config = 0;
      devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_led4_config = 0;
      devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_measconfig = 3;
    }
    devices[device].deviceCfg.ecgCfg.ecg_adcconfig = 0x8;
    devices[device].deviceCfg.ecgCfg.ecg_feconfig = 0;
    devices[device].deviceCfg.ecgCfg.ecg_ld_meascount = 0;
    devices[device].deviceCfg.ecgCfg.ecg_ldconfig = 0;
    devices[device].deviceCfg.ecgCfg.ecg_measconfig = 0;
    devices[device].deviceCfg.ecgCfg.ecg_meascount = 0;
    devices[device].deviceCfg.ecgCfg.ecg_threshold = 0;
    devices[device].deviceCfg.biozCfg.bioz_dc_value = 0;
    devices[device].deviceCfg.biozCfg.bioz_meascount = 0;
    for (i=0;i<2;i++)
    {
      devices[device].deviceCfg.biozCfg.biozConfig[i].bioz_control1 = 0;
      devices[device].deviceCfg.biozCfg.biozConfig[i].bioz_control2 = 0;
      devices[device].deviceCfg.biozCfg.biozConfig[i].bioz_fe_gain1 = 0;
      devices[device].deviceCfg.biozCfg.biozConfig[i].bioz_fe_gain2 = 0;
      devices[device].deviceCfg.biozCfg.biozConfig[i].bioz_fe_mux = 0;
    }
    //reset part, send hw_key, read part id,
    leadDetect[device] = false;
    leadDetectionFlag[device]=false;
    wristDetectionFlag[device]=false;
    retval = Si117xReset(devices[device].deviceHandle);
    retval += Si117xWriteToRegister(devices[device].deviceHandle, REG_HW_KEY, 0x00); // Set HW_KEY to 0
    Si117xReadFromRegister (devices[device].deviceHandle, REG_HW_KEY); // Read back HW_KEY to check
    devices[device].deviceStatus.partNumber = Si117xReadFromRegister (devices[device].deviceHandle, REG_PART_ID); // Read part id
    //detect device
    return ECODE_SI117XDRV_OK;
}


/***************************************************************************//**
 * @brief
 *    Returns the part number read from the Si117x
 *
 * @param[in] device select ID
 *
 * @param[in] pointer to byte to store part id
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_GetPartInfo (SI117XDRV_DeviceSelect_t device, uint8_t *id, uint8_t *rev, uint8_t *mfr_id, uint8_t *pkg_led_cfg)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	*id = devices[device].deviceStatus.partNumber;
	*rev = Si117xReadFromRegister (devices[device].deviceHandle, REG_REV_ID);
	*mfr_id = Si117xReadFromRegister (devices[device].deviceHandle, REG_MFR_ID) >> 5;
	*pkg_led_cfg = Si117xReadFromRegister (devices[device].deviceHandle, REG_MFR_ID) & 0x1F;
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Returns the current fifo interrupt level the Si117x
 *
 * @param[in] device select ID
 *
 * @param[in] pointer to uint16_t store the fifo level
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_GetFifoIntLevel (SI117XDRV_DeviceSelect_t device, uint16_t *fifo_int_level)
{
    *fifo_int_level = devices[device].deviceCfg.globalCfg.fifo_int_level;
    return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Configure ECG parameters
 *
 * @param[in] device select ID
 *
 * @param[in] pointer to ECG configuration structure
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_InitECG (SI117XDRV_DeviceSelect_t device, SI117XDRV_ECGCfg_t *ecgCfg)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running)
	{
		return ECODE_SI117XDRV_RUNNING;
	}
	devices[device].deviceCfg.ecgCfg.ecg_adcconfig = ecgCfg->ecg_adcconfig;
	Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_ADCCONFIG,ecgCfg->ecg_adcconfig);
	devices[device].deviceCfg.ecgCfg.ecg_measconfig = ecgCfg->ecg_measconfig;
	Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_MEASCONFIG,ecgCfg->ecg_measconfig);
	devices[device].deviceCfg.ecgCfg.ecg_feconfig = ecgCfg->ecg_feconfig;
	Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_FECONFIG,ecgCfg->ecg_feconfig);
	return ECODE_SI117XDRV_OK;
}


static void InitializeEcgBiasVoltage(SI117XDRV_DeviceSelect_t device)
{
	// enables ECG suspend mode to briefly initalize the ECG front end circuit to the correct bias voltage.
	// this is needed prior to ECG_LD if ECG is not active
   // charge up the ECG_REF voltage

	if (!(devices[device].deviceCfg.globalCfg.taskEnable & 0x40))
	{
		Si117xParamSet(devices[device].deviceHandle, PARAM_TASK_ENABLE, devices[device].deviceCfg.globalCfg.taskEnable | 0x40);
		Si117xParamSet(devices[device].deviceHandle, PARAM_ECG_ADCCONFIG, devices[device].deviceCfg.ecgCfg.ecg_adcconfig | 0x08);
		delay_10ms();
		Si117xParamSet(devices[device].deviceHandle, PARAM_TASK_ENABLE, devices[device].deviceCfg.globalCfg.taskEnable);
		Si117xParamSet(devices[device].deviceHandle, PARAM_ECG_ADCCONFIG, devices[device].deviceCfg.ecgCfg.ecg_adcconfig);
	}
}

/***************************************************************************//**
 * @brief
 *    Configure ECG LD parameters
 *
 * @param[in] device select ID
 *
 * @param[in] pointer to ECG LD configuration structure
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_InitECGLD (SI117XDRV_DeviceSelect_t device, SI117XDRV_ECGLDCfg_t *ecgCfg)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running)
	{
		return ECODE_SI117XDRV_RUNNING;
	}
	InitializeEcgBiasVoltage(device);
	devices[device].deviceCfg.ecgCfg.ecg_threshold = ecgCfg->ecg_threshold;
	Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_THRESHOLD,ecgCfg->ecg_threshold);
	devices[device].deviceCfg.ecgCfg.ecg_ldconfig = ecgCfg->ecg_ldconfig;
	Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_LDCONFIG,ecgCfg->ecg_ldconfig);
	devices[device].deviceCfg.globalCfg.input_freq_sel = (ecgCfg->ecg_ld_freq_sel<<4) | (devices[device].deviceCfg.globalCfg.input_freq_sel&0xf);
	Si117xParamSet(devices[device].deviceHandle,PARAM_INPUT_FREQ_SEL,devices[device].deviceCfg.globalCfg.input_freq_sel);
	return ECODE_SI117XDRV_OK;
}
/***************************************************************************//**
 * @brief
 *    Configure PPG parameters
 *
 * @param[in] device select ID
 *
 * @param[in] pointer to PPG configuration structure
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_InitPPG (SI117XDRV_DeviceSelect_t device, SI117XDRV_PPGCfg_t *ppgCfg)
{
	int i;
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running)
	{
		return ECODE_SI117XDRV_RUNNING;
	}
	devices[device].deviceCfg.ppgCfg.ppg4_threshold = ppgCfg->ppg4_threshold;
	Si117xParamSet(devices[device].deviceHandle,PARAM_PPG4_THRESHOLD,ppgCfg->ppg4_threshold);
    for (i=0;i<4;i++)
    {
    	devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_adcconfig = ppgCfg->ppgCfg[i].ppg_adcconfig;
    	Si117xParamSet(devices[device].deviceHandle,PARAM_PPG1_ADCCONFIG+i*PPG_OFFSET,ppgCfg->ppgCfg[i].ppg_adcconfig);
    	devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_measconfig = ppgCfg->ppgCfg[i].ppg_measconfig;
    	Si117xParamSet(devices[device].deviceHandle,PARAM_PPG1_MEASCONFIG+i*PPG_OFFSET,ppgCfg->ppgCfg[i].ppg_measconfig);
    	devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_mode = ppgCfg->ppgCfg[i].ppg_mode;
    	Si117xParamSet(devices[device].deviceHandle,PARAM_PPG1_MODE+i*PPG_OFFSET,ppgCfg->ppgCfg[i].ppg_mode);
    	devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_led1_config = ppgCfg->ppgCfg[i].ppg_led1_config;
    	Si117xParamSet(devices[device].deviceHandle,PARAM_PPG1_LED1_CONFIG+i*PPG_OFFSET,ppgCfg->ppgCfg[i].ppg_led1_config);
    	devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_led2_config = ppgCfg->ppgCfg[i].ppg_led2_config;
    	Si117xParamSet(devices[device].deviceHandle,PARAM_PPG1_LED2_CONFIG+i*PPG_OFFSET,ppgCfg->ppgCfg[i].ppg_led2_config);
    	devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_led3_config = ppgCfg->ppgCfg[i].ppg_led3_config;
    	Si117xParamSet(devices[device].deviceHandle,PARAM_PPG1_LED3_CONFIG+i*PPG_OFFSET,ppgCfg->ppgCfg[i].ppg_led3_config);
    	devices[device].deviceCfg.ppgCfg.ppgCfg[i].ppg_led4_config = ppgCfg->ppgCfg[i].ppg_led4_config;
    	Si117xParamSet(devices[device].deviceHandle,PARAM_PPG1_LED4_CONFIG+i*PPG_OFFSET,ppgCfg->ppgCfg[i].ppg_led4_config);
    }
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Configure PPG tasks
 *
 * @param[in] device select ID
 *
 * @param[in] task enable bits for PPG
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_SetPPGTaskEnable (SI117XDRV_DeviceSelect_t device, uint8_t ppgTasks) //returns an error if device is currently running
{
	Si117x_Ecode_t retval;
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running)
	{
		return ECODE_SI117XDRV_RUNNING;
	}
	devices[device].deviceCfg.globalCfg.taskEnable = devices[device].deviceCfg.globalCfg.taskEnable&0xf0;
	devices[device].deviceCfg.globalCfg.taskEnable |= ppgTasks&0xf;
	retval = Si117xParamSet(devices[device].deviceHandle,PARAM_TASK_ENABLE,devices[device].deviceCfg.globalCfg.taskEnable);
	return retval;
}


/***************************************************************************//**
 * @brief
 *    Configure ECG tasks
 *
 * @param[in] device select ID
 *
 * @param[in] task enable bits for ECG
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_SetECGTaskEnable (SI117XDRV_DeviceSelect_t device, uint8_t ecgTasks)
{
	Si117x_Ecode_t retval;
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running)
	{
		return ECODE_SI117XDRV_RUNNING;
	}
	devices[device].deviceCfg.globalCfg.taskEnable = devices[device].deviceCfg.globalCfg.taskEnable&0x3F;
	devices[device].deviceCfg.globalCfg.taskEnable |= ecgTasks&0xc0;
	retval = Si117xParamSet(devices[device].deviceHandle,PARAM_TASK_ENABLE,devices[device].deviceCfg.globalCfg.taskEnable);
	return retval;
}

/***************************************************************************//**
 * @brief
 *    Enable/disable fifo interrupt
 *
 * @param[in] device select ID
 *
 * @param[in] Set enable to true to enable interrupt
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_FifoIntConfig (SI117XDRV_DeviceSelect_t device, bool enable)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	devices[device].deviceCfg.globalCfg.irq_enable = devices[device].deviceCfg.globalCfg.irq_enable & ~(0x1);
	if (enable)
	{
	    devices[device].deviceCfg.globalCfg.irq_enable |= 0x1;
	}
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Enable/disable PPG interrupt
 *
 * @param[in] device select ID
 *
 * @param[in] Set enable to true to enable interrupt
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_PPG1IntConfig (SI117XDRV_DeviceSelect_t device, bool enable)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	devices[device].deviceCfg.globalCfg.irq_enable = devices[device].deviceCfg.globalCfg.irq_enable & ~(0x2);
	if (enable)
	{
	    devices[device].deviceCfg.globalCfg.irq_enable |= 0x2;
	}
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Enable/disable lead detection interrupt
 *
 * @param[in] device select ID
 *
 * @param[in] Set enable to true to enable interrupt
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_LDIntConfig (SI117XDRV_DeviceSelect_t device, bool enable)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	devices[device].deviceCfg.globalCfg.irq_enable = devices[device].deviceCfg.globalCfg.irq_enable & ~(0x8);
	if (enable)
	{
	    devices[device].deviceCfg.globalCfg.irq_enable |= 0x8;
	}
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Enable/disable wrist detection interrupt
 *
 * @param[in] device select ID
 *
 * @param[in] Set enable to true to enable interrupt
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_WDIntConfig (SI117XDRV_DeviceSelect_t device, bool enable)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	devices[device].deviceCfg.globalCfg.irq_enable = devices[device].deviceCfg.globalCfg.irq_enable & ~(0x4);
	if (enable)
	{
	    devices[device].deviceCfg.globalCfg.irq_enable |= 0x4;
	}
	return ECODE_SI117XDRV_OK;
}


/***************************************************************************//**
 * @brief
 *    Start legacy PPG mode (ECG & BIOZ will be disabled before start)
 *    This function overwrites some configuration settings which are then
 *    restored in the accompanying Stop function.
 *
 * @param[in] device select ID
 *
 * @param[in] ppg_meascount value to use (overwrites any current setting)
 *
 * @param[in] measrate value to use (overwrites any current setting)
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_StartLegacyPPG (SI117XDRV_DeviceSelect_t device, uint8_t ppg_meascount, uint16_t measrate)  //used with HRM algorithm (required for DC sense)
{
	Si117x_Ecode_t retval = ECODE_SI117XDRV_OK;
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running)
	{
		return ECODE_SI117XDRV_RUNNING;
	}
  //move anything other than ppg to pendingTasks
  devices[device].deviceStatus.savedTasks = devices[device].deviceCfg.globalCfg.taskEnable;
  devices[device].deviceStatus.irqSave = devices[device].deviceCfg.globalCfg.irq_enable;
  devices[device].deviceStatus.savedMeasrate = devices[device].deviceCfg.globalCfg.measrate;
  devices[device].deviceStatus.savedPPG_meascount = devices[device].deviceCfg.ppgCfg.ppg_meascount;
  //devices[device].deviceStatus.savedECG_meascount = devices[device].deviceCfg.ecgCfg.ecg_meascount;
  devices[device].deviceCfg.globalCfg.taskEnable = devices[device].deviceCfg.globalCfg.taskEnable & 0xf;
  devices[device].deviceCfg.globalCfg.irq_enable = (SI117XDRV_IRQ_EN_FIFO+SI117XDRV_IRQ_EN_PPG1);
  retval += Si117xParamSet(devices[device].deviceHandle,PARAM_TASK_ENABLE,devices[device].deviceCfg.globalCfg.taskEnable);
  //switch to new rate
  //Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_MEASCOUNT,0);
  retval += Si117xParamSet(devices[device].deviceHandle,PARAM_PPG_MEASCOUNT,ppg_meascount);
  retval += Si117xParamSet(devices[device].deviceHandle, PARAM_MEASRATE_L, (measrate & 0xff));
  retval += Si117xParamSet(devices[device].deviceHandle, PARAM_MEASRATE_H, (measrate & 0xff00)>>8);
  retval += SI117XDRV_InitializeSi117xFifoStateMachine(device);
  preStartCallbacks(device);

  retval = Si117xWriteToRegister (devices[device].deviceHandle, REG_IRQ_ENABLE, (SI117XDRV_IRQ_EN_FIFO+SI117XDRV_IRQ_EN_PPG1));
  SI117XDRV_FifoFlush(device);
  retval += Si117xStart(devices[device].deviceHandle);
  devices[device].deviceStatus.running = true;
  postStartCallbacks(device);
  return retval;

}

/***************************************************************************//**
 * @brief
 *    Stop legacy PPG mode (restores ECG & BIOZ tasks as previously set)
 *
 * @param[in] device select ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_StopLegacyPPG (SI117XDRV_DeviceSelect_t device)
{
	//restore ppg meascounts, measrate, and irqenable etc to previous state...
	uint8_t retval = 0;
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running == false)
	{
		return ECODE_SI117XDRV_NOT_RUNNING;
	}
	SI117XDRV_Stop(device);
	devices[device].deviceCfg.globalCfg.taskEnable = devices[device].deviceStatus.savedTasks;
	devices[device].deviceCfg.globalCfg.irq_enable = devices[device].deviceStatus.irqSave;
	devices[device].deviceCfg.globalCfg.measrate = devices[device].deviceStatus.savedMeasrate;
	devices[device].deviceCfg.ppgCfg.ppg_meascount = devices[device].deviceStatus.savedPPG_meascount;
	//devices[device].deviceCfg.ecgCfg.ecg_meascount = devices[device].deviceStatus.savedECG_meascount;
	//Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_MEASCOUNT,devices[device].deviceStatus.savedECG_meascount);
	retval += Si117xParamSet(devices[device].deviceHandle,PARAM_PPG_MEASCOUNT,devices[device].deviceStatus.savedPPG_meascount);
	retval += Si117xParamSet(devices[device].deviceHandle, PARAM_MEASRATE_L, (devices[device].deviceStatus.savedMeasrate & 0xff));
	retval += Si117xParamSet(devices[device].deviceHandle, PARAM_MEASRATE_H, (devices[device].deviceStatus.savedMeasrate & 0xff00)>>8);
	retval += Si117xParamSet(devices[device].deviceHandle,PARAM_TASK_ENABLE,devices[device].deviceStatus.savedTasks);
	retval += Si117xWriteToRegister (devices[device].deviceHandle, REG_IRQ_ENABLE, 0);
	devices[device].deviceStatus.running = false;
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Stop running measurements in the Si117x
 *
 * @param[in] device select ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_Stop (SI117XDRV_DeviceSelect_t device)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running == false)
	{
			return ECODE_SI117XDRV_NOT_RUNNING;
	}
	Si117xStop(devices[device].deviceHandle);

	Si117xWriteToRegister (devices[device].deviceHandle, REG_IRQ_ENABLE,0);
	devices[device].deviceStatus.running = false;
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Start running measurements with the current configuration
 *
 * @param[in] device select ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_Start (SI117XDRV_DeviceSelect_t device)
{
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
	if (devices[device].deviceStatus.running)
	{
		return ECODE_SI117XDRV_RUNNING;
	}
	preStartCallbacks(device);
	SI117XDRV_InitializeSi117xFifoStateMachine(device);
	Si117xWriteToRegister (devices[device].deviceHandle, REG_IRQ_ENABLE, devices[device].deviceCfg.globalCfg.irq_enable);
	SI117XDRV_FifoFlush(device);
	Si117xStart(devices[device].deviceHandle);
	devices[device].deviceStatus.running = true;
	postStartCallbacks(device);
	return ECODE_SI117XDRV_OK;
}


static bool findMeasRate (uint16_t freqDivider, uint16_t *measrate, uint8_t *meascount, uint32_t accelSyncus)
{
    //maximize measrate
	*meascount = 1;
	*measrate = freqDivider;
	return true;

}

static uint16_t gcd2 (uint16_t n1, uint16_t n2)
{
	uint16_t i, gcd=0;

	    for(i=3; i <= n1 && i <= n2; ++i)
	    {
	        // Checks if i is factor of both integers
	        if(n1%i==0 && n2%i==0)
	            gcd = i;
	    }

	    return gcd;
}

static uint16_t gcd3 (uint16_t n1, uint16_t n2, uint16_t n3)
{
	uint16_t i, gcd=0;

	    for(i=3; i <= n1 && i <= n2 && i <= n3; ++i)
	    {
	        // Checks if i is factor of all integers
	        if(n1%i==0 && n2%i==0 && n3%i==0)
	            gcd = i;
	    }

	    return gcd;
}

static bool findTwoMeasCount (uint16_t freqDivider1, uint16_t freqDivider2, uint16_t *measrate, uint8_t *meascount1, uint8_t *meascount2, uint32_t accelSyncus)
{
	int j,k;
	bool foundVal=false;
    //maximize measrate
	if (freqDivider1 == freqDivider2)
	{
		*meascount1 = 1;
		*meascount2 = 1;
		*measrate = freqDivider1;
		return true;
	}
	else
	{
		*measrate = gcd2(freqDivider1, freqDivider2);
	}

		for (j=0;j<MEASCOUNT_MAX;j++)
		{
          for (k=0;k<MEASCOUNT_MAX;k++)
          {
			if ((((*measrate)*j) == freqDivider1) && (((*measrate)*k) == freqDivider2))
            {
        	  foundVal = true;
        	  *meascount1 = j;
        	  *meascount2 = k;
            }
          }
		}

	return foundVal;
}

static bool findThreeMeasCount (uint16_t freqDivider1, uint16_t freqDivider2, uint16_t freqDivider3, uint16_t *measrate, uint8_t *meascount1, uint8_t *meascount2, uint8_t *meascount3, uint32_t accelSyncus)
{
	//this is certainly not efficient...to be rewritten
	int j,k,m;
	bool foundVal=false;
    //maximize measrate
	if ((freqDivider1 == freqDivider2) ==  freqDivider3)
		{
			*meascount1 = 1;
			*meascount2 = 1;
			*meascount3 = 1;
			*measrate = freqDivider1;
			return true;
		}
		else
		{
			*measrate = gcd3(freqDivider1, freqDivider2,freqDivider3);
		}
		for (j=0;j<MEASCOUNT_MAX;j++)
		{
          for (k=0;k<MEASCOUNT_MAX;k++)
          {
        	for (m=0;m<MEASCOUNT_MAX;m++)
        	{
			  if ((((*measrate)*j) == freqDivider1) && (((*measrate)*k) == freqDivider2)&& (((*measrate)*m) == freqDivider3))
              {
        	    foundVal = true;
        	    *meascount1 = j;
        	    *meascount2 = k;
        	    *meascount3 = m;
              }
        	}
          }
		}

	return foundVal;
}

/***************************************************************************//**
 * @brief
 *    Configure global Si117x settings
 *
 * @param[in] device select ID
 *
 * @param[in] pointer to global configuration structure
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_InitGlobal (SI117XDRV_DeviceSelect_t device, SI117XDRV_GlobalCfg_t *globalCfg)
{
	uint16_t measrate;
	uint16_t freqDivider;
	uint16_t freqDivider2;
	uint16_t freqDivider3;
	uint8_t ppg_meascount=0, ecg_meascount=0, bioz_meascount=0, ecgld_meascount=0;
	//todo: handle accelSyncRateus in calculations
	if (initFlag[device] != 1)
	{
		return ECODE_SI117XDRV_UNINITIALIZED;
	}
    if (devices[device].deviceStatus.running)
    {
    	return ECODE_SI117XDRV_RUNNING;
    }
	devices[device].deviceCfg.globalCfg.synch_config = globalCfg->synch_mode | (globalCfg->ms_polar<<4);
	Si117xParamSet(devices[device].deviceHandle,PARAM_SYNCH_CONFIG,devices[device].deviceCfg.globalCfg.synch_config);
	devices[device].deviceCfg.globalCfg.meas_cntl = ((globalCfg->ppg_sw_avg&7)<<3) | (globalCfg->fifo_disable<<6) | (globalCfg->fifo_self_test<<7) | (globalCfg->ppgSampleSize) | (globalCfg->ecgSampleSize<<2) | (globalCfg->biozSampleSize<<1);
	Si117xParamSet(devices[device].deviceHandle,PARAM_MEAS_CNTL,devices[device].deviceCfg.globalCfg.meas_cntl);
	devices[device].deviceCfg.globalCfg.taskEnable = globalCfg->taskEnable;
	Si117xParamSet(devices[device].deviceHandle,PARAM_TASK_ENABLE,devices[device].deviceCfg.globalCfg.taskEnable);
	devices[device].deviceCfg.globalCfg.irq_enable = globalCfg->irq_enable;
	//find measrate, meascounts
    if (globalCfg->taskEnable & (SI117XDRV_TASK_BIOZ1_AC_EN|SI117XDRV_TASK_BIOZ_DC_EN))
    {
    	//make sure no other task enabled
    	if (globalCfg->taskEnable & ~(SI117XDRV_TASK_BIOZ1_AC_EN|SI117XDRV_TASK_BIOZ_DC_EN))
    	{
    		return ECODE_SI117XDRV_PARAM_ERROR;
    	}
    	//otherwise calculate bioz_meascount
        freqDivider = globalCfg->biozSampleRateus/50;
        if (findMeasRate(freqDivider,&measrate,&bioz_meascount, globalCfg->accelSyncRateus)!= true)
        {
        	return ECODE_SI117XDRV_PARAM_ERROR;
        }
        devices[device].deviceCfg.globalCfg.measrate = measrate;
        Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_H,measrate>>8);
        Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_L,measrate&0xff);
        devices[device].deviceCfg.biozCfg.bioz_meascount = bioz_meascount;
        Si117xParamSet(devices[device].deviceHandle,PARAM_BIOZ_MEASCOUNT,bioz_meascount);
    }
    if (globalCfg->taskEnable & (SI117XDRV_TASK_PPG1_EN|SI117XDRV_TASK_PPG2_EN|SI117XDRV_TASK_PPG3_EN|SI117XDRV_TASK_PPG4_EN))
    {
    	devices[device].deviceStatus.hrmOversamplingRatio = 40000 / globalCfg->ppgSampleRateus;
    	if (globalCfg->taskEnable & 0xC0)
    	{
    	    //ECG and PPG tasks at same time
    		        freqDivider3 = globalCfg->ppgSampleRateus/50;
    		    	if ((globalCfg->taskEnable & SI117XDRV_TASK_ECG_EN) && (globalCfg->taskEnable & SI117XDRV_TASK_ECG_LD_EN))
    		    	{
    		    		freqDivider  = globalCfg->ecgSampleRateus/50;
    		    		freqDivider2 = (globalCfg->ecgldSampleRateus/4)/50; //ecg_ld has 4x divider
    		    		if (findThreeMeasCount (freqDivider, freqDivider2, freqDivider3, &measrate, &ecg_meascount, &ecgld_meascount,&ppg_meascount, globalCfg->accelSyncRateus)!= true)
    		    		{
    		    			 return ECODE_SI117XDRV_PARAM_ERROR;
    		    		}
    		    		devices[device].deviceCfg.globalCfg.measrate = measrate;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_H,measrate>>8);
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_L,measrate&0xff);
    		    		devices[device].deviceCfg.ecgCfg.ecg_meascount = ecg_meascount;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_MEASCOUNT,ecg_meascount);
    		    		devices[device].deviceCfg.ecgCfg.ecg_ld_meascount = ecgld_meascount;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_LD_MEASCOUNT,ecgld_meascount);
    		    		devices[device].deviceCfg.ppgCfg.ppg_meascount = ppg_meascount;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_PPG_MEASCOUNT,ppg_meascount);
    		    	}
    		    	else if (globalCfg->taskEnable & SI117XDRV_TASK_ECG_EN)
    		    	{
    		    		freqDivider  = globalCfg->ecgSampleRateus/50;
    		    		if (findTwoMeasCount(freqDivider,freqDivider3,&measrate,&ecg_meascount,&ppg_meascount, globalCfg->accelSyncRateus)!= true)
    		    		{
    		    		     return ECODE_SI117XDRV_PARAM_ERROR;
    		    		}
    		    		devices[device].deviceCfg.globalCfg.measrate = measrate;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_H,measrate>>8);
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_L,measrate&0xff);
    		    		devices[device].deviceCfg.ecgCfg.ecg_meascount = ecg_meascount;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_MEASCOUNT,ecg_meascount);
    		    		devices[device].deviceCfg.ppgCfg.ppg_meascount = ppg_meascount;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_PPG_MEASCOUNT,ppg_meascount);
    		    	}
    		    	else if (globalCfg->taskEnable & SI117XDRV_TASK_ECG_LD_EN)
    		    	{
    		    		freqDivider = (globalCfg->ecgldSampleRateus/4)/50; //ecg_ld has 4x divider
    		    		if (findTwoMeasCount(freqDivider,freqDivider3,&measrate,&ecgld_meascount,&ppg_meascount, globalCfg->accelSyncRateus)!= true)
    		    		{
    		    		     return ECODE_SI117XDRV_PARAM_ERROR;
    		    		}
    		    		devices[device].deviceCfg.globalCfg.measrate = measrate;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_H,measrate>>8);
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_L,measrate&0xff);
    		    		devices[device].deviceCfg.ecgCfg.ecg_ld_meascount = ecgld_meascount;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_LD_MEASCOUNT,ecgld_meascount);
    		    		devices[device].deviceCfg.ppgCfg.ppg_meascount = ppg_meascount;
    		    		Si117xParamSet(devices[device].deviceHandle,PARAM_PPG_MEASCOUNT,ppg_meascount);
    		    	}
    	}
    	else
    	{
             //just PPG
    		freqDivider = globalCfg->ppgSampleRateus/50;
    		if (findMeasRate(freqDivider,&measrate,&ppg_meascount, globalCfg->accelSyncRateus)!= true)
    		{
    		    return ECODE_SI117XDRV_PARAM_ERROR;
    		}
    		devices[device].deviceCfg.globalCfg.measrate = measrate;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_H,measrate>>8);
    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_L,measrate&0xff);
    		devices[device].deviceCfg.ppgCfg.ppg_meascount = ppg_meascount;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_PPG_MEASCOUNT,ppg_meascount);
    	}
    }
    else if (globalCfg->taskEnable & 0xc0)
    {
    	//ECG tasks on their own
    	if ((globalCfg->taskEnable & SI117XDRV_TASK_ECG_EN) && (globalCfg->taskEnable & SI117XDRV_TASK_ECG_LD_EN))
    	{
    		freqDivider  = globalCfg->ecgSampleRateus/50;
    		freqDivider2 = (globalCfg->ecgldSampleRateus/4)/50; //ecg_ld has 4x divider
    		if (findTwoMeasCount (freqDivider, freqDivider2, &measrate, &ecg_meascount, &ecgld_meascount, globalCfg->accelSyncRateus)!= true)
    		{
    			 return ECODE_SI117XDRV_PARAM_ERROR;
    		}
    		devices[device].deviceCfg.globalCfg.measrate = measrate;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_H,measrate>>8);
    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_L,measrate&0xff);
    		devices[device].deviceCfg.ecgCfg.ecg_meascount = ecg_meascount;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_MEASCOUNT,ecg_meascount);
    		devices[device].deviceCfg.ecgCfg.ecg_ld_meascount = ecgld_meascount;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_LD_MEASCOUNT,ecgld_meascount);
    	}
    	else if (globalCfg->taskEnable & SI117XDRV_TASK_ECG_EN)
    	{
    		freqDivider  = globalCfg->ecgSampleRateus/50;
    		if (findMeasRate(freqDivider,&measrate,&ecg_meascount, globalCfg->accelSyncRateus)!= true)
    		{
    		     return ECODE_SI117XDRV_PARAM_ERROR;
    		}
    		devices[device].deviceCfg.globalCfg.measrate = measrate;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_H,measrate>>8);
    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_L,measrate&0xff);
    		devices[device].deviceCfg.ecgCfg.ecg_meascount = ecg_meascount;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_MEASCOUNT,ecg_meascount);
    	}
    	else if (globalCfg->taskEnable & SI117XDRV_TASK_ECG_LD_EN)
    	{
    		freqDivider = (globalCfg->ecgldSampleRateus/4)/50; //ecg_ld has 4x divider
    		if (findMeasRate(freqDivider,&measrate,&ecgld_meascount, globalCfg->accelSyncRateus)!= true)
    		{
    		     return ECODE_SI117XDRV_PARAM_ERROR;
    		}
    		devices[device].deviceCfg.globalCfg.measrate = measrate;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_H,measrate>>8);
    		Si117xParamSet(devices[device].deviceHandle,PARAM_MEASRATE_L,measrate&0xff);
    		devices[device].deviceCfg.ecgCfg.ecg_ld_meascount = ecgld_meascount;
    		Si117xParamSet(devices[device].deviceHandle,PARAM_ECG_LD_MEASCOUNT,ecgld_meascount);
    	}
    }
    //lastly, set fifo_int_level
    devices[device].deviceCfg.globalCfg.fifo_int_level = globalCfg->fifo_int_level;
    Si117xParamSet(devices[device].deviceHandle,PARAM_FIFO_INT_LEVEL_H,globalCfg->fifo_int_level>>8);
    Si117xParamSet(devices[device].deviceHandle,PARAM_FIFO_INT_LEVEL_L,globalCfg->fifo_int_level&0xff);
    return ECODE_SI117XDRV_OK;

}

/***************************************************************************//**
 * @brief
 *   Initializes API memory and device handle
 *
 * @param[in] device select ID
 *
 * @param[in] device handle to pass to low level functions
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_InitAPI (SI117XDRV_DeviceSelect_t device, HANDLE deviceHandle)
{
	int i;
	devices[device].deviceHandle = deviceHandle;
	for (i=0;i<SI117XDRV_MAX_NUM_CALLBACKS;i++)
	{
		callbacks[device].preStartCallbacks[ i ].allocated = false;
		callbacks[device].postStartCallbacks[ i ].allocated = false;
		callbacks[device].preFifoFlushCallbacks[ i ].allocated = false;
		callbacks[device].postFifoFlushCallbacks[ i ].allocated = false;
	}
	leadDetect[device] = false;
	leadDetectionFlag[device]=false;
	wristDetectionFlag[device]=false;
	initFlag[device] = 1;
	return ECODE_SI117XDRV_OK;

}

/***************************************************************************//**
 * @brief
 *    Register an accelerometer sync message callback
 *
 * @param[in] device select ID
 *
 * @param[in] callback function pointer
 *
 * @param[in] user parameter to pass to callback function
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_RegisterAccelSyncCallback (SI117XDRV_DeviceSelect_t device, SI117XDRV_AccelCallback_t callback, void *user)
{
	   int i      = 0;

			  // Iterate through the table of the queues until the first available.
			  while ( ( i < SI117XDRV_MAX_NUM_CALLBACKS ) && ( callbacks[device].accelSyncRecvCallbacks[ i ].allocated ) ) {
			    i++;
			  }

			  // Check if we reached the end of the table.
			  if ( i == SI117XDRV_MAX_NUM_CALLBACKS ) {
			    return ECODE_SI117XDRV_ALL_CB_USED;
			  } else {
				  callbacks[device].accelSyncRecvCallbacks[ i ].allocated = true;
				  callbacks[device].accelSyncRecvCallbacks[ i ].callback = callback;
				  callbacks[device].accelSyncRecvCallbacks[ i ].user = user;
			  }

		return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Register a pre start callback
 *
 * @param[in] device select ID
 *
 * @param[in] callback function pointer
 *
 * @param[in] user parameter to pass to callback function
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_RegisterPreStartCallback (SI117XDRV_DeviceSelect_t device, SI117XDRV_Callback_t callback, void *user)
{
	      int i      = 0;

		  // Iterate through the table of the queues until the first available.
		  while ( ( i < SI117XDRV_MAX_NUM_CALLBACKS ) && ( callbacks[device].preStartCallbacks[ i ].allocated ) ) {
		    i++;
		  }

		  // Check if we reached the end of the table.
		  if ( i == SI117XDRV_MAX_NUM_CALLBACKS ) {
		    return ECODE_SI117XDRV_ALL_CB_USED;
		  } else {
			  callbacks[device].preStartCallbacks[ i ].allocated = true;
			  callbacks[device].preStartCallbacks[ i ].callback = callback;
			  callbacks[device].preStartCallbacks[ i ].user = user;
		  }

	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Register a post start callback
 *
 * @param[in] device select ID
 *
 * @param[in] callback function pointer
 *
 * @param[in] user parameter to pass to callback function
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_RegisterPostStartCallback (SI117XDRV_DeviceSelect_t device, SI117XDRV_Callback_t callback, void *user)
{
	int i      = 0;

			  // Iterate through the table of the queues until the first available.
			  while ( ( i < SI117XDRV_MAX_NUM_CALLBACKS ) && ( callbacks[device].postStartCallbacks[ i ].allocated ) ) {
			    i++;
			  }

			  // Check if we reached the end of the table.
			  if ( i == SI117XDRV_MAX_NUM_CALLBACKS ) {
			    return ECODE_SI117XDRV_ALL_CB_USED;
			  } else {
				  callbacks[device].postStartCallbacks[ i ].allocated = true;
				  callbacks[device].postStartCallbacks[ i ].callback = callback;
				  callbacks[device].postStartCallbacks[ i ].user = user;
			  }

	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Register a pre fifo flush callback
 *
 * @param[in] device select ID
 *
 * @param[in] callback function pointer
 *
 * @param[in] user parameter to pass to callback function
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_RegisterPreFifoFlushCallback (SI117XDRV_DeviceSelect_t device, SI117XDRV_Callback_t callback, void *user)
{
	int i      = 0;

				  // Iterate through the table of the queues until the first available.
				  while ( ( i < SI117XDRV_MAX_NUM_CALLBACKS ) && ( callbacks[device].preFifoFlushCallbacks[ i ].allocated ) ) {
				    i++;
				  }

				  // Check if we reached the end of the table.
				  if ( i == SI117XDRV_MAX_NUM_CALLBACKS ) {
				    return ECODE_SI117XDRV_ALL_CB_USED;
				  } else {
					  callbacks[device].preFifoFlushCallbacks[ i ].allocated = true;
					  callbacks[device].preFifoFlushCallbacks[ i ].callback = callback;
					  callbacks[device].preFifoFlushCallbacks[ i ].user = user;
				  }
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Register a post fifo flush callback
 *
 * @param[in] device select ID
 *
 * @param[in] callback function pointer
 *
 * @param[in] user parameter to pass to callback function
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_RegisterPostFifoFlushCallback (SI117XDRV_DeviceSelect_t device, SI117XDRV_Callback_t callback, void *user)
{
	int i      = 0;

				  // Iterate through the table of the queues until the first available.
				  while ( ( i < SI117XDRV_MAX_NUM_CALLBACKS ) && ( callbacks[device].postFifoFlushCallbacks[ i ].allocated ) ) {
				    i++;
				  }

				  // Check if we reached the end of the table.
				  if ( i == SI117XDRV_MAX_NUM_CALLBACKS ) {
				    return ECODE_SI117XDRV_ALL_CB_USED;
				  } else {
					  callbacks[device].postFifoFlushCallbacks[ i ].allocated = true;
					  callbacks[device].postFifoFlushCallbacks[ i ].callback = callback;
					  callbacks[device].postFifoFlushCallbacks[ i ].user = user;
				  }
	return ECODE_SI117XDRV_OK;
}
/***************************************************************************//**
 * Data Fifo Queue Functionality
 ******************************************************************************/

/***************************************************************************//**
 * Data Fifo Queue typedefs
 ******************************************************************************/
/// Raw data FIFO queue typedef
typedef struct {
  volatile uint16_t head;                  ///< Index of next byte to get.
  volatile uint16_t tail;                  ///< Index of where to enqueue next byte.
  volatile uint16_t used;                  ///< Number of bytes queued.
  uint16_t size;                     ///< Size of FIFO.
  int8_t *fifo;                            ///< Pointer to FIFO of queue data (allocated by user)
} SI117XDRV_FifoQueue_t;

/// Raw data FIFO queue typedef
typedef struct {
  SI117XDRV_FifoQueue_t queue;
  bool allocated;
  bool ppg;
  bool ppg16;
  bool ecg;
  bool ecgld;
  uint8_t sampleSize;
} SI117XDRV_FifoQueueConfig_t;

static SI117XDRV_FifoQueueConfig_t            queues[ SI117XDRV_MAX_DATA_QUEUES ];

/***************************************************************************//**
 * Static functions
 ******************************************************************************/
static Si117x_Ecode_t SI117XDRV_EnqueueRawData(SI117XDRV_FifoQueue_t *queue,
                             int8_t *inputBuffer,
                             uint16_t size);
static Si117x_Ecode_t SI117XDRV_DequeueRawData(SI117XDRV_FifoQueue_t *queue,
                             int8_t *buffer, uint16_t size);
static Si117x_Ecode_t SI117XDRV_ClearRawQueue (SI117XDRV_FifoQueue_t *queue);
static uint16_t       SI117XDRV_NumBytesInQueue (SI117XDRV_FifoQueue_t *queue);

/***************************************************************************//**
 * @brief
 *    Allocate a fifo queue for 16 bit PPG data
 *
 * @param[in] pointer to queue ID
 *
 * @param[in] pointer to buffer to use for fifo queue
 *
 * @param[in] queue buffer size in bytes
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_AllocatePPGDataQueue(SI117XDRV_DataQueueID_t *id, SI117XDRV_PPGSample_t *queueBuffer, int16_t queueSizeInBytes)
{
	int i      = 0;
	int retVal = 0;
	// Iterate through the table of the queues until the first available.
	  while ( ( i < SI117XDRV_MAX_DATA_QUEUES ) && ( queues[ i ].allocated ) ) {
	    i++;
	  }

	  // Check if we reached the end of the table.
	  if ( i == SI117XDRV_MAX_DATA_QUEUES ) {
	    retVal = ECODE_SI117XDRV_ALL_QUEUES_USED;
	  } else {
	    // Check if a NULL pointer was passed.
	    if ( id != NULL ) {
	      queues[ i ].allocated = true;
	      queues[ i ].ppg = false;
	      queues[ i ].ppg16 = true;
	      queues[ i ].ecg = false;
	      queues[ i ].ecgld = false;
	      queues[ i ].queue.head = 0;
	      queues[ i ].queue.tail = 0;
	      queues[ i ].queue.used = 0;
	      queues[ i ].queue.size = queueSizeInBytes;
	      queues[ i ].queue.fifo = (int8_t*)queueBuffer;
	      queues[ i ].sampleSize  = SI117XDRV_PPG_SAMPLE_SIZE_BYTES;
	      *id = i;
	      retVal = ECODE_SI117XDRV_OK;
	      if ((queueSizeInBytes % SI117XDRV_PPG_SAMPLE_SIZE_BYTES) != 0)
	      {
	      	    retVal = ECODE_SI117XDRV_QUEUE_SIZE_ERROR;
	      }
	    } else {
	      retVal = ECODE_SI117XDRV_PARAM_ERROR;
	    }
	  }
	  return retVal;
}
/***************************************************************************//**
 * @brief
 *    Allocate a fifo queue for PPG data
 *
 * @param[in] pointer to queue ID
 *
 * @param[in] pointer to buffer to use for fifo queue
 *
 * @param[in] queue buffer size in bytes
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_Allocate24bPPGDataQueue(SI117XDRV_DataQueueID_t *id, SI117XDRV_PPG24bSample_t *queueBuffer,  int16_t queueSizeInBytes)
{
	int i      = 0;
	int retVal = 0;
	// Iterate through the table of the queues until the first available.
	  while ( ( i < SI117XDRV_MAX_DATA_QUEUES ) && ( queues[ i ].allocated ) ) {
	    i++;
	  }

	  // Check if we reached the end of the table.
	  if ( i == SI117XDRV_MAX_DATA_QUEUES ) {
	    retVal = ECODE_SI117XDRV_ALL_QUEUES_USED;
	  } else {
	    // Check if a NULL pointer was passed.
	    if ( id != NULL ) {
	      queues[ i ].allocated = true;
	      queues[ i ].ppg = true;
	      queues[ i ].ppg16 = false;
	      queues[ i ].ecg = false;
	      queues[ i ].ecgld = false;
	      queues[ i ].queue.head = 0;
	      queues[ i ].queue.tail = 0;
	      queues[ i ].queue.used = 0;
	      queues[ i ].queue.size = queueSizeInBytes;
	      queues[ i ].queue.fifo = (int8_t*)queueBuffer;
	      queues[ i ].sampleSize  = SI117XDRV_PPG_24BIT_SAMPLE_SIZE_BYTES;
	      *id = i;
	      retVal = ECODE_SI117XDRV_OK;
	      if ((queueSizeInBytes % SI117XDRV_PPG_24BIT_SAMPLE_SIZE_BYTES) != 0)
	      {
	      	    retVal = ECODE_SI117XDRV_QUEUE_SIZE_ERROR;
	      }
	    } else {
	      retVal = ECODE_SI117XDRV_PARAM_ERROR;
	    }
	  }
	  return retVal;
}

/***************************************************************************//**
 * @brief
 *    Allocate a fifo queue for ECG data
 *
 * @param[in] pointer to queue ID
 *
 * @param[in] pointer to buffer to use for fifo queue
 *
 * @param[in] queue buffer size in bytes
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_AllocateECGDataQueue(SI117XDRV_DataQueueID_t *id, SI117XDRV_ECGSample_t *queueBuffer,  int16_t queueSizeInBytes)
{
	int i      = 0;
	int retVal = 0;
	// Iterate through the table of the queues until the first available.
	  while ( ( i < SI117XDRV_MAX_DATA_QUEUES ) && ( queues[ i ].allocated ) ) {
	    i++;
	  }

	  // Check if we reached the end of the table.
	  if ( i == SI117XDRV_MAX_DATA_QUEUES ) {
	    retVal = ECODE_SI117XDRV_ALL_QUEUES_USED;
	  } else {
	    // Check if a NULL pointer was passed.
	    if ( id != NULL ) {
	      queues[ i ].allocated = true;
	      queues[ i ].ppg = false;
	      queues[ i ].ecg = true;
	      queues[ i ].ppg16 = false;
	      queues[ i ].ecgld = false;
	      queues[ i ].queue.head = 0;
	      queues[ i ].queue.tail = 0;
	      queues[ i ].queue.used = 0;
	      queues[ i ].queue.size = queueSizeInBytes;
	      queues[ i ].queue.fifo = (int8_t*)queueBuffer;
	      queues[ i ].sampleSize  = SI117XDRV_ECG_SAMPLE_SIZE_BYTES;
	      *id = i;
	      retVal = ECODE_SI117XDRV_OK;
	      if ((queueSizeInBytes % SI117XDRV_ECG_SAMPLE_SIZE_BYTES) != 0)
	      {
	      	    retVal = ECODE_SI117XDRV_QUEUE_SIZE_ERROR;
	      }
	    } else {
	      retVal = ECODE_SI117XDRV_PARAM_ERROR;
	    }
	  }
	  return retVal;
}

/***************************************************************************//**
 * @brief
 *    Allocate a fifo queue for ECG data
 *
 * @param[in] pointer to queue ID
 *
 * @param[in] pointer to buffer to use for fifo queue
 *
 * @param[in] queue buffer size in bytes
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_AllocateECGLDDataQueue(SI117XDRV_DataQueueID_t *id, SI117XDRV_ECGLDSample_t *queueBuffer,  int16_t queueSizeInBytes)
{
	int i      = 0;
	int retVal = 0;
	// Iterate through the table of the queues until the first available.
	  while ( ( i < SI117XDRV_MAX_DATA_QUEUES ) && ( queues[ i ].allocated ) ) {
	    i++;
	  }

	  // Check if we reached the end of the table.
	  if ( i == SI117XDRV_MAX_DATA_QUEUES ) {
	    retVal = ECODE_SI117XDRV_ALL_QUEUES_USED;
	  } else {
	    // Check if a NULL pointer was passed.
	    if ( id != NULL ) {
	      queues[ i ].allocated = true;
	      queues[ i ].ppg = false;
	      queues[ i ].ecg = false;
	      queues[ i ].ecgld = true;
	      queues[ i ].ppg16 = false;
	      queues[ i ].queue.head = 0;
	      queues[ i ].queue.tail = 0;
	      queues[ i ].queue.used = 0;
	      queues[ i ].queue.size = queueSizeInBytes;
	      queues[ i ].queue.fifo = (int8_t*)queueBuffer;
	      queues[ i ].sampleSize  = SI117XDRV_ECG_LD_SAMPLE_SIZE_BYTES;
	      *id = i;
	      retVal = ECODE_SI117XDRV_OK;
	      if ((queueSizeInBytes % SI117XDRV_ECG_LD_SAMPLE_SIZE_BYTES) != 0)
	      {
	      	    retVal = ECODE_SI117XDRV_QUEUE_SIZE_ERROR;
	      }
	    } else {
	      retVal = ECODE_SI117XDRV_PARAM_ERROR;
	    }
	  }
	  return retVal;
}

/***************************************************************************//**
 * @brief
 *    De-allocate a fifo queue
 *
 * @param[in] queue ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_FreeDataQueue(SI117XDRV_DataQueueID_t id)
{
	// Check if valid callback ID.
	  if ( id >= SI117XDRV_MAX_DATA_QUEUES ) {
	    return ECODE_SI117XDRV_PARAM_ERROR;
	  }

	  //CORE_ATOMIC_SECTION(
	  queues[ id ].allocated = false;
	//  )

	  return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief Empty queue
 ******************************************************************************/
static Si117x_Ecode_t SI117XDRV_ClearRawQueue (SI117XDRV_FifoQueue_t *queue)
{
	queue->head = 0;
	queue->tail = 0;
	queue->used = 0;
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    Empty a fifo queue
 *
 * @param[in] queue ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_ClearQueue (SI117XDRV_DataQueueID_t id)
{
	return SI117XDRV_ClearRawQueue(&(queues[id].queue));
}



/***************************************************************************//**
 * @brief Enqueue Si117x raw data
 ******************************************************************************/
static Si117x_Ecode_t SI117XDRV_EnqueueRawData(SI117XDRV_FifoQueue_t *queue,            
                             int8_t *inputBuffer,
                             uint16_t size)
{
  int i;
  for(i=0; i<size; i++)
  {
	  queue->fifo[queue->head] = inputBuffer[i];
	  queue->head++;
      if(queue->head == queue->size)
    	  queue->head = 0;
  }
  queue->used += size;
  i = queue->tail;
  if (i == queue->head)
  {
      queue->tail += size;     // if we have wrapped around then we must delete one sample
      queue->used -= size;
      if(queue->tail >= queue->size)            //handle wrapping
    	  queue->tail = queue->tail - queue->size;
      return ECODE_SI117XDRV_QUEUE_FULL; //indicate to caller something bad happened
  }
  return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief Dequeue si117x raw data
 ******************************************************************************/
static Si117x_Ecode_t SI117XDRV_DequeueRawData(SI117XDRV_FifoQueue_t *queue,
                             int8_t *buffer, uint16_t size)
{
  int i;

  if (queue->used < size) {
    buffer = NULL;

    return ECODE_SI117XDRV_QUEUE_EMPTY;
  }
  for(i=0; i<size; i++)
  {
        buffer[i] = queue->fifo[queue->tail];
        queue->tail++;
        if(queue->tail == queue->size)
        	queue->tail = 0;

  }
  queue->used -= size;

  return ECODE_SI117XDRV_OK;
}
/***************************************************************************//**
 * @brief Get number of bytes in queue
 ******************************************************************************/
static uint16_t SI117XDRV_NumBytesInQueue (SI117XDRV_FifoQueue_t *queue)
{
	return queue->used;
}

/***************************************************************************//**
 * @brief
 *    Get number of samples in the queue
 *
 * @param[in] queue ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
uint16_t SI117XDRV_NumSamplesInQueue (SI117XDRV_DataQueueID_t id)
{
	uint16_t samples;
	if (id > SI117XDRV_MAX_DATA_QUEUES)
		return 0;
	samples = SI117XDRV_NumBytesInQueue(&(queues[id].queue))/(queues[id].sampleSize);
	return samples;
}

/***************************************************************************//**
 * @brief Enqueue Si117x 16 bit PPG sample data
 ******************************************************************************/
static Si117x_Ecode_t SI117XDRV_EnqueuePPG16bSampleData(SI117XDRV_PPGSample_t *sample)
{
    int8_t buffer[SI117XDRV_PPG_SAMPLE_SIZE_BYTES];
    int8_t *buf = buffer;
    int retVal = ECODE_SI117XDRV_OK;
    int i;
    int temp;
    for (i=0; i< SI117XDRV_MAX_DATA_QUEUES; i++)
    {
    	if (queues[ i ].allocated && queues[ i ].ppg16)
    	{
    		buf = buffer;
    		*(uint16_t*)(buf) = sample->sequence;
		    buf+=2;
    		*(uint16_t*)(buf) = sample->timestamp;
    		buf+=2;
    		*(uint8_t*)(buf) = sample->syncMessage;
    		buf+=1;
    		*(int16_t*)(buf) = sample->ppg1; //make sure sign is preserved??
    		buf+=2;
    		*(int16_t*)(buf) = sample->ppg2;
    		buf+=2;
    		*(int16_t*)(buf) = sample->ppg3;
    		buf+=2;
    		*(int16_t*)(buf) = sample->ppg4;
    		buf+=2;
    		temp = SI117XDRV_EnqueueRawData (&(queues[i].queue), buffer, queues[i].sampleSize);
    		if (temp != ECODE_SI117XDRV_OK)
    			retVal = temp;
    	}

    }
	return retVal;
}

/***************************************************************************//**
 * @brief Enqueue Si117x PPG sample data
 ******************************************************************************/
static Si117x_Ecode_t SI117XDRV_EnqueuePPG24bSampleData(SI117XDRV_PPG24bSample_t *sample)
{
    int8_t buffer[SI117XDRV_PPG_24BIT_SAMPLE_SIZE_BYTES];
    int8_t *buf = buffer;
    int retVal = ECODE_SI117XDRV_OK;
    int i;
    int temp;
    for (i=0; i< SI117XDRV_MAX_DATA_QUEUES; i++)
    {
    	if (queues[ i ].allocated && queues[ i ].ppg)
    	{
    		buf = buffer;
    		*(uint16_t*)(buf) = sample->sequence;
		    buf+=2;
    		*(uint16_t*)(buf) = sample->timestamp;
    		buf+=2;
    		*(uint8_t*)(buf) = sample->syncMessage;
    		buf+=1;
    		*(int32_t*)(buf) = sample->ppg1;
    		buf+=4;
    		*(int32_t*)(buf) = sample->ppg2;
    		buf+=4;
    		*(int32_t*)(buf) = sample->ppg3;
    		buf+=4;
    		*(int32_t*)(buf) = sample->ppg4;
    		buf+=4;
    		temp = SI117XDRV_EnqueueRawData (&(queues[i].queue), buffer, queues[i].sampleSize);
    		if (temp != ECODE_SI117XDRV_OK)
    			retVal = temp;
    	}

    }

	return retVal;
}



/***************************************************************************//**
 * @brief
 *    Remove a PPG sample from the queue
 *
 * @param[in] queue ID
 *
 * @param[in] pointer to sample
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_Dequeue24bPPGSampleData (SI117XDRV_DataQueueID_t id, SI117XDRV_PPG24bSample_t *sample)
{
    int8_t buffer[SI117XDRV_PPG_24BIT_SAMPLE_SIZE_BYTES];
    int8_t *buf = buffer;
	int retVal = SI117XDRV_DequeueRawData (&(queues[id].queue), buf, queues[id].sampleSize);
	sample->sequence = *(uint16_t*)(buf);
	buf+=2;
	sample->timestamp = *(uint16_t*)(buf);
	buf+=2;
	sample->syncMessage = *(uint8_t*)(buf);
    buf+=1;
    sample->ppg1 = *(int32_t*)(buf);
	buf+=4;
	sample->ppg2 = *(int32_t*)(buf);
	buf+=4;
	sample->ppg3 = *(int32_t*)(buf);
	buf+=4;
	sample->ppg4 = *(int32_t*)(buf);
	buf+=4;

	return retVal;
}

/***************************************************************************//**
 * @brief
 *    Remove a 16bit PPG sample from the queue
 *
 * @param[in] queue ID
 *
 * @param[in] pointer to sample
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_DequeuePPGSampleData (SI117XDRV_DataQueueID_t id, SI117XDRV_PPGSample_t *sample)
{
    int8_t buffer[SI117XDRV_PPG_SAMPLE_SIZE_BYTES];
    int8_t *buf = buffer;
	int retVal = SI117XDRV_DequeueRawData (&(queues[id].queue), buf, queues[id].sampleSize);
	sample->sequence = *(uint16_t*)(buf);
	buf+=2;
	sample->timestamp = *(uint16_t*)(buf);
	buf+=2;
	sample->syncMessage = *(uint8_t*)(buf);
    buf+=1;
    sample->ppg1 = *(int16_t*)(buf);
	buf+=2;
	sample->ppg2 = *(int16_t*)(buf);
	buf+=2;
	sample->ppg3 = *(int16_t*)(buf);
	buf+=2;
	sample->ppg4 = *(int16_t*)(buf);
	buf+=2;

	return retVal;
}

/***************************************************************************//**
 * @brief Enqueue Si117x ECG sample data
 ******************************************************************************/
static Si117x_Ecode_t SI117XDRV_EnqueueECGSampleData(SI117XDRV_ECGSample_t *sample)
{
    int8_t buffer[SI117XDRV_ECG_SAMPLE_SIZE_BYTES];
    int8_t *buf = buffer;
    int retVal = ECODE_SI117XDRV_OK;
    int i;
    int temp;
    for (i=0; i< SI117XDRV_MAX_DATA_QUEUES; i++)
    {
    	if (queues[ i ].allocated && queues[i].ecg)
    	{
    		*(uint16_t*)(buf) = sample->sequence;
			buf+=2;
    		*(uint16_t*)(buf) = sample->timestamp;
    		buf+=2;
    		*(uint8_t*)(buf) = sample->syncMessage;
    		buf+=1;
    		*(int32_t*)(buf) = sample->ecg;
    		buf+=4;
    		temp = SI117XDRV_EnqueueRawData (&(queues[i].queue), buffer, queues[i].sampleSize);
    		if (temp != ECODE_SI117XDRV_OK)
    			retVal = temp;
    	}

    }
	return retVal;
}

/***************************************************************************//**
 * @brief Enqueue Si117x ECG LD sample data
 ******************************************************************************/
static Si117x_Ecode_t SI117XDRV_EnqueueECGLDSampleData(SI117XDRV_ECGLDSample_t *sample)
{
    int8_t buffer[SI117XDRV_ECG_LD_SAMPLE_SIZE_BYTES];
    int8_t *buf = buffer;
    int retVal = ECODE_SI117XDRV_OK;
    int i;
    int temp;
    for (i=0; i< SI117XDRV_MAX_DATA_QUEUES; i++)
    {
    	if (queues[ i ].allocated && queues[i].ecgld)
    	{
    		*(uint16_t*)(buf) = sample->sequence;
			buf+=2;
    		*(uint16_t*)(buf) = sample->timestamp;
    		buf+=2;
    		*(uint8_t*)(buf) = sample->syncMessage;
    		buf+=1;
    		*(uint8_t*)(buf) = sample->data[0];
    		buf+=1;
    		*(uint8_t*)(buf) = sample->data[1];
    		buf+=1;
    		*(uint8_t*)(buf) = sample->data[2];
    		buf+=1;
    		*(uint8_t*)(buf) = sample->data[3];
    		buf+=1;
    		temp = SI117XDRV_EnqueueRawData (&(queues[i].queue), buffer, queues[i].sampleSize);
    		if (temp != ECODE_SI117XDRV_OK)
    			retVal = temp;
    	}

    }
	return retVal;
}

/***************************************************************************//**
 * @brief
 *    Remove an ECG LD sample from the queue
 *
 * @param[in] queue ID
 *
 * @param[in] pointer to ECG LD sample
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_DequeueECGLDSampleData (SI117XDRV_DataQueueID_t id, SI117XDRV_ECGLDSample_t *sample)
{
  int8_t buffer[SI117XDRV_ECG_LD_SAMPLE_SIZE_BYTES];
  int8_t *buf = buffer;
	int retVal = SI117XDRV_DequeueRawData (&(queues[id].queue), buf, queues[id].sampleSize);
	sample->sequence = *(uint16_t*)(buf);
	buf+=2;
	sample->timestamp = *(uint16_t*)(buf);
	buf+=2;
	sample->syncMessage = *(uint8_t*)(buf);
	buf+=1;
	sample->data[0] = *(uint8_t*)(buf);
	buf+=1;
	sample->data[1] = *(uint8_t*)(buf);
	buf+=1;
	sample->data[2] = *(uint8_t*)(buf);
	buf+=1;
	sample->data[3] = *(uint8_t*)(buf);
	buf+=1;
	return retVal;
}
/***************************************************************************//**
 * @brief
 *    Remove an ECG sample from the queue
 *
 * @param[in] queue ID
 *
 * @param[in] pointer to ECG sample
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_DequeueECGSampleData (SI117XDRV_DataQueueID_t id, SI117XDRV_ECGSample_t *sample)
{
  int8_t buffer[SI117XDRV_ECG_SAMPLE_SIZE_BYTES];
  int8_t *buf = buffer;

  int retVal = SI117XDRV_DequeueRawData (&(queues[id].queue), buf, queues[id].sampleSize);
  sample->sequence = *(uint16_t*)(buf);
  buf+=2;
  sample->timestamp = *(uint16_t*)(buf);
  buf+=2;
  sample->syncMessage = *(uint8_t*)(buf);
  buf+=1;
  sample->ecg = *(int32_t*)(buf);
  buf+=4;

  return retVal;
}


/***************************************************************************//**
 * @brief
 *    Lead detection algorithm
 *
 * @param[in] device select ID
 *
 * @param[in] pointer bool to indicate lead detection status
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_LeadDetection (SI117XDRV_DeviceSelect_t device, bool *leadDetectStatus)
{
	int16_t retval;
	uint8_t value;
	retval = Si117xParamRead(devices[device].deviceHandle, PARAM_ECG_LDCONFIG);
	if (leadDetectionFlag[device] && retval >= 0)
	{
		value = (uint8_t)retval;
		if(value & 0x80) // lead on detection
		{
			//lead on
			// Flip direction
			devices[device].deviceCfg.ecgCfg.ecg_ldconfig &= ~0x80;
			Si117xParamSet(devices[device].deviceHandle, PARAM_ECG_LDCONFIG, (value & 0x7F));
			leadDetect[device] = true;
		}else // lead off detection
		{
			// lead off
			// Flip direction
			devices[device].deviceCfg.ecgCfg.ecg_ldconfig |= 0x80;
			Si117xParamSet(devices[device].deviceHandle, PARAM_ECG_LDCONFIG, (value | 0x80));
			leadDetect[device] = false;
		}
		leadDetectionFlag[device] = false;
	}
	*leadDetectStatus = leadDetect[device];
	return ECODE_SI117XDRV_OK;
}


/***************************************************************************//**
 * @brief
 *    wrist detection algorithm
 *
 * @param[in] device select ID
 *
 * @param[in] pointer bool to indicate wrist detection status
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_WristDetection (SI117XDRV_DeviceSelect_t device, bool *wristDetectStatus)
{
	*wristDetectStatus = wristDetectionFlag[device];
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    wrist detection algorithm
 *
 * @param[in] device select ID
 *
 * @param[in] pointer bool to indicate wrist detection status
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_ClearWristDetection (SI117XDRV_DeviceSelect_t device)
{
	wristDetectionFlag[device] = false;
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    lead detection interrupt handler
 *
 * @param[in] device select ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_ProcessLDInterrupt (SI117XDRV_DeviceSelect_t device)
{
	leadDetectionFlag[device] = true;
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    wrist detection interrupt handler
 *
 * @param[in] device select ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_ProcessWDInterrupt (SI117XDRV_DeviceSelect_t device)
{
	wristDetectionFlag[device] = true;
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * Fifo/Interrupt Processing functions
 ******************************************************************************/


typedef enum si117xFifoState
{
  SI117x_FIFO_STATE_PPG1_HI=0,
  SI117x_FIFO_STATE_PPG1_MID=1,
  SI117x_FIFO_STATE_PPG1_LO=2,
  SI117x_FIFO_STATE_PPG2_HI=3,
  SI117x_FIFO_STATE_PPG2_MID=4,
  SI117x_FIFO_STATE_PPG2_LO=5,
  SI117x_FIFO_STATE_PPG3_HI=6,
  SI117x_FIFO_STATE_PPG3_MID=7,
  SI117x_FIFO_STATE_PPG3_LO=8,
  SI117x_FIFO_STATE_PPG4_HI=9,
  SI117x_FIFO_STATE_PPG4_MID=10,
  SI117x_FIFO_STATE_PPG4_LO=11,
  SI117x_FIFO_STATE_BIOZ1_HI = 12,
  SI117x_FIFO_STATE_BIOZ1_MID = 13,
  SI117x_FIFO_STATE_BIOZ1_LO = 14,
  SI117x_FIFO_STATE_BIOZ2_HI = 15,
  SI117x_FIFO_STATE_BIOZ2_MID = 16,
  SI117x_FIFO_STATE_BIOZ2_LO = 17,
  SI117x_FIFO_STATE_ECG_HI=18,
  SI117x_FIFO_STATE_ECG_MID=19,
  SI117x_FIFO_STATE_ECG_LO=20,
  SI117x_FIFO_STATE_ECG_LD_BYTE1=21,
  SI117x_FIFO_STATE_ECG_LD_BYTE2=22,
  SI117x_FIFO_STATE_ECG_LD_BYTE3=23,
  SI117x_FIFO_STATE_ECG_LD_BYTE4=24,
  SI117x_FIFO_STATE_UNKNOWN = 0xff   //a dle has not been received so the state is unknown
}si117xFifoState_t;

typedef struct si117xFifoStateMachine
{
	si117xFifoState_t fifoState;
	uint8_t isMsb;      //Is the current state waiting for the MSB
	int32_t ppg_value[4];
	int32_t ecg_value;
	int32_t bioz_value[2];
	uint8_t ecg_ld_value[4];
}si117xFifoStateMachine_t;

static si117xFifoStateMachine_t fifoStateMachines[SI117XDRV_MAX_NUM_DEVICES];

#define SI117X_FIFO_DEFAULT_FIFO_STATE {SI117x_FIFO_STATE_UNKNOWN,1,0,0,0,0,0,0,0}

/**************************************************************************//**
 * @brief
 *****************************************************************************/
static bool Unprocessed_DLE=false;
static bool Unprocessed_DLE_S=false;

static uint8_t found_DLE_S=0, DLE_S_tag;

typedef enum si117xFifoStateTaskComplete
{
  SI117x_TASK_NOT_COMPLETE,
  SI117x_TASK_COMPLETE_PPG,
  SI117x_TASK_COMPLETE_BIOZ,
  SI117x_TASK_COMPLETE_ECG,
  SI117x_TASK_COMPLETE_ECG_LD
}si117xFifoStateTaskComplete_t;





static int16_t si117xFifoStateMachine_Next(SI117XDRV_DeviceSelect_t device, uint8_t isDle, uint8_t dleDataByte, si117xFifoStateTaskComplete_t *taskComplete);



//dlc:  We now have two different ways to perform accelerometer synchronization.  The newer (and better) method does not use the accel synch markers in the FIFO.  Need to make sure this architecture works with both methods
static uint16_t ppg1_sample_count[SI117XDRV_MAX_NUM_DEVICES];
static uint16_t ppg2_sample_count[SI117XDRV_MAX_NUM_DEVICES];
static uint16_t ppg3_sample_count[SI117XDRV_MAX_NUM_DEVICES];
static uint16_t ppg4_sample_count[SI117XDRV_MAX_NUM_DEVICES];



static uint16_t ppg_count;

static void resetSynchSampleCounters (SI117XDRV_DeviceSelect_t device)
{
	ppg1_sample_count[device]=0;
	ppg2_sample_count[device]=0;
	ppg3_sample_count[device]=0;
	ppg4_sample_count[device]=0;
}

/***************************************************************************//**
 * @brief
 *    Initializes the FIFO processing state machine
 *
 * @param[in] device select ID
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_InitializeSi117xFifoStateMachine(SI117XDRV_DeviceSelect_t device)
{
	si117xFifoStateMachine_t *fifoStateMachine = &(fifoStateMachines[device]);
	fifoStateMachine->fifoState = SI117x_FIFO_STATE_UNKNOWN;
	fifoStateMachine->isMsb = 1;
	fifoStateMachine->ppg_value[0] = 0;
	fifoStateMachine->ppg_value[1] = 0;
	fifoStateMachine->ppg_value[2] = 0;
	fifoStateMachine->ppg_value[3] = 0;
	fifoStateMachine->bioz_value[0] = 0;
	fifoStateMachine->bioz_value[1] = 0;
	fifoStateMachine->ecg_value = 0;
	Unprocessed_DLE = false;
	resetSynchSampleCounters(device);
	return ECODE_SI117XDRV_OK;
}

/***************************************************************************//**
 * @brief
 *    PPG1 interrupt handler
 *
 * @param[in] device select ID
 *
 * @param[in] pointer fifo data read from the sensor
 *
 * @param[in] 16 bit timestamp of interrupt occurance
 *
 * @param[in] irq sequence number
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_ProcessPPGInterrupt (SI117XDRV_DeviceSelect_t device, uint8_t *fifoData, uint16_t timestamp, uint16_t irqSequence)
{
	uint16_t ppg[4];
	Si117x_Ecode_t ret = 0;
	SI117XDRV_PPG24bSample_t sample;
	SI117XDRV_PPGSample_t sample16;
	int i;
	uint32_t j=0;
	for (i=0; i<4; i++)
	{
		      if ( fifoData[j]==0x10 && fifoData[j+1]=='P' )  // discard PPG header
		        j=j+2;
		      if ( fifoData[j]==0x10 && fifoData[j+1]=='A' )  // discard SYNC header
		        j=j+2;
		      if (fifoData[j]== 0x10 && fifoData[j+1]== 0x10)  // double DLE? Dump one
		        j++;
		      ppg[i] = 256 *fifoData[j] + fifoData[j+1];     // combine MSB with LSB. LSB does not have DLE shielding
		      j = j+2;
	}
    sample.sequence = irqSequence;
    sample.syncMessage = 0;
    sample.timestamp = timestamp;
    sample.ppg1 = ppg[0];
    sample.ppg2 = ppg[1];
    sample.ppg3 = ppg[2];
    sample.ppg4 = ppg[3];
    ret = SI117XDRV_EnqueuePPG24bSampleData(&sample);
    sample16.timestamp = sample.timestamp;
    sample16.syncMessage = sample.syncMessage;
    sample16.sequence = sample.sequence;
    sample16.ppg1 = sample.ppg1;
    sample16.ppg2 = sample.ppg2;
    sample16.ppg3 = sample.ppg3;
    sample16.ppg4 = sample.ppg4;
    ret |= SI117XDRV_EnqueuePPG16bSampleData(&sample16);
    return ret;
}

void setFifoProcessingValues(int oversampling, uint8_t meas_cntl, uint8_t taskEnable, uint8_t ppg_measconfig0, uint8_t ppg_measconfig1, uint8_t ppg_measconfig2, uint8_t ppg_measconfig3 )
//temporary function to get demo working
{
	devices[0].deviceCfg.globalCfg.taskEnable = taskEnable;
	devices[0].deviceCfg.globalCfg.meas_cntl = meas_cntl;
	devices[0].deviceStatus.hrmOversamplingRatio = oversampling;
	devices[0].deviceCfg.ppgCfg.ppgCfg[0].ppg_measconfig = ppg_measconfig0;
	devices[0].deviceCfg.ppgCfg.ppgCfg[1].ppg_measconfig = ppg_measconfig1;
	devices[0].deviceCfg.ppgCfg.ppgCfg[2].ppg_measconfig = ppg_measconfig2;
	devices[0].deviceCfg.ppgCfg.ppgCfg[3].ppg_measconfig = ppg_measconfig3;
}

/***************************************************************************//**
 * @brief
 *    FIFO interrupt handler
 *
 * @param[in] device select ID
 *
 * @param[in] pointer fifo data read from the sensor
 *
 * @param[in] number of bytes read from fifo
 *
 * @param[in] 16 bit timestamp of interrupt occurrance
 *
 * @param[in] irq sequence number
 *
 * @param[out] number of bytes in fifo that were part of a DLE packet header. 
 *             If null, there will be no output, and pointer will not be used.
 *
 * @return
 *    Si117XDRV error code
 ******************************************************************************/
Si117x_Ecode_t SI117XDRV_ProcessFifoData(SI117XDRV_DeviceSelect_t device, uint8_t *record, uint16_t fifo_length, uint16_t timestamp, uint16_t irqSequence, uint16_t *dlePacketBytesOut)
{
	int i;
	char data_byte = 0;
	uint8_t *record_ptr;
	Si117x_Ecode_t ret = ECODE_SI117XDRV_OK;
	si117xFifoStateTaskComplete_t task_complete;
	SI117XDRV_PPG24bSample_t sample;
	SI117XDRV_PPGSample_t sample16;
	SI117XDRV_ECGSample_t sampleECG;
	SI117XDRV_ECGLDSample_t sampleECGLD;
	si117xFifoStateMachine_t *fifoStateMachine = &(fifoStateMachines[device]);
	SI117XDRV_GlobalRegisters_t *configuration;
	SI117XDRV_PPGRegisters_t *ppgCfg;
	uint16_t dlePacketBytes = 0;

	configuration = &(devices[device].deviceCfg.globalCfg);
	ppgCfg = &(devices[device].deviceCfg.ppgCfg);
	record_ptr = &(*record);   					// start of data read from FIFO

	for (i = 0; i < fifo_length; i += 1)   	// process until end of buffer
	{
	  data_byte = record_ptr[i];

	  if (Unprocessed_DLE==true)  		// If last record ended with a DLE, process it using first data bytes from this record .
	  {
	    Unprocessed_DLE = false;		// Don't increment i
	    if (data_byte != 0x10)      	// if the second byte is not a DLE then decode it as a command
	    {
	      if (data_byte=='S')
	      { // <DLE>S found.
	        found_DLE_S = 1;				// insert S marker into next sample structure
	        i++;
	        dlePacketBytes++;
	        DLE_S_tag = record_ptr[i];		// N, ID tag associated with <DLE>S
	        continue;
	      }

	      si117xFifoStateMachine_Next(device, 1, data_byte, &task_complete);
	      continue;
	    }
	    else
	      goto Take_DLE_as_data;
	  }

	  if (Unprocessed_DLE_S == true)
	  {
	    Unprocessed_DLE_S = false;
	    found_DLE_S = 1;
	    DLE_S_tag = data_byte;
	    continue;
	  }


	  if ((data_byte == 0x10) && fifoStateMachine->isMsb && (i <= fifo_length-2))   //if DLE & we are looking at MSB then check the next byte
	  {																			// but will only work if there are at least two more data bytes left in this record.																																												// "-5" includes the two ASCII DLE bytes + two more data bytes + 0x10 (real DLE) terminator.

	    i += 1;
	    data_byte = record_ptr[i];
	    dlePacketBytes++;  // This handles the DLE byte, the

	    if (data_byte == 'S')
	    {
	      if (i == fifo_length-1)	// <DLE>S, but we don't have N in this block since it is end of data block
	      {
	        Unprocessed_DLE_S = true;
	        continue;
	      }

	      found_DLE_S = 1;
	      i++;
	      dlePacketBytes++;
	      DLE_S_tag = record_ptr[i];
	      continue;
	    }

	    if (data_byte != 0x10)  // if the second byte is not a DLE then decode it as a command
	    {
	      dlePacketBytes++;
	      si117xFifoStateMachine_Next(device,  1, data_byte, &task_complete);
	      continue;
	    }
	  }
	  else if  ((data_byte == 0x10) && fifoStateMachine->isMsb)    // (DLE) is at end of record. Cannot deal with it yet.
	  {
	    Unprocessed_DLE = true;		// set flag to tell next record to deal with it
	    dlePacketBytes++;
	    return ret;
	  }


Take_DLE_as_data:
		switch (fifoStateMachine->fifoState)
		{
		case SI117x_FIFO_STATE_PPG1_HI:
			fifoStateMachine->ppg_value[0] = (data_byte << 16) & 0xFF0000;
			break;
		case SI117x_FIFO_STATE_PPG1_MID:
			fifoStateMachine->ppg_value[0] |= (data_byte << 8) & 0x00FF00;
			ppg1_sample_count[device]++;
			break;
		case SI117x_FIFO_STATE_PPG1_LO:
			fifoStateMachine->ppg_value[0] |= data_byte & 0x0000FF;
			break;
		case SI117x_FIFO_STATE_PPG2_HI:
			fifoStateMachine->ppg_value[1] = (data_byte << 16) & 0xFF0000;
			break;
		case SI117x_FIFO_STATE_PPG2_MID:
			fifoStateMachine->ppg_value[1] |= (data_byte << 8) & 0x00FF00;
			ppg2_sample_count[device]++;
			break;
		case SI117x_FIFO_STATE_PPG2_LO:
			fifoStateMachine->ppg_value[1] |= data_byte & 0x0000FF;
			break;
		case SI117x_FIFO_STATE_PPG3_HI:
			fifoStateMachine->ppg_value[2] = (data_byte << 16) & 0xFF0000;
			break;
		case SI117x_FIFO_STATE_PPG3_MID:
			fifoStateMachine->ppg_value[2] |= (data_byte << 8) & 0x00FF00;
			ppg3_sample_count[device]++;
			break;
		case SI117x_FIFO_STATE_PPG3_LO:
			fifoStateMachine->ppg_value[2] |= data_byte & 0x0000FF;
			break;
		case SI117x_FIFO_STATE_PPG4_HI:
			fifoStateMachine->ppg_value[3] = (data_byte << 16) & 0xFF0000;
			break;
		case SI117x_FIFO_STATE_PPG4_MID:
			fifoStateMachine->ppg_value[3] |= (data_byte << 8) & 0x00FF00;
			ppg4_sample_count[device]++;
			break;
		case SI117x_FIFO_STATE_PPG4_LO:
			fifoStateMachine->ppg_value[3] |= data_byte & 0x0000FF;
			break;
		case SI117x_FIFO_STATE_BIOZ1_HI:
			fifoStateMachine->bioz_value[0] = (data_byte << 16) & 0xFF0000;
			break;
		case SI117x_FIFO_STATE_BIOZ1_MID:
			fifoStateMachine->bioz_value[0] |= (data_byte << 8) & 0x00FF00;
			break;
		case SI117x_FIFO_STATE_BIOZ1_LO:
			fifoStateMachine->bioz_value[0] |= data_byte & 0x0000FF;
			break;
		case SI117x_FIFO_STATE_BIOZ2_HI:
			fifoStateMachine->bioz_value[1] = (data_byte << 16) & 0xFF0000;
			break;
		case SI117x_FIFO_STATE_BIOZ2_MID:
			fifoStateMachine->bioz_value[1] |= (data_byte << 8) & 0x00FF00;
			break;
		case SI117x_FIFO_STATE_BIOZ2_LO:
			fifoStateMachine->bioz_value[1] |= data_byte & 0x0000FF;
			break;
		case SI117x_FIFO_STATE_ECG_HI:
			fifoStateMachine->ecg_value = (data_byte << 16) & 0xFF0000;
			break;
		case SI117x_FIFO_STATE_ECG_MID:
			fifoStateMachine->ecg_value |= (data_byte << 8) & 0x00FF00;
			break;
		case SI117x_FIFO_STATE_ECG_LO:
			fifoStateMachine->ecg_value |= data_byte & 0x0000FF;
			break;
		case SI117x_FIFO_STATE_ECG_LD_BYTE1:
			fifoStateMachine->ecg_ld_value[0] = data_byte;
			break;
		case SI117x_FIFO_STATE_ECG_LD_BYTE2:
			fifoStateMachine->ecg_ld_value[1] = data_byte;
			break;
		case SI117x_FIFO_STATE_ECG_LD_BYTE3:
			fifoStateMachine->ecg_ld_value[2] = data_byte;
			break;
		case SI117x_FIFO_STATE_ECG_LD_BYTE4:
			fifoStateMachine->ecg_ld_value[3] = data_byte;
			break;
		case SI117x_FIFO_STATE_UNKNOWN:
		  break;
		}

		si117xFifoStateMachine_Next(device, 0, 0, &task_complete);

		if (task_complete == SI117x_TASK_COMPLETE_ECG)
		{

			sampleECG.sequence = irqSequence;
			sampleECG.timestamp = timestamp;
			sampleECG.syncMessage = 0;
			sampleECG.ecg = fifoStateMachine->ecg_value;
			ret |= SI117XDRV_EnqueueECGSampleData(&sampleECG);
			fifoStateMachine->ecg_value =0;
		}
		if (task_complete == SI117x_TASK_COMPLETE_ECG_LD)
		{

		    sampleECGLD.sequence = irqSequence;
			sampleECGLD.timestamp = timestamp;
			sampleECGLD.syncMessage = 0;
			sampleECGLD.data[0] = fifoStateMachine->ecg_ld_value[0];
			sampleECGLD.data[1] = fifoStateMachine->ecg_ld_value[1];
			sampleECGLD.data[2] = fifoStateMachine->ecg_ld_value[2];
			sampleECGLD.data[3] = fifoStateMachine->ecg_ld_value[3];
			ret |= SI117XDRV_EnqueueECGLDSampleData(&sampleECGLD);
			fifoStateMachine->ecg_ld_value[0] =0;
			fifoStateMachine->ecg_ld_value[1] =0;
			fifoStateMachine->ecg_ld_value[2] =0;
			fifoStateMachine->ecg_ld_value[3] =0;
		}
		if (task_complete == SI117x_TASK_COMPLETE_PPG)
		{
			int j;
			uint32_t mask;

			for (j = 0; j < 4; j++)
			{
				if ((ppgCfg->ppgCfg[j].ppg_measconfig & 0x10) != 0)  // if raw ppg mode then the number is 2's complement and me must signe extend it
				{
					mask = (uint32_t)0x8000 << (8 * (configuration->meas_cntl & 0x01));        //create the 16 or 24-bit mask
					if (fifoStateMachine->ppg_value[j] & mask)                                                   //if sign bit is high then sign-extend
						fifoStateMachine->ppg_value[j] |= 0xFFFF0000 << (8 * (configuration->meas_cntl & 0x01));
				}
			}

            sample.timestamp = timestamp;
            sample.syncMessage = 0;
            sample.sequence = irqSequence;
            sample.ppg1 = fifoStateMachine->ppg_value[0];
            sample.ppg2 = fifoStateMachine->ppg_value[1];
            sample.ppg3 = fifoStateMachine->ppg_value[2];
            sample.ppg4 = fifoStateMachine->ppg_value[3];
			if (found_DLE_S==true)
			{
				sample.syncMessage = DLE_S_tag;
				found_DLE_S=false;
			}

			ret |= SI117XDRV_EnqueuePPG24bSampleData(&sample);
            sample16.timestamp = sample.timestamp;
            sample16.syncMessage = sample.syncMessage;
            sample16.sequence = sample.sequence;
            sample16.ppg1 = sample.ppg1;
            sample16.ppg2 = sample.ppg2;
            sample16.ppg3 = sample.ppg3;
            sample16.ppg4 = sample.ppg4;
			ret |= SI117XDRV_EnqueuePPG16bSampleData(&sample16);
			fifoStateMachine->ppg_value[0] = 0;
			fifoStateMachine->ppg_value[1] = 0;
			fifoStateMachine->ppg_value[2] = 0;
			fifoStateMachine->ppg_value[3] = 0;

		}
	}

	if (dlePacketBytesOut != 0) {
	  *dlePacketBytesOut = dlePacketBytes;
	}

  return ret;
}



/**************************************************************************//**
 * @brief
 *****************************************************************************/
static int16_t si117xFifoStateMachine_Next(SI117XDRV_DeviceSelect_t device, uint8_t isDle, uint8_t dleDataByte, si117xFifoStateTaskComplete_t *taskComplete)
{
	int16_t error = 0;
	si117xFifoStateMachine_t *fifoStateMachine = &(fifoStateMachines[device]);
	SI117XDRV_GlobalRegisters_t *configuration;
	uint16_t oversampling;

	configuration = &(devices[device].deviceCfg.globalCfg);
#if 1
	*taskComplete = SI117x_TASK_NOT_COMPLETE;
	fifoStateMachine->isMsb = 1;  //the code below will set this to 0 when it should
    oversampling = devices[device].deviceStatus.hrmOversamplingRatio;
	if (isDle == 1)
	{
		switch (dleDataByte)
		{
		  case 'P':
			  if (configuration->taskEnable & 0x1)
				  fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG1_HI : SI117x_FIFO_STATE_PPG1_MID;
			  else if (configuration->taskEnable & 0x2)
				  fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG2_HI : SI117x_FIFO_STATE_PPG2_MID;
			  else if (configuration->taskEnable & 0x4)
				  fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG3_HI : SI117x_FIFO_STATE_PPG3_MID;
			  else if (configuration->taskEnable & 0x8)
				  fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG4_HI : SI117x_FIFO_STATE_PPG4_MID;
			  else
				  fifoStateMachine->fifoState = SI117x_FIFO_STATE_UNKNOWN;
			  break;
		  case 'E':
			  if (configuration->taskEnable & 0x40)
				  fifoStateMachine->fifoState = (configuration->meas_cntl & 0x4) ? SI117x_FIFO_STATE_ECG_HI : SI117x_FIFO_STATE_ECG_MID;
			  else
				  fifoStateMachine->fifoState = SI117x_FIFO_STATE_UNKNOWN;
			  break;
		  case 'B':
			  if (configuration->taskEnable & 0x10)
				  fifoStateMachine->fifoState = (configuration->meas_cntl & 0x2) ? SI117x_FIFO_STATE_BIOZ1_HI : SI117x_FIFO_STATE_BIOZ1_MID;
			  else if (configuration->taskEnable & 0x20)
				  fifoStateMachine->fifoState = (configuration->meas_cntl & 0x2) ? SI117x_FIFO_STATE_BIOZ2_HI : SI117x_FIFO_STATE_BIOZ2_MID;
			  else
				  fifoStateMachine->fifoState = SI117x_FIFO_STATE_UNKNOWN;
			  break;

		  case 'A':

			  if (configuration->taskEnable & 0x01) {
				  ppg_count = ppg1_sample_count[device];
				  ppg1_sample_count[device] = 0; }
			  else if (configuration->taskEnable & 0x02) {
				  ppg_count = ppg2_sample_count[device];
				  ppg2_sample_count[device] = 0; }
			  else if (configuration->taskEnable & 0x04) {
				  ppg_count = ppg3_sample_count[device];
				  ppg3_sample_count[device] = 0; }
			  else if (configuration->taskEnable & 0x08) {
				  ppg_count = ppg4_sample_count[device];
				  ppg4_sample_count[device] = 0; }


			  // Only oversampling of 2, 4 and 8 are supported right now
			  if (oversampling == 8)
				  ppg_count = (ppg_count + 4) >> 3;	// divide by configuration->oversampling with rounding
			  if (oversampling == 4)
				  ppg_count = (ppg_count + 2) >> 2;	// divide by configuration->oversampling with rounding
			  if (oversampling == 2)
				  ppg_count = (ppg_count + 1) >> 1;	// divide by configuration->oversampling with rounding
			  accelSyncCallbacks(device, ppg_count);
			  //AccelResample_SynchMessageReceived(ppg_count);

			  break;
		  case 'S':
			  //user sync - If this is used, add 1 for user synch to the dlePacketBytes count in SI117XDRV_ProcessFifoData.
			  break;
		  case 'F':
			  //measurement overrun
              break;
		  case 'L':
			  fifoStateMachine->fifoState = SI117x_FIFO_STATE_ECG_LD_BYTE1;
			  break;
		  default:
			  //fifoStateMachine->fifoState = SI117x_FIFO_STATE_UNKNOWN;
			  break;
	  }
	}

	else
	{
		switch (fifoStateMachine->fifoState)
		{
  		case SI117x_FIFO_STATE_PPG1_HI: case SI117x_FIFO_STATE_PPG1_MID:
		case SI117x_FIFO_STATE_PPG2_HI: case SI117x_FIFO_STATE_PPG2_MID:
		case SI117x_FIFO_STATE_PPG3_HI: case SI117x_FIFO_STATE_PPG3_MID:
		case SI117x_FIFO_STATE_PPG4_HI: case SI117x_FIFO_STATE_PPG4_MID:
		case SI117x_FIFO_STATE_BIOZ1_HI: case SI117x_FIFO_STATE_BIOZ1_MID:
		case SI117x_FIFO_STATE_BIOZ2_HI: case SI117x_FIFO_STATE_BIOZ2_MID:
		case SI117x_FIFO_STATE_ECG_HI: case SI117x_FIFO_STATE_ECG_MID:
		case SI117x_FIFO_STATE_ECG_LD_BYTE1: case SI117x_FIFO_STATE_ECG_LD_BYTE2:
		case SI117x_FIFO_STATE_ECG_LD_BYTE3:
			fifoStateMachine->fifoState = (si117xFifoState_t)(fifoStateMachine->fifoState + 1);
			fifoStateMachine->isMsb = 0;
			break;
		case SI117x_FIFO_STATE_ECG_LD_BYTE4:
			fifoStateMachine->fifoState = SI117x_FIFO_STATE_ECG_LD_BYTE1;
			*taskComplete = SI117x_TASK_COMPLETE_ECG_LD;
			break;
		case SI117x_FIFO_STATE_PPG1_LO:
			if (configuration->taskEnable & 0x2)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG2_HI : SI117x_FIFO_STATE_PPG2_MID;
			else if (configuration->taskEnable & 0x4)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG3_HI : SI117x_FIFO_STATE_PPG3_MID;
			else if (configuration->taskEnable & 0x8)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG4_HI : SI117x_FIFO_STATE_PPG4_MID;
			else
			{
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG1_HI : SI117x_FIFO_STATE_PPG1_MID;
				*taskComplete = SI117x_TASK_COMPLETE_PPG;
			}
			break;
		case SI117x_FIFO_STATE_PPG2_LO:
			if (configuration->taskEnable & 0x4)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG3_HI : SI117x_FIFO_STATE_PPG3_MID;
			else if (configuration->taskEnable & 0x8)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG4_HI : SI117x_FIFO_STATE_PPG4_MID;
			else if (configuration->taskEnable & 0x1)
			{
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG1_HI : SI117x_FIFO_STATE_PPG1_MID;
				*taskComplete = SI117x_TASK_COMPLETE_PPG;
			}
			else
			{
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG2_HI : SI117x_FIFO_STATE_PPG2_MID;
			    *taskComplete = SI117x_TASK_COMPLETE_PPG;
		    }
		    break;
		case SI117x_FIFO_STATE_PPG3_LO:
			if (configuration->taskEnable & 0x8)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG4_HI : SI117x_FIFO_STATE_PPG4_MID;
			else if (configuration->taskEnable & 0x1)
			{
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG1_HI : SI117x_FIFO_STATE_PPG1_MID;
				*taskComplete = SI117x_TASK_COMPLETE_PPG;
			}
			else if (configuration->taskEnable & 0x2)
			{
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG2_HI : SI117x_FIFO_STATE_PPG2_MID;
				*taskComplete = SI117x_TASK_COMPLETE_PPG;
			}
			else
			{
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG3_HI : SI117x_FIFO_STATE_PPG3_MID;
				*taskComplete = SI117x_TASK_COMPLETE_PPG;
			}

			break;
		case SI117x_FIFO_STATE_PPG4_LO:
			if (configuration->taskEnable & 0x1)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG1_HI : SI117x_FIFO_STATE_PPG1_MID;
			else if (configuration->taskEnable & 0x2)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG2_HI : SI117x_FIFO_STATE_PPG2_MID;
			else if (configuration->taskEnable & 0x4)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG3_HI : SI117x_FIFO_STATE_PPG3_MID;
			else
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x1) ? SI117x_FIFO_STATE_PPG4_HI : SI117x_FIFO_STATE_PPG4_MID;
			*taskComplete = SI117x_TASK_COMPLETE_PPG;
			break;
		case SI117x_FIFO_STATE_BIOZ1_LO:
			if (configuration->taskEnable & 0x20)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x2) ? SI117x_FIFO_STATE_BIOZ2_HI : SI117x_FIFO_STATE_BIOZ2_MID;
			else
			{
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x2) ? SI117x_FIFO_STATE_BIOZ1_HI : SI117x_FIFO_STATE_BIOZ1_MID;
				*taskComplete = SI117x_TASK_COMPLETE_BIOZ;
			}
			break;
		case SI117x_FIFO_STATE_BIOZ2_LO:
			if (configuration->taskEnable & 0x10)
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x2) ? SI117x_FIFO_STATE_BIOZ1_HI : SI117x_FIFO_STATE_BIOZ1_MID;
			else
				fifoStateMachine->fifoState = (configuration->meas_cntl & 0x2) ? SI117x_FIFO_STATE_BIOZ2_HI : SI117x_FIFO_STATE_BIOZ2_MID;
			*taskComplete = SI117x_TASK_COMPLETE_BIOZ;
			break;
		case SI117x_FIFO_STATE_ECG_LO:
			fifoStateMachine->fifoState = (configuration->meas_cntl & 0x4) ? SI117x_FIFO_STATE_ECG_HI : SI117x_FIFO_STATE_ECG_MID;
			*taskComplete = SI117x_TASK_COMPLETE_ECG;
			break;
		case SI117x_FIFO_STATE_UNKNOWN:
		  break;
		}
	}

	if (fifoStateMachine->fifoState == SI117x_FIFO_STATE_UNKNOWN)
		fifoStateMachine->isMsb = 0;
#endif
	return error;
}

/* *INDENT-OFF* */
/******** THE REST OF THE FILE IS DOCUMENTATION ONLY !**********************//**
 * @addtogroup emdrv
 * @{
 * @addtogroup SI117XDRV
 * @brief SI117XDRV Si117x Driver
 * @{

@details
  The source files for the SI117x driver library resides in the
  si117xdrv folder, and are named si117xdrv.c and si117xdrv.h.

  @li @ref si117xdrv_intro
  @li @ref si117xdrv_conf
  @li @ref si117xdrv_api

@n @section si117xdrv_intro Introduction
  The Si117x driver supports the Si117x optical sensor.

  @note This driver require the low level i2c functions to be implemented by the user.

@n @section si117xdrv_conf Configuration Options

  Some properties of the SI117XDRV driver are compile-time configurable. These
  properties are set in a file named @ref si117xdrv_config.h. A template for this
  file, containing default values, resides in the emdrv/config folder.
  To configure SI117XDRV for your application, provide your own configuration file,
  or override the defines on the compiler command line.
  These are the available configuration parameters with default values defined.
  @code

  // Max number of data queues that will be allocated
  #define SI117XDRV_MAX_DATA_QUEUES    6

  // Max number of si117x devices in the system to be used
  #define SI117XDRV_MAX_NUM_DEVICES    2

  // Max number of callbacks to be used
  #define SI117XDRV_MAX_NUM_CALLBACKS       6

  The properties of each SI117X driver instance are set at run-time via the
  @ref UARTDRV_InitUart_t data structure input parameter to the @ref UARTDRV_InitUart()
  function for UART and USART peripherals, and the @ref UARTDRV_InitLeuart_t
  data structure input parameter to the @ref UARTDRV_InitLeuart() function for
  LEUART peripherals.

@n @section si117xdrv_api The API

  This section contain brief descriptions of the functions in the API. You will
  find detailed information on input and output parameters and return values by
  clicking on the hyperlinked function names. Most functions return an error
  code, @ref ECODE_SI117XDRV_OK is returned on success,
  see @ref si117xdrv.h for other error codes.

  Your application code must include one header file: @em si117xdrv.h.

  @ref UARTDRV_InitUart(), @ref UARTDRV_InitLeuart() and @ref UARTDRV_DeInit() @n
    These functions initialize and deinitialize the UARTDRV driver. Typically
    @htmlonly UARTDRV_InitUart() @endhtmlonly (for UART/USART) or
    @htmlonly UARTDRV_InitLeuart() @endhtmlonly (for LEUART) are called once in
    your startup code.

  @ref UARTDRV_GetReceiveStatus() and @ref UARTDRV_GetTransmitStatus() @n
    Query the status of a current transmit or receive operations. Reports number
    of items (frames) transmitted and remaining.

  @ref UARTDRV_GetReceiveDepth() and  @ref UARTDRV_GetTransmitDepth() @n
    Get the number of queued receive or transmit operations.

  @ref UARTDRV_Transmit(), UARTDRV_Receive() @n
  UARTDRV_TransmitB(), UARTDRV_ReceiveB() @n
  UARTDRV_ForceTransmit() and UARTDRV_ForceReceive() @n
    Transfer functions come in both blocking and non-blocking versions,
    the blocking versions have an uppercase B (for Blocking) at the end of
    their function name. Blocking functions will not return before the transfer
    has completed. The non-blocking functions signal transfer completion with a
    callback function. @ref UARTDRV_ForceTransmit() and
    @ref UARTDRV_ForceReceive() are also blocking. These two functions access
    the UART peripheral directly without using DMA or interrupts.
    @ref UARTDRV_ForceTransmit() does not respect flow control.
    @ref UARTDRV_ForceReceive() forces RTS low.



 * @} end group SI117XDRV *******************************************************
 * @} end group emdrv *********************************************************/
