File content as of revision 0:ac4dea3e2894:

/*******************************************************************************
 * Copyright (C) 2018 Maxim Integrated Products, Inc., All Rights Reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining a
 * copy of this software and associated documentation files (the "Software"),
 * to deal in the Software without restriction, including without limitation
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,
 * and/or sell copies of the Software, and to permit persons to whom the
 * Software is furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL MAXIM INTEGRATED BE LIABLE FOR ANY CLAIM, DAMAGES
 * OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
 * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
 * OTHER DEALINGS IN THE SOFTWARE.
 *
 * Except as contained in this notice, the name of Maxim Integrated
 * Products, Inc. shall not be used except as stated in the Maxim Integrated
 * Products, Inc. Branding Policy.
 *
 * The mere transfer of this software does not imply any licenses
 * of trade secrets, proprietary technology, copyrights, patents,
 * trademarks, maskwork rights, or any other form of intellectual
 * property whatsoever. Maxim Integrated Products, Inc. retains all
 * ownership rights.
 *******************************************************************************
 */
#include "SH_Max8614x_BareMetal.h"
#include "SHComm.h"
#include "HostAccelHelper.h"
#include <string.h> //for memset
#include <stdint.h>

#include "demoDefinitions.h"

/* Do not warn sign/unsigned miss alignment*/
//#pragma warning (disable : 4018 )

uint16_t HrmResult       = 0;
uint8_t  HrmConfidence   = 0;

//#define SERIALOUT printf
#define DEBUG_INFO

#if defined(DEBUG_INFO)
	#define __DBGMESSAGE( str , val ) {printf(str, val);}
#else
	#define __DBGMESSAGE( str , val )
#endif


// Defines
#define SSMAX8614X_REG_SIZE  		1
#define SSMAX8614X_MODE1_DATASIZE  18				//Taken from API doc
#define SSWHRM_MODE1_DATASIZE 		6				//Taken from API doc
#define SSACCEL_MODE1_DATASIZE  	6				//Taken from API doc
#define SSAGC_MODE1_DATASIZE  		0				//Taken from API doc
#define SSBPT_MODE1_2_DATASIZE  	4				//Taken from API doc /* TODO */

#define MIN_MACRO(a,b) ((a)<(b)?(a):(b))

// sensor configuration
//#define ENABLE_SENSOR_HUB_ACCEL
#define USE_HOST_ACCEL
// end of senor and algorithm configuration
#define MAX_NUM_WR_ACC_SAMPLES			 5
#define BMI160_SAMPLE_RATE				25
// end of defines

//function pointer use to perform arithmetic operation
typedef void (*rx_data_callback)(uint8_t *);
typedef struct {
	int data_size;
	rx_data_callback rx_data_parser;
} ss_data_req;

typedef struct {
	int16_t x;
	int16_t y;
	int16_t z;
} accel_mode1_data;

typedef struct {
	uint32_t led1;
	uint32_t led2;
	uint32_t led3;
	uint32_t led4;
	uint32_t led5;
	uint32_t led6;
} max8614x_mode1_data;

typedef struct {
	uint16_t hr;
	uint8_t hr_conf;
	uint16_t RESERVED;
	uint8_t status;
} whrm_mode1_data;


typedef struct Max86140_SH_Status_Tracker {
	uint8_t sensor_data_from_host;
	uint8_t data_type_enabled;					// what type of data is enabled
	uint8_t sample_count_enabled;				// does me11 provide sample count
	uint32_t sample_count;
	uint8_t data_buf_storage[512];				// store data read from SH
	ss_data_req algo_callbacks[SH_NUM_CURRENT_ALGOS];
	ss_data_req sensor_callbacks[SH_NUM_CURRENT_SENSORS];
	uint8_t sensor_enabled_mode[SH_NUM_CURRENT_SENSORS];
	uint8_t algo_enabled_mode[SH_NUM_CURRENT_ALGOS];
	int input_fifo_size;
} Max86140_SH_Status_Tracker_t;

// Max8614x Default Callbacks
void max8614x_data_rx(uint8_t* data_ptr)
{
	max8614x_mode1_data sample;
	sample.led1 = (data_ptr[0] << 16) | (data_ptr[1] << 8) | data_ptr[2];
	sample.led2 = (data_ptr[3] << 16) | (data_ptr[4] << 8) | data_ptr[5];
	sample.led3 = (data_ptr[6] << 16) | (data_ptr[7] << 8) | data_ptr[8];
	sample.led4 = (data_ptr[9] << 16) | (data_ptr[10] << 8) | data_ptr[11];
	sample.led5 = (data_ptr[12] << 16) | (data_ptr[13] << 8) | data_ptr[14];
	sample.led6 = (data_ptr[15] << 16) | (data_ptr[16] << 8) | data_ptr[17];

	//SERIALOUT("led1=%.6X led2=%.6X led3=%.6X led4=%.6X led5=%.6X led6=%.6X\r\n",
	//		sample.led1, sample.led2, sample.led3, sample.led4, sample.led5, sample.led6);

	//enqueue(&max8614x_queue, &sample);
}
void whrm_data_rx(uint8_t* data_ptr) {
	//See API doc for data format
	whrm_mode1_data sample;
	sample.hr = (data_ptr[0] << 8) | data_ptr[1];
	sample.hr_conf = data_ptr[2];
	sample.RESERVED = (data_ptr[3] << 8) | data_ptr[4];
	sample.status = data_ptr[5];
	HrmResult  = sample.hr / 10;
	HrmConfidence = sample.hr_conf;
	SERIALOUT("hr_c=%d\r\n", HrmResult);
#if defined(DEBUG_INFO)
	SERIALOUT("hr=%.1f conf=%d status=%d\r\n", (float)sample.hr / 10.0, sample.hr_conf, sample.status);
#endif
	//enqueue(&whrm_queue, &sample);
}

void accel_data_rx(uint8_t* data_ptr) {
	//See API doc for data format
	accel_mode1_data sample;
	sample.x = (data_ptr[0] << 8) | data_ptr[1];
	sample.y = (data_ptr[2] << 8) | data_ptr[3];
	sample.z = (data_ptr[4] << 8) | data_ptr[5];
#if defined(DEBUG_INFO)
	//SERIALOUT("x:%d, y:%d, z:%d\r\n", sample.x, sample.y, sample.z);
#endif
}

void agc_data_rx(uint8_t* data_ptr) {
	//NOP: AGC does not collect data
}
// end of Max8614x Default Callbacks


static Max86140_SH_Status_Tracker * get_config_struct() {

    /* assigns a static adress to configuration struct*/
	static Max86140_SH_Status_Tracker glbl_max8614x_status_track;
	return &glbl_max8614x_status_track;
}

void initialize_config_struct() {
	Max86140_SH_Status_Tracker *p_glbl_max8614x_status_track = get_config_struct();
	/*
	 * Desc: Configuration init flow, Perform this action at init stage of data acquisition. Raw sesnsor data buffer pointer is input to each
	 *       enabled sensor/algorithm,s funtion that is responsible to extract numeric data from data byte stream from sensor hub.
	 *
	 *       - Append Sensor Raw Data structure with raw sensor data sample size and pointer to function of sensor that is reposible to parse
	 *         data byte stream from sesnor hub and extract sensor numeric data.
	 *       - Append accompanying sensors to main state of sensor. ie Accelerometer from Host with sensor data sample size and pointer to function of
	 *         sensor that is reposible to parse data byte stream from sesnor hub and extract sensor numeric data.
	 *       - Append algorithms to be enabled  with algorithm data sample size and pointer to function of
	 *         algorithm that is reposible to parse data byte stream from sensor hub and extract sensor numeric data.
	 *
	 * */

	//set all the values to 0
	memset(p_glbl_max8614x_status_track, 0, sizeof(*p_glbl_max8614x_status_track));
	// max8614x
	p_glbl_max8614x_status_track->sensor_callbacks[SH_SENSORIDX_MAX8614X].data_size = SSMAX8614X_MODE1_DATASIZE;
	p_glbl_max8614x_status_track->sensor_callbacks[SH_SENSORIDX_MAX8614X].rx_data_parser = &max8614x_data_rx;
	// accelerometer
	p_glbl_max8614x_status_track->sensor_callbacks[SH_SENSORIDX_ACCEL].data_size = SSACCEL_MODE1_DATASIZE;
	p_glbl_max8614x_status_track->sensor_callbacks[SH_SENSORIDX_ACCEL].rx_data_parser = &accel_data_rx;
	// whrm NOTE: do not register aec and scd as they do not have data reporter callbacks and defined within WHRM suite. Look at 8614c command table for aec/scd on/off !!!!
	p_glbl_max8614x_status_track->algo_callbacks[SH_ALGOIDX_WHRM].data_size = SSWHRM_MODE1_DATASIZE;
	p_glbl_max8614x_status_track->algo_callbacks[SH_ALGOIDX_WHRM].rx_data_parser = &whrm_data_rx;

}


void SH_Max8614x_get_reg(uint8_t addr, uint32_t *val) {
	int status = sh_get_reg(SH_SENSORIDX_MAX8614X, addr, val);

	if (status == 0) {
		__DBGMESSAGE("\r\n reg_val=%02X err=0 \r\n", ((uint8_t)*val))
	} else {
		__DBGMESSAGE("\r\n err=%d\r\n", -1)
	}

	return;
}

int CSTMR_SH_FeedAccDataIntoSH(Max86140_SH_Status_Tracker_t *p_max8614x_status_track) {
	static accel_data_t peek_buf[MAX_NUM_WR_ACC_SAMPLES];
	static uint8_t tx_buf[MAX_NUM_WR_ACC_SAMPLES * sizeof(accel_mode1_data) + 2]; // 2 bytes for the command
	if(!p_max8614x_status_track->sensor_data_from_host) {
		return -1;
	} else {
		accel_data_t accel_data = {0};
		accel_mode1_data acc_sample;
		int num_tx, num_samples, num_bytes = 0, num_wr_bytes = 0;
		int num_written_samples, nb_expected;
		int ret = 0;

		// get accelerometer data
		ret = CSTMR_SH_HostAccelerometerGet_sensor_xyz(&accel_data);
		if (ret < 0)
			return ret;

		if(CSTMR_SH_HostAccelerometerEnqueueData(&accel_data) != 0) {
			__DBGMESSAGE("Thrown an accel sample\n", NULL)
		}

		if(CSTMR_SH_HostAccelerometerGetDataCount() < MAX_NUM_WR_ACC_SAMPLES) {
			return -1;
		}

		ret = sh_get_num_bytes_in_input_fifo(&num_bytes);
		if (ret != 0) {
			__DBGMESSAGE("Unable to read num bytes in input fifo\r\n", NULL)
			return -1;
		}
		num_tx = p_max8614x_status_track->input_fifo_size - num_bytes;
		if (num_tx <= 0) {
			__DBGMESSAGE("num_tx can't be negative\r\n",NULL)
			return -1;
		}
		num_samples = num_tx / sizeof(accel_mode1_data);
		num_samples = MIN_MACRO(num_samples, MAX_NUM_WR_ACC_SAMPLES);
		num_tx = num_samples * sizeof(accel_mode1_data);
		if (num_samples == 0) {
			__DBGMESSAGE("Input FIFO is Full\r\n",NULL)
			return -1;
		}

		for(int i = 0; i < num_samples; ++i) {
			ret |= CSTMR_SH_HostAccelerometerDequeuData(&peek_buf[i]);
		}
		if (ret != 0) {
			__DBGMESSAGE("CSTMR_SH_HostAccelerometerDequeuData failed\r\n",NULL)
			return -1;
		}


		for (int i = 2, j = 0; j < num_samples; i+= sizeof(accel_mode1_data), j++) {
			accel_data = peek_buf[j];
			acc_sample.x = (int16_t)(accel_data.x*1000);
			acc_sample.y = (int16_t)(accel_data.y*1000);
			acc_sample.z = (int16_t)(accel_data.z*1000);
			tx_buf[i] = acc_sample.x;
			tx_buf[i + 1] = acc_sample.x >> 8;
			tx_buf[i + 2] = acc_sample.y;
			tx_buf[i + 3] = acc_sample.y >> 8;
			tx_buf[i + 4] = acc_sample.z;
			tx_buf[i + 5] = acc_sample.z >> 8;

		}

		ret = sh_feed_to_input_fifo(tx_buf, num_tx + 2, &num_wr_bytes);
		if(ret != 0) {
			__DBGMESSAGE("sh_feed_to_input_fifo\r\n",NULL)
			return -1;
		}
		num_written_samples = num_wr_bytes / sizeof(accel_mode1_data);
		if(num_written_samples != num_samples) {
			__DBGMESSAGE("num_written_samples failed\r\n",NULL)
			return -1;
		}
	}
	return 0;
}


void SH_Max8614x_set_reg(uint8_t addr, uint32_t val) {
	int status;
	status = sh_set_reg(SH_SENSORIDX_MAX8614X, addr, val, SSMAX8614X_REG_SIZE);
	__DBGMESSAGE("\r\n err=%d\r\n", status);
}



int SH_Max8614x_data_report_execute(void) {

	int num_samples, databufLen;
	uint8_t *databuf;


	Max86140_SH_Status_Tracker_t *p_glbl_max8614x_status_track = get_config_struct();

	// prepare the buffer to store the results
	databuf = p_glbl_max8614x_status_track->data_buf_storage;
	databufLen = sizeof(p_glbl_max8614x_status_track->data_buf_storage);

	// poll SH
	sh_ss_execute_once(databuf, databufLen, &num_samples);
	//__DBGMESSAGE( "nsamplesFIFO: %d \r\n" , num_samples)

	if(num_samples > 0 && num_samples <255) {
		//Skip status byte
		uint8_t *data_ptr = &databuf[1];

		int i = 0;
		for (i = 0; i < num_samples; i++) {
			int sh_data_type = p_glbl_max8614x_status_track->data_type_enabled;
			if (p_glbl_max8614x_status_track->sample_count_enabled) {
				p_glbl_max8614x_status_track->sample_count = *data_ptr++;
			}
			//Chop up data and send to modules with enabled sensors
			if (sh_data_type == SS_DATATYPE_RAW || sh_data_type == SS_DATATYPE_BOTH) {
				for (int i = 0; i < SH_NUM_CURRENT_SENSORS; i++) {
					if (p_glbl_max8614x_status_track->sensor_enabled_mode[i]) {
						p_glbl_max8614x_status_track->sensor_callbacks[i].rx_data_parser(data_ptr);
						data_ptr += p_glbl_max8614x_status_track->sensor_callbacks[i].data_size;
					}
				}
			}
			if (sh_data_type == SS_DATATYPE_ALGO || sh_data_type == SS_DATATYPE_BOTH) {
				for (int i = 0; i < SH_NUM_CURRENT_ALGOS; i++) {
					if (p_glbl_max8614x_status_track->algo_enabled_mode[i]) {
						p_glbl_max8614x_status_track->algo_callbacks[i].rx_data_parser(data_ptr);
						data_ptr += p_glbl_max8614x_status_track->algo_callbacks[i].data_size;
					}
				}
			}
		}
        /* JUST*/
		CSTMR_SH_FeedAccDataIntoSH(p_glbl_max8614x_status_track);
	}
	// feed accelerometer into me11
	//////////////////CSTMR_SH_FeedAccDataIntoSH(p_glbl_max8614x_status_track);


	return num_samples;
}

int SH_Max8614x_algo_init(enum enAlgoMode paramAlgoMode) {

	/*
	 *
	 * */
	int status;
	Max86140_SH_Status_Tracker_t *p_glbl_max8614x_status_track = get_config_struct();
	if(p_glbl_max8614x_status_track->algo_enabled_mode[SH_ALGOIDX_WHRM]) {
		    __DBGMESSAGE("\r\n Algo already enabled\r\n",NULL)
		return -1;
	}

	if(paramAlgoMode == kAlgoModeHeartRate) {
		status = sh_enable_algo(SH_ALGOIDX_WHRM, SSWHRM_MODE1_DATASIZE);
		if (status != SS_SUCCESS) {
			__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
			__DBGMESSAGE("FAILED at line %d, enable whrm\n", __LINE__)
			return status;
		}
		p_glbl_max8614x_status_track->algo_enabled_mode[SH_ALGOIDX_WHRM] = 0x01;
	}
}



int SH_Max8614x_default_init(enum enAlgoMode paramAlgoMode) {
	/*
	 *  Desc: Initialization flow to get algorithm estimation results:
	 *        1. initialize algorithm config struct
	 *        2. enable data type to both raw sensor and algorithm data
	 *        3. get input fifo size to learn fifo capacity
	 *        4. set fifo threshold for mfio event frequency
	 *        5. enable sensor to acquire ppg data
	 *        6. enable accompanying accel sensor
	 *        7. enable algorithm
	 *        8. Sensor Hub now starts to write raw sensor/algorithm data to its data report FIFO which
	 *           reports mfio event when data size determined by fifo threshold is written to report fifo
	 *           data can be read by SH_Max8614x_data_report_execute function.
	 *
	 * */

	int status;

	// first initialize the global config struct
	initialize_config_struct();
	Max86140_SH_Status_Tracker_t *p_glbl_max8614x_status_track = get_config_struct();

	// get input fifo size
	status = sh_get_input_fifo_size(&p_glbl_max8614x_status_track->input_fifo_size);
	if (status != SS_SUCCESS) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return COMM_GENERAL_ERROR;;
	}

	// enable both data stype
	p_glbl_max8614x_status_track->data_type_enabled = SS_DATATYPE_BOTH;
	p_glbl_max8614x_status_track->sample_count_enabled = false;
	status = sh_set_data_type(p_glbl_max8614x_status_track->data_type_enabled,
							  p_glbl_max8614x_status_track->sample_count_enabled);
	if (status != 0) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return COMM_GENERAL_ERROR;
	}

	status = sh_set_fifo_thresh(5);
	if (status != 0) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return COMM_GENERAL_ERROR;
	}



	status = sh_sensor_enable(SH_SENSORIDX_MAX8614X, SSMAX8614X_MODE1_DATASIZE, SH_INPUT_DATA_DIRECT_SENSOR);
	if (status != 0) {
		__DBGMESSAGE("\r\n err=%d\r\n",  COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return COMM_GENERAL_ERROR;
	}
	p_glbl_max8614x_status_track->sensor_enabled_mode[SH_SENSORIDX_MAX8614X] = 0x01;

#ifdef ENABLE_SENSOR_HUB_ACCEL
	status = sh_sensor_enable(SH_SENSORIDX_ACCEL, SSACCEL_MODE1_DATASIZE, SH_INPUT_DATA_DIRECT_SENSOR);
	if (status != SS_SUCCESS) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
	}
	p_glbl_max8614x_status_track->sensor_data_from_host = false;
	p_glbl_max8614x_status_track->sensor_enabled_mode[SH_SENSORIDX_ACCEL] = 0x01;
#elif defined(USE_HOST_ACCEL)
	CSTMR_SH_HostAccelerometerInitialize();
	CSTMR_SH_HostAccelerometerSetDefaults();
	status = CSTMR_SH_HostAccelerometerSetSampleRate(BMI160_SAMPLE_RATE);
	if (status != 0) {
		__DBGMESSAGE("Unable to set BMI160's sample rate\n",NULL)
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return status;
	}

	status = CSTMR_SH_HostAccelerometerEnableDataReadyInterrupt();
	if(status != 0){
		__DBGMESSAGE("Unable to enable BMI160 Interrupt, ret: %d\n", status)
		return status;
	}

	status = sh_sensor_enable(SH_SENSORIDX_ACCEL, SSACCEL_MODE1_DATASIZE, SH_INPUT_DATA_FROM_HOST);
	if (status != 0) {
		__DBGMESSAGE("\r\n err=%d\r\n", COMM_GENERAL_ERROR)
		__DBGMESSAGE("FAILED at line %d\n", __LINE__)
		return status;
	}
	p_glbl_max8614x_status_track->sensor_data_from_host = true;
	p_glbl_max8614x_status_track->sensor_enabled_mode[SH_SENSORIDX_ACCEL] = 0x01;
#endif
	status = SH_Max8614x_algo_init(paramAlgoMode);
	if(status != 0) {
		__DBGMESSAGE("AlgoInitFailed\r\n",NULL)
	}

	    __DBGMESSAGE("\r\n err=%d\r\n",  status)
	return status;
}

void SH_Max8614x_stop() {

	sh_disable_irq_mfioevent();
	Max86140_SH_Status_Tracker_t *p_glbl_max8614x_status_track = get_config_struct();

	for(int i = 0; i < SH_NUM_CURRENT_SENSORS; ++i) {
		if(p_glbl_max8614x_status_track->sensor_enabled_mode[i]) {
			p_glbl_max8614x_status_track->sensor_enabled_mode[i] = 0;
			sh_sensor_disable(i);
		}

	}

	for(int i = 0; i < SH_NUM_CURRENT_ALGOS; ++i) {
		if(p_glbl_max8614x_status_track->algo_enabled_mode[i]) {
			p_glbl_max8614x_status_track->algo_enabled_mode[i] = 0;
			sh_disable_algo(i);
		}
	}

	if(p_glbl_max8614x_status_track->sensor_data_from_host) {
		CSTMR_SH_HostAccelerometerInitialize();
		p_glbl_max8614x_status_track->sensor_data_from_host = 0;
	}

	sh_clear_mfio_event_flag();
	sh_enable_irq_mfioevent();
}






/* **********************************************************************************************
 * 																							   	*
 *   					   COMMAND INTERFACE RELATED METHODS								 	*
 *																								*
 * **********************************************************************************************/



//MYG: CHECK FOR STDIN WITH SSCANF < WHY THEY NEEDED PARSE_CMD?????   PAY ATTENTION: HEX AND/OR DECIMAL PARAMETER ENRTY!!!


static int SH_Max8614x_set_singleparamcfg_(const char *cfg , const int algo_idx, const int cfg_idx , const int paramsz){
	int status = -1;
	uint32_t val;
	if(paramsz == 1 || paramsz == 2) {
    	if( sscanf(cfg, "%*s %*s %*s %10x", &val) == 1 ){
    		uint8_t Temp[2] = { (uint8_t)((val >> 8) & 0xFF),  (uint8_t) (val & 0xFF) };
    		status = sh_set_algo_cfg(algo_idx, cfg_idx , &Temp[2-paramsz], paramsz);
    	}
	}
    return status;  // if command error return -1 if operational error return >0 error
}


static int SH_Max8614x_get_singleparamcfg_( const int algo_idx, const int cfg_idx ,const int paramsz, int *val){
	// CMD: get_cfg spo2 samplerate

	int tmp;
	int status = -1;
	uint8_t rxBuff[3]; // first byte is status 1/2 bytes cfgparam asked for.
	if(paramsz == 1 || paramsz == 2) {
	    status = sh_get_algo_cfg(algo_idx, cfg_idx, &rxBuff[0], paramsz+1);
		if( status == 0){
			tmp = (int)((rxBuff[1]<<(8*(paramsz-1))) + rxBuff[2]*(paramsz-1));  // MYG: CHECK IF TRUE!
		}else
			tmp = -1;
	}else
		tmp = -1;

	*val = tmp;
	return status;
}





int SH_Max8614x_set_ppgreg(const char *addr_value_args){

	//CMD: set_reg ppgsensor 0xAA 0xAA
	int addr,val,status;
	if( sscanf(addr_value_args,"%*s %*s %4x %10x", &addr , &addr ) == 2 ){
        status = sh_set_reg(SH_SENSORIDX_MAX8614X, (uint8_t) addr, (uint32_t) val, SSMAX8614X_REG_SIZE);
        if(status == 0)
        	SERIALOUT("OK \r\n");
	}else
		    SERIALOUT("ERR \r\n");

    return status;
}

int SH_Max8614x_get_ppgreg(const char *addr_arg){

	//CMD: get_reg ppgsensor 0xAA
	int addr;
	int status = -1;
	uint32_t val;

	if( sscanf(addr_arg,"%*s %*s %4x", &addr) == 1 ){
		int status = sh_get_reg(SH_SENSORIDX_MAX8614X, (uint8_t) addr, &val);
        if(status == 0)
        	SERIALOUT("reg_val=%02X \r\n",val);
	}else
		    SERIALOUT("ERR \r\n");

    return status;
}

int SH_Max8614x_self_test_ppg(const char *null_arg){

	// MYG: mfio interaction needed!
    return -1;
}

int SH_Max8614x_self_test_acc(const char *null_arg){
	// MYG: mfio interaction needed!
	return -1;
}

//************************************WHRM METHODS***********************************************/

int SH_Max8614x_get_whrm_dataformat(const char *null_arg){

	 SERIALOUT("\r\n  format={smpleCnt,16},"
		"{grnCnt,20},{grn2Cnt,20},{accelX,14,3},{accelY,14,3},"
		"{accelZ,14,3},{hr,12},{hrconf,8},{spo2,11,1},{activity,8} err=0\r\n" );
      return 0;
}

int SH_Max8614x_measure_whrm(const char *null_arg){

	 int status;
	 SH_Max8614x_stop();
	 status = SH_Max8614x_default_init(kAlgoModeHeartRate);
	 if(status == 0)
		 SERIALOUT("whrm started \r\n");
	 else
		 SERIALOUT("ERR");

     return status;
}

int SH_Max8614x_set_whrm_aecusage(const char *onoff_arg){

	int status = SH_Max8614x_set_singleparamcfg_( onoff_arg , SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_AEC_ENABLE , 1 );
    if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	if( status == -1)
    		SERIALOUT("CMDERR \r\n");
    	else
    		SERIALOUT("ERR \r\n");
    }
    return status;

}

int SH_Max8614x_get_whrm_aecusage(const char *null_arg){

	int val;
	int status =  SH_Max8614x_get_singleparamcfg_( SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_AEC_ENABLE, 1 , &val);
    if(val != -1)
    	SERIALOUT("whrm_aec_usage= %d", val);
    else
    	SERIALOUT("ERR \r\n");

    return status;

}

int SH_Max8614x_set_whrm_scdusage(const char *onoff_arg){

	int status = SH_Max8614x_set_singleparamcfg_( onoff_arg , SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_SCD_ENABLE, 1 );
    if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	if( status == -1)
    		SERIALOUT("CMDERR \r\n");
    	else
    		SERIALOUT("ERR \r\n");
    }
    return status;

}


int SH_Max8614x_get_whrm_scdusage(const char *null_arg){

	int val;
	int status =  SH_Max8614x_get_singleparamcfg_( SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_SCD_ENABLE, 1 , &val);
    if(val != -1)
    	SERIALOUT("whrm_scd_usage= %d", val);
    else
    	SERIALOUT("ERR \r\n");

    return status;

}

int SH_Max8614x_set_whrm_scdadjperiod(const char *period_arg){

	int status = SH_Max8614x_set_singleparamcfg_(period_arg , SH_ALGOIDX_WHRM , SS_CFGIDX_WHRM_ADJ_TARGET_PD_CURRENT_PERIOD , 2);
    if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	if( status == -1)
    		SERIALOUT("CMDERR \r\n");
    	else
    		SERIALOUT("ERR \r\n");
    }
    return status;
}

int SH_Max8614x_get_whrm_scdadjperiod(const char *null_arg){

	int val;
	int status =  SH_Max8614x_get_singleparamcfg_( SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_ADJ_TARGET_PD_CURRENT_PERIOD, 2, &val);
    if(val != -1)
    	SERIALOUT("whrm_scd_adjperiod= 0x%x", val);
    else
    	SERIALOUT("ERR \r\n");

    return status;

}

int SH_Max8614x_set_whrm_scddebouncewin(const char *dwindow_arg){

	int status = SH_Max8614x_set_singleparamcfg_(dwindow_arg , SH_ALGOIDX_WHRM , SS_CFGIDX_WHRM_SCD_DEBOUNCE_WINDOW , 2);
    if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	if( status == -1)
    		SERIALOUT("CMDERR \r\n");
    	else
    		SERIALOUT("ERR \r\n");
    }
    return status;

}
int SH_Max8614x_get_whrm_scddebouncewin(const char *null_arg){

	int val;
	int status =  SH_Max8614x_get_singleparamcfg_( SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_SCD_DEBOUNCE_WINDOW , 2, &val);
    if(val != -1)
    	SERIALOUT("whrm_scd_debouncewin= 0x%x", val);
    else
    	SERIALOUT("ERR \r\n");

    return status;

}

int SH_Max8614x_set_whrm_motionthresh(const char *motion_arg){

	int status = SH_Max8614x_set_singleparamcfg_(motion_arg , SH_ALGOIDX_WHRM , SS_CFGIDX_WHRM_MOTION_MAG_THRESHOLD , 2);
    if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	if( status == -1)
    		SERIALOUT("CMDERR \r\n");
    	else
    		SERIALOUT("ERR \r\n");
    }
    return status;

}

int SH_Max8614x_get_whrm_motionthresh(const char *null_arg){

	int val;
	int status =  SH_Max8614x_get_singleparamcfg_( SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_MOTION_MAG_THRESHOLD , 2, &val);
    if(val != -1)
    	SERIALOUT("whrm_motion_threshold= 0x%x", val);
    else
    	SERIALOUT("ERR \r\n");

    return status;

}

int SH_Max8614x_set_whrm_minpdiodecurr(const char *curr_arg){

	int status = SH_Max8614x_set_singleparamcfg_(curr_arg, SH_ALGOIDX_WHRM , SS_CFGIDX_WHRM_MIN_PD_CURRENT , 2);
    if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	if( status == -1)
    		SERIALOUT("CMDERR \r\n");
    	else
    		SERIALOUT("ERR \r\n");
    }
    return status;

}

int SH_Max8614x_get_whrm_minpdiodecurr(const char *null_arg){

	int val;
	int status =  SH_Max8614x_get_singleparamcfg_( SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_MIN_PD_CURRENT , 2, &val);
    if(val != -1)
    	SERIALOUT("whrm_currentrange= %d", val);  // MYG: CHECKIF EXPRESSION IS TRUE!
    else
    	SERIALOUT("ERR \r\n");

    return status;

}

int SH_Max8614x_set_whrm_pdiodeconfig(const char *cfg_arg){

	int status = SH_Max8614x_set_singleparamcfg_( cfg_arg , SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_PD_CONFIG , 1 );
    if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	if( status == -1)
    		SERIALOUT("CMDERR \r\n");
    	else
    		SERIALOUT("ERR \r\n");
    }
    return status;

}

int SH_Max8614x_get_whrm_pdiodeconfig(const char *null_arg){

	int val;
	int status =  SH_Max8614x_get_singleparamcfg_( SH_ALGOIDX_WHRM, SS_CFGIDX_WHRM_PD_CONFIG, 1 , &val);
    if(val != -1)
    	SERIALOUT("whrm_pdiode_config= %d", val);
    else
    	SERIALOUT("ERR \r\n");

    return status;

}

int SH_Max8614x_stop_acquisition(const char *null_arg){
	      SH_Max8614x_stop();
}



/*FOR THIS DEMO PURPOSE*/
int Max8614x_Set_WSPO2Mode(int mode){

	uint8_t Temp[1] = { (uint8_t)(mode) };
	int status = sh_set_algo_cfg(SH_ALGOIDX_WSPO2, SS_CFGIDX_WSPO2_ALGO_MODE , &Temp[0], 1);
    return status;

}


#if defined(RAW_DATA_ONLY)

int SH_Max8614x_disable_whrm (const char *onoff_arg){

	int status = sh_disable_algo(SH_ALGOIDX_WHRM);
    if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	SERIALOUT("ERR \r\n");
    }
    return status;

}

int SH_Max8614x_enable_pureraw(const char *onoff_arg){

	int status;

	status = sh_disable_algo(SH_ALGOIDX_WHRM);
	wait_ms(500);
	status = sh_set_data_type(SS_DATATYPE_RAW ,	false);
	status = sh_set_fifo_thresh(5);
	status = sh_sensor_enable(SH_SENSORIDX_MAX8614X, SSMAX8614X_MODE1_DATASIZE, SH_INPUT_DATA_DIRECT_SENSOR);

	if( status == 0)
    	SERIALOUT("OK \r\n");
    else {
    	SERIALOUT("ERR \r\n");
    }
    return status;

}
#endif