#include <stdio.h>
#include <stdbool.h>
#include <stdint.h>
#include <stdlib.h>
#include <math.h>

#include "hx3695.h"
#include "hr_port.h"
#include "service_hrsensor.h"
#include "service_gsensor.h"
#include "cmsis_os2.h"
#include "gsensor_api.h"
#include "task_service_timer.h"
#include "sys_typedef.h"

#ifdef  TYHX_DEMO
#include "demo_ctrl.h"
#include "twi_master.h"
#include "SEGGER_RTT.h"
#include "app_timer.h"
#include "nrf_delay.h"
#include "nrf_gpio.h"
#include "nrf_drv_gpiote.h"
#include "drv_oled.h"
#include "opr_oled.h"

#include "oled_iic.h"
#include "word.h"
#include "iic.h"
#endif
#include "hx3695_agc.h"
#include "tyhx_hrs_alg.h"
#include "tyhx_spo2_alg.h"
#include "tyhx_hrv_alg.h"
#include "hx3695_factory_test.h"

extern hrsensor_data_t g_hr_data;
osTimerId_t g_hrsensor_320ms_timer = NULL;
osTimerId_t g_hrsensor_40ms_timer = NULL;

uint8_t read_fifo_first_flg = 0;
uint8_t s_ppg_state = 0;
uint8_t s_cal_state = 0;
uint8_t check_touch_cnt = 0;
wear_msg_code_t wear_status = MSG_NO_WEAR;
wear_msg_code_t wear_status_pre = MSG_NO_WEAR;
WORK_MODE_T work_mode_flag = HRS_MODE;
ppg_sensor_data_t ppg_sensor_data;
int16_t gsen_data_x_fifo[64];
int16_t gsen_data_y_fifo[64];
int16_t gsen_data_z_fifo[64];
uint8_t alg_ram[11 * 1024] __attribute__((aligned(4)));

extern hrs_sports_mode_t hrs_sports_mode;

//////// spo2 para and switches
const uint8_t COUNT_BLOCK_NUM = 25;           // delay the block of some single good signal after a series of bad signal
const uint8_t SPO2_LOW_XCORR_THRE = 30;       //(64*xcorr)'s square below this threshold, means error signal
const uint8_t SPO2_CALI = 1;                  // spo2_result cali mode
const uint8_t XCORR_MODE = 1;                 // xcorr mode switch
const uint8_t QUICK_RESULT = 1;               // come out the spo2 result quickly ;0 is normal,1 is quick
const uint16_t MEAN_NUM = 32;                 // the length of smooth-average ;the value of MEAN_NUM can be given only 256 and 512
const uint8_t G_SENSOR = 0;                   // if =1, open the gsensor mode
const uint8_t SPO2_GSEN_POW_THRE = 150;       // gsen pow judge move, range:0-200;
const uint32_t SPO2_BASE_LINE_INIT = 153000;  // spo2 baseline init, = 103000 + ratio(a variable quantity,depends on different cases)*SPO2_SLOPE
const int32_t SOP2_DEGLITCH_THRE = 100000;    // remove signal glitch over this threshold
const int32_t SPO2_REMOVE_JUMP_THRE = 50000;  // remove signal jump over this threshold
const uint32_t SPO2_SLOPE = 50000;            // increase this slope, spo2 reduce more
const uint16_t SPO2_LOW_CLIP_END_TIME = 1500; // low clip mode before this data_cnt, normal clip mode after this
const uint16_t SPO2_LOW_CLIP_DN = 150;        // spo2 reduce 0.15/s at most in low clip mode
const uint16_t SPO2_NORMAL_CLIP_DN = 500;     // spo2 reduce 0.5/s at most in normal clip mode
const uint8_t SPO2_LOW_SNR_THRE = 40;         // snr below this threshold, means error signal
const uint16_t IR_AC_TOUCH_THRE = 200;        // AC_min*0.3
const uint16_t IR_FFT_POW_THRE = 500;         // fft_pow_min
const uint8_t SLOPE_PARA_MAX = 28;
const uint8_t SLOPE_PARA_MIN = 3;

const uint16_t static_thre_val = 150;
const uint8_t gsen_lv_val = 0;

void hx3695_spi_bus_init(void)
{
}
// us ��ʱ������ �ͻ�����д
void hx3695_delay_us(uint32_t us)
{
#ifdef  TYHX_DEMO
    nrf_delay_us(us);
#endif
	hrsensor_delay_us(us);
}


void hx3695_delay_ms(uint32_t ms)
{
#ifdef  TYHX_DEMO
    nrf_delay_ms(ms);
#endif
	hrsensor_delay_ms(ms);
}
// i2c д������ �ͻ�����д, ��ַ0x44(7bit)
bool hx3695_write_reg(uint8_t addr, uint8_t data)
{
#ifdef  TYHX_DEMO
    uint8_t data_buf[2];    
    data_buf[0] = addr;
    data_buf[1] = data;
    twi_pin_switch(1);
    twi_master_transfer(0x88, data_buf, 2, true);    //write    
#endif
	if(hrsensor_iic_write_reg(addr, data) != 0){
        return false;
    }
    return true;
}

uint8_t hx3695_read_reg(uint8_t addr)
{
    uint8_t data_buf = 0;  
#ifdef  TYHX_DEMO
    twi_pin_switch(1);
    twi_master_transfer(0x88, &addr, 1, false);      //write
    twi_master_transfer(0x89, &data_buf, 1, true);//read
#endif
	hrsensor_iic_read_reg(addr, &data_buf, 1);
    return data_buf;
}

// i2c ���������� �ͻ�����д
void hx3695_brust_read_reg(uint8_t addr, uint8_t *buf, uint16_t length)
{
#ifdef  TYHX_DEMO
	twi_pin_switch(1);
    twi_master_transfer(0x88, &addr, 1, false);      //write
    twi_master_transfer(0x89, buf, length, true); //read
#endif
	hrsensor_iic_read_reg(addr, buf, length);
}

static int tjd_service_timer_320ms_cb(void *param)
{
	heart_rate_meas_timeout_handler(NULL);
	return 250;
}

static int tjd_service_timer_40ms_cb(void *param)
{
	agc_timeout_handler(NULL);
	return 40;
}

/* ��ɶ�ȡPPG���ݶ�ʱ���ӿڣ� ѭ�����ã� Ĭ��320ms*/
// void heart_rate_meas_timeout_handler(void *p_context)
void hx3695_320ms_timer_cfg(bool en)
{
    if(en)
    {
		DEBUG_PRINTF("320ms_timers_start");
        // if (g_hrsensor_320ms_timer == NULL) {
		// 	g_hrsensor_320ms_timer = osTimerNew(heart_rate_meas_timeout_handler, osTimerPeriodic, NULL, NULL);
		// 	osTimerStart(g_hrsensor_320ms_timer, 320);
		// }

		queue_default_info_t msg_data = { tjd_service_timer_320ms_cb, (void *)NULL, 250, NULL};
		int ret = osal_msg_queue_write_copy(tjd_task_service_timer_get_queue_id(), (void *)&msg_data, sizeof(queue_default_info_t), 0);
		if(ret != 0) {
			DEBUG_PRINTF("[%s] osal_msg_queue_write_copy fail", __func__);
		}

    }
    else
    {
		DEBUG_PRINTF("320ms_timers_end");
        // if (g_hrsensor_320ms_timer != NULL) {
		// 	osTimerStop(g_hrsensor_320ms_timer);
		// 	osTimerDelete(g_hrsensor_320ms_timer);
		// 	g_hrsensor_320ms_timer = NULL;
		// }
		queue_default_info_t msg_data = { tjd_service_timer_320ms_cb, (void *)NULL, 0, NULL};
		int ret = osal_msg_queue_write_copy(tjd_task_service_timer_get_queue_id(), (void *)&msg_data, sizeof(queue_default_info_t), 0);
		if(ret != 0) {
			DEBUG_PRINTF("[%s] osal_msg_queue_write_copy fail", __func__);
		}
    }

}

void hx3695_40ms_timer_cfg(bool en)
{
    if(en)
    {
		DEBUG_PRINTF("40ms_timers_start");
        // if (g_hrsensor_40ms_timer == NULL) {
		// 	g_hrsensor_40ms_timer = osTimerNew(agc_timeout_handler, osTimerPeriodic, NULL, NULL);
		// 	osTimerStart(g_hrsensor_40ms_timer, 40);
		// }

		queue_default_info_t msg_data = { tjd_service_timer_40ms_cb, (void *)NULL, 40, NULL};
		int ret = osal_msg_queue_write_copy(tjd_task_service_timer_get_queue_id(), (void *)&msg_data, sizeof(queue_default_info_t), 0);
		if(ret != 0) {
			DEBUG_PRINTF("[%s] osal_msg_queue_write_copy fail", __func__);
		}
    }
    else
    {
		DEBUG_PRINTF("40ms_timers_end");
        // if (g_hrsensor_40ms_timer != NULL) {
		// 	osTimerStop(g_hrsensor_40ms_timer);
		// 	osTimerDelete(g_hrsensor_40ms_timer);
		// 	g_hrsensor_40ms_timer = NULL;
		// }

		queue_default_info_t msg_data = { tjd_service_timer_40ms_cb, (void *)NULL, 0, NULL};
		int ret = osal_msg_queue_write_copy(tjd_task_service_timer_get_queue_id(), (void *)&msg_data, sizeof(queue_default_info_t), 0);
		if(ret != 0) {
			DEBUG_PRINTF("[%s] osal_msg_queue_write_copy fail", __func__);
		}
    }
}

void hx3695_gpioint_cfg(bool en)
{
    if (en)
    {
        hx3695_gpioint_enable();
    }
    else
    {
        hx3695_gpioint_disable();
    }
}

void Efuse_Mode_Check(void)
{
    uint8_t REG_50_E, REG_51_E, REG_52_E, REG_53_E;

	hx3695_write_reg(0x5c, 0x08);
	hx3695_write_reg(0x5d, 0x02);
	hx3695_delay_us(10);

	hx3695_write_reg(0x5a,0x10);

	hx3695_write_reg(0x5b,0x04);
	hx3695_write_reg(0x5b,0x00);
    hx3695_delay_us(10);
	hx3695_write_reg(0x5b,0x44);
	hx3695_write_reg(0x5b,0x40);

	REG_50_E = hx3695_read_reg(0x50);
	REG_51_E = hx3695_read_reg(0x51);
	REG_52_E = hx3695_read_reg(0x52);
	REG_53_E = hx3695_read_reg(0x53);

    hx3695_write_reg(0x5a,0x20);
    hx3695_write_reg(0x50,REG_50_E);
    hx3695_write_reg(0x51,REG_51_E);
    hx3695_write_reg(0x52,REG_52_E);
    hx3695_write_reg(0x53,REG_53_E);

//    DEBUG_PRINTF("EFUSE MC2: 0x50=0x%2x  0x51= 0x%2x  0x52= 0x%2x  0x53= 0x%2x ",REG_50_E,REG_51_E,REG_52_E,REG_53_E);

	hx3695_write_reg(0x5a,0x00);
    hx3695_write_reg(0x5c,0x00); //0x5c
    hx3695_write_reg(0x5d,0x00); //0x5d

}

bool hx3695_chip_check(void)
{
    uint8_t i = 0;
    uint8_t chip_id = 0;
	// hx3695_write_reg(0x02, 0x00);
	// hx3695_delay_ms(5);
	// uint8_t ret = hx3695_read_reg(0x1b);
	// printf("addr 1b value:%x\n", ret);
	// hx3695_write_reg(0x1b, 0x02);
	// hx3695_delay_ms(5);
	// ret = hx3695_read_reg(0x1b);
	// printf("addr 1b value:%x\n", ret);
    for (i = 0; i < 10; i++)
    {
		hx3695_write_reg(0x02, 0x00);
		hx3695_delay_ms(5);
        chip_id = hx3695_read_reg(0x00);

        if (chip_id == 0x65)
        {
			printf("chip_id:%x hx3695_chip_check succ\n", chip_id);
            return true;
        }
    }
	printf("chip_id:%x hx3695_chip_check fail\n", chip_id);
    return false;
}

uint8_t hx3695_read_fifo_size(void)
{
    uint8_t data_len_h, data_len_l;
    uint16_t fifo_data_length;
    data_len_h = hx3695_read_reg(0x21);
    data_len_l = hx3695_read_reg(0x20);
    fifo_data_length = (((data_len_h & 0x01) << 8) | data_len_l);
    return fifo_data_length;
}

uint16_t hx3695_led_data_analysis(ppg_sensor_data_t *ppg_sensor_dat, uint16_t fifo_read_length, uint8_t sig)
{
    int32_t data_temp = 0;
    uint8_t fifo_data_flag = 0;
    uint16_t count = 0;
    uint32_t ii = 0;
	
/*增加fifo长度限制，防止死机*/
		DEBUG_PRINTF("fifo_read_length=%d ",fifo_read_length);
		
		if(fifo_read_length > FIFO_DATA_LENGTH)
		{
			fifo_read_length = FIFO_DATA_LENGTH;
		}	
    for(ii=0; ii<fifo_read_length; ii++)
    {
        fifo_data_flag = ppg_sensor_dat->s_buf[ii*3+2]>>5;
        count = ii/ppg_sensor_dat->led_activated;
        if(sig==0)
        {
            data_temp = (int32_t)(ppg_sensor_dat->s_buf[ii*3]|(ppg_sensor_dat->s_buf[ii*3+1]<<8)|((ppg_sensor_dat->s_buf[ii*3+2]&0x1f)<<16));
        }
        else
        {
            if((ppg_sensor_dat->s_buf[ii*3+2]&0x10)!=0)
            {
                data_temp = (int32_t)(ppg_sensor_dat->s_buf[ii*3]|(ppg_sensor_dat->s_buf[ii*3+1]<<8)|((ppg_sensor_dat->s_buf[ii*3+2]&0x0f)<<16))-1048576;
            }
            else
            {
                data_temp = (int32_t)(ppg_sensor_dat->s_buf[ii*3]|(ppg_sensor_dat->s_buf[ii*3+1]<<8)|((ppg_sensor_dat->s_buf[ii*3+2]&0x1f)<<16));
            }
        }
		//DEBUG_PRINTF("fifo_data: %d %d", ii, data_temp);
        ppg_sensor_dat->led_data[fifo_data_flag].data_val[count] = data_temp;
    }
    for(ii=0;ii<4;ii++)
    {
        if(ppg_sensor_dat->led_data[ii].led_state == Activated)
        {
            ppg_sensor_dat->led_data[ii].data_count = count+1;
        }
    }
	return count+1;
}

void hx3695_get_led_para(ppg_sensor_data_t *ppg_sensor_dat)
{
	uint8_t led_index = 0;
	for(led_index=0;led_index<4;led_index++)
	{
		if(ppg_sensor_dat->led_data[led_index].led_state == Activated)
		{
			hx3695_write_reg(0X43, led_index*2+1);
			ppg_sensor_dat->led_data[led_index].led_current = hx3695_read_reg(0x45);
			ppg_sensor_dat->led_data[led_index].offset_idac = hx3695_read_reg(0x48);
			ppg_sensor_dat->led_data[led_index].tia_rf = hx3695_read_reg(0x44)&0x0f;
		}
	}
}

uint16_t hx3695_read_fifo_data(ppg_sensor_data_t *ppg_sensor_dat)
{
    uint8_t databuf[3];
//    uint32_t ii=0;
    uint16_t data_len_h = 0;
    uint16_t data_len_l = 0;
    uint16_t fifo_data_length = 0;
    uint16_t fifo_read_length = 0;
    uint16_t fifo_read_bytes = 0;
	uint16_t fifo_out_count = 0;

    data_len_h = hx3695_read_reg(0x21);
    data_len_l = hx3695_read_reg(0x20);
    fifo_data_length = (((data_len_h&0x01)<<8)|data_len_l);

    if(fifo_data_length<2*ppg_sensor_dat->led_activated)
    {
        return 0;
    }
    fifo_read_length = ((fifo_data_length-ppg_sensor_dat->led_activated)/ppg_sensor_dat->led_activated)*ppg_sensor_dat->led_activated;
    fifo_read_bytes = fifo_read_length*3;
    if(read_fifo_first_flg == 1)
    {
        hx3695_brust_read_reg(0x22, databuf, 3);
        read_fifo_first_flg = 0;
    }
    hx3695_brust_read_reg(0x22, ppg_sensor_dat->s_buf, fifo_read_bytes);
    fifo_out_count = hx3695_led_data_analysis(ppg_sensor_dat,fifo_read_length,1);

	hx3695_get_led_para(ppg_sensor_dat);

    return fifo_out_count;
}

void hx3695_read_phase_data(int32_t *s_buf)
{
    uint8_t  databuf1[6] = {0};
    uint8_t  databuf2[6] = {0};
    uint8_t  databuf3[6] = {0};
	uint8_t  databuf4[6] = {0};

    hx3695_brust_read_reg(0x03, databuf1, 6);
    hx3695_brust_read_reg(0x09, databuf2, 6);
    hx3695_brust_read_reg(0x0f, databuf3, 6);
    hx3695_brust_read_reg(0x15, databuf4, 6);

    s_buf[0] = ((databuf1[0])|(databuf1[1]<<8)|(databuf1[2]<<16));
	s_buf[1] = ((databuf1[3])|(databuf1[4]<<8)|(databuf1[5]<<16));
	s_buf[2] = ((databuf2[0])|(databuf2[1]<<8)|(databuf2[2]<<16));
    s_buf[3] = ((databuf2[3])|(databuf2[4]<<8)|(databuf2[5]<<16));
    s_buf[4] = ((databuf3[0])|(databuf3[1]<<8)|(databuf3[2]<<16));
	s_buf[5] = ((databuf3[3])|(databuf3[4]<<8)|(databuf3[5]<<16));
	s_buf[6] = ((databuf4[0])|(databuf4[1]<<8)|(databuf4[2]<<16));
    s_buf[7] = ((databuf4[3])|(databuf4[4]<<8)|(databuf4[5]<<16));
	// DEBUG_PRINTF("phase_buf = %d,%d,%d,%d,%d,%d,%d,%d ", s_buf[0],s_buf[1],s_buf[2],s_buf[3],s_buf[4],s_buf[5],s_buf[6],s_buf[7]);
}

void hx3695_read_led_data(int32_t *s_buf)
{
    uint8_t  databuf1[3] = {0};
    uint8_t  databuf2[3] = {0};
    uint8_t  databuf3[3] = {0};
	uint8_t  databuf4[3] = {0};

    hx3695_brust_read_reg(0x06, databuf1, 3);
    hx3695_brust_read_reg(0x0c, databuf2, 3);
    hx3695_brust_read_reg(0x12, databuf3, 3);
    hx3695_brust_read_reg(0x18, databuf4, 3);

    s_buf[0] = ((databuf1[0])|(databuf1[1]<<8)|(databuf1[2]<<16));
	s_buf[1] = ((databuf2[0])|(databuf2[1]<<8)|(databuf2[2]<<16));
    s_buf[2] = ((databuf3[0])|(databuf3[1]<<8)|(databuf3[2]<<16));
	s_buf[3] = ((databuf4[0])|(databuf4[1]<<8)|(databuf4[2]<<16));
	// DEBUG_PRINTF("led_buf = %d,%d,%d,%d", s_buf[0],s_buf[1],s_buf[2],s_buf[3]);
}

void hx3695_ppg_off(void)
{
    hx3695_write_reg(0x02, 0x01);
}

void hx3695_ppg_on(void)
{
    hx3695_write_reg(0x02, 0x00);
    hx3695_delay_ms(5);
}

void hx3695_software_reset(void)
{
	uint8_t ii = 0;
	ppg_sensor_data.mode = NULL_MODE;
	ppg_sensor_data.fs = 25;
	ppg_sensor_data.watermark = 0;
	ppg_sensor_data.led_activated = 0;
	for(ii=0;ii<4;ii++)
	{
		ppg_sensor_data.led_data[ii].isneed_cal_start = false;
		ppg_sensor_data.led_data[ii].led_current = 0;
		ppg_sensor_data.led_data[ii].offset_idac = 0;
		ppg_sensor_data.led_data[ii].tia_rf = 0;
		ppg_sensor_data.led_data[ii].led_state = InActivated;
	}
	read_fifo_first_flg = 0;
	s_ppg_state = 0;
	s_cal_state = 0;
    check_touch_cnt = 0;
	hx3695_agc_reset();
	#if defined(INT_MODE)
	hx3695_gpioint_cfg(false);
	#else
	hx3695_320ms_timer_cfg(false);
	hx3695_40ms_timer_cfg(false);
	#endif
}

bool hx3695_check_wear(ppg_sensor_data_t *ppg_sensor_dat)
{
    uint8_t ii = 0;

    for(ii=0;ii<ppg_sensor_dat->led_data[3].data_count;ii++)
    {
		// DEBUG_PRINTF("led_data_val[%d] = %d", ii, ppg_sensor_dat->led_data[3].data_val[ii]);
        if(ppg_sensor_dat->led_data[3].data_val[ii] > CHECK_WEAR_THRE)
        {
            if(check_touch_cnt < CHECK_TOUCH_NUM)
            {
                check_touch_cnt++; 
            }  
            if(check_touch_cnt >= CHECK_TOUCH_NUM)  
            {
                wear_status = MSG_WEAR;
            }   
        }
        else if(ppg_sensor_dat->led_data[3].data_val[ii] < CHECK_UNWEAR_THRE)
        {
            if(check_touch_cnt>0)
            {
                check_touch_cnt--;
            }
            if(check_touch_cnt == 0)
            {
                wear_status = MSG_NO_WEAR;                
            }      
        }

        if(wear_status_pre != wear_status)
        {
            wear_status_pre = wear_status;
            if(wear_status == MSG_NO_WEAR)
            {
                hx3695_wear_low_power(ppg_sensor_dat);                 
            }
            else if(wear_status == MSG_WEAR)
            {
                hx3695_init(work_mode_flag);
            }  
        }  
    }
	return true;
}

void hx3695_ppg_para_init(ppg_sensor_data_t *ppg_sensor_dat)
{
    uint16_t sample_rate = 25;
    uint32_t prf_clk_num = 32000/sample_rate;
//	uint16_t watermark = 40;

    uint8_t adc_rst_post_num=0;
		uint8_t samp_delay_leden_num = 0;
    uint8_t samp_copy_avg = 0;
    uint8_t data_avg_num = 0;
		uint8_t phase7_8_internal = 0;

    uint8_t phase1_enable = 0;
    uint8_t phase2_enable = 0;
    uint8_t phase3_enable = 0;
    uint8_t phase4_enable = 0;
    uint8_t phase5_enable = 0;
    uint8_t phase6_enable = 0;
    uint8_t phase7_enable = 0;
		uint8_t phase8_enable = 0;

    uint8_t phase1_adc_osr = 3;
    uint8_t phase2_adc_osr = 3;
    uint8_t phase3_adc_osr = 3;
    uint8_t phase4_adc_osr = 3;
    uint8_t phase5_adc_osr = 3;
	uint8_t phase6_adc_osr = 3;
    uint8_t phase7_adc_osr = 3;
	uint8_t phase8_adc_osr = 3;

	uint8_t phase1_mux_sel = 0;
	uint8_t phase2_mux_sel = 1;
	uint8_t phase3_mux_sel = 2;
	uint8_t phase4_mux_sel = 3;
	uint8_t phase5_mux_sel = 4;
	uint8_t phase6_mux_sel = 5;
	uint8_t phase7_mux_sel = 6;
	uint8_t phase8_mux_sel = 7;

	uint8_t phase1_inner_avg = 0;
    uint8_t phase1_tia_res = 0;
	uint8_t phase1_tia_cap = 4;
    uint8_t phase1_led_drv1_cha_sel = 0;
    uint8_t phase1_led_drv2_cha_sel = 0;
	uint8_t phase1_pd_sel = LED0_PD_SEL;
    uint8_t phase1_als_offset_idac = 0;
	uint8_t phase1_led_offset_idac = 0;
    uint8_t phase1_ldr1_cur = 0;
	uint8_t phase1_ldr2_cur = 0;
    uint8_t phase1_ad_inr_ctrl = 3;
    uint8_t phase1_ad_of_idac_ctrl = 0;
    uint8_t phase1_led_en = 1;
	uint8_t phase1_dsp_mov_avg_num = 0;
    uint8_t phase1_dsp_iir_lp_coeff = 0;

	uint8_t phase2_inner_avg = 0;
    uint8_t phase2_tia_res = 0;
    uint8_t phase2_tia_cap = 4;
    uint8_t phase2_led_drv1_cha_sel = LED0_LDR_SEL;
    uint8_t phase2_led_drv2_cha_sel = LED0_LDR_SEL;
	uint8_t phase2_pd_sel = LED0_PD_SEL;
    uint8_t phase2_als_offset_idac = 0;
	uint8_t phase2_led_offset_idac = 0;
    uint8_t phase2_ldr1_cur = 0;
	uint8_t phase2_ldr2_cur = 0;
    uint8_t phase2_ad_inr_ctrl = 3;
    uint8_t phase2_ad_of_idac_ctrl = 0;
    uint8_t phase2_led_en = 1;
	uint8_t phase2_dsp_mov_avg_num = 0;
    uint8_t phase2_dsp_iir_lp_coeff = 0;

    uint8_t phase3_inner_avg = 0;
    uint8_t phase3_tia_res = 0;
	uint8_t phase3_tia_cap = 4;
    uint8_t phase3_led_drv1_cha_sel = 0;
    uint8_t phase3_led_drv2_cha_sel = 0;
	uint8_t phase3_pd_sel = LED1_PD_SEL;
    uint8_t phase3_als_offset_idac = 0;
	uint8_t phase3_led_offset_idac = 0;
    uint8_t phase3_ldr1_cur = 0;
	uint8_t phase3_ldr2_cur = 0;
    uint8_t phase3_ad_inr_ctrl = 3;
    uint8_t phase3_ad_of_idac_ctrl = 0;
    uint8_t phase3_led_en = 1;
	uint8_t phase3_dsp_mov_avg_num = 0;
    uint8_t phase3_dsp_iir_lp_coeff = 0;

    uint8_t phase4_inner_avg = 0;
    uint8_t phase4_tia_res = 0;
	uint8_t phase4_tia_cap = 4;
    uint8_t phase4_led_drv1_cha_sel = LED1_LDR_SEL;
    uint8_t phase4_led_drv2_cha_sel = LED1_LDR_SEL;
	uint8_t phase4_pd_sel = LED1_PD_SEL;
	#ifdef DOUBLE_GREEN
	if(ppg_sensor_dat->mode == HRS_MODE)
	{
		phase4_led_drv1_cha_sel = LED1N_LDR_SEL;
		phase4_led_drv2_cha_sel = LED1N_LDR_SEL;
		phase4_pd_sel = LED1N_PD_SEL;
	}
	#endif
    uint8_t phase4_als_offset_idac = 0;
	uint8_t phase4_led_offset_idac = 0;
    uint8_t phase4_ldr1_cur = 0;
	uint8_t phase4_ldr2_cur = 0;
    uint8_t phase4_ad_inr_ctrl = 3;
    uint8_t phase4_ad_of_idac_ctrl = 0;
    uint8_t phase4_led_en = 1;
	uint8_t phase4_dsp_mov_avg_num = 0;
    uint8_t phase4_dsp_iir_lp_coeff = 0;

    uint8_t phase5_inner_avg = 0;
    uint8_t phase5_tia_res = 0;
	uint8_t phase5_tia_cap = 4;
    uint8_t phase5_led_drv1_cha_sel = 0;
    uint8_t phase5_led_drv2_cha_sel = 0;
	uint8_t phase5_pd_sel = LED2_PD_SEL;
    uint8_t phase5_als_offset_idac = 0;
	uint8_t phase5_led_offset_idac = 0;
    uint8_t phase5_ldr1_cur = 0;
	uint8_t phase5_ldr2_cur = 0;
    uint8_t phase5_ad_inr_ctrl = 3;
    uint8_t phase5_ad_of_idac_ctrl = 0;
    uint8_t phase5_led_en = 1;
	uint8_t phase5_dsp_mov_avg_num = 0;
    uint8_t phase5_dsp_iir_lp_coeff = 0;

    uint8_t phase6_inner_avg = 0;
    uint8_t phase6_tia_res = 0;
	uint8_t phase6_tia_cap = 4;
    uint8_t phase6_led_drv1_cha_sel = LED2_LDR_SEL;
    uint8_t phase6_led_drv2_cha_sel = LED2_LDR_SEL;
		uint8_t phase6_pd_sel = LED2_PD_SEL;
    uint8_t phase6_als_offset_idac = 0;
	uint8_t phase6_led_offset_idac = 0;
    uint8_t phase6_ldr1_cur = 0;
	uint8_t phase6_ldr2_cur = 0;
    uint8_t phase6_ad_inr_ctrl = 3;
    uint8_t phase6_ad_of_idac_ctrl = 0;
    uint8_t phase6_led_en = 1;
	uint8_t phase6_dsp_mov_avg_num = 0;
    uint8_t phase6_dsp_iir_lp_coeff = 0;

    uint8_t phase7_inner_avg = 0;
    uint8_t phase7_tia_res = 1;
	uint8_t phase7_tia_cap = 4;
    uint8_t phase7_led_drv1_cha_sel = 0;
    uint8_t phase7_led_drv2_cha_sel = 0;
	uint8_t phase7_pd_sel = LED3_PD_SEL;
    uint8_t phase7_als_offset_idac = 0;
	uint8_t phase7_led_offset_idac = 0;
    uint8_t phase7_ldr1_cur = 0;
	uint8_t phase7_ldr2_cur = 0;
    uint8_t phase7_ad_inr_ctrl = 3;
    uint8_t phase7_ad_of_idac_ctrl = 0;
    uint8_t phase7_led_en = 1;
	uint8_t phase7_dsp_mov_avg_num = 0;
    uint8_t phase7_dsp_iir_lp_coeff = 0;

    uint8_t phase8_inner_avg = 0;
    uint8_t phase8_tia_res = 1;
	uint8_t phase8_tia_cap = 4;
    uint8_t phase8_led_drv1_cha_sel = LED3_LDR_SEL;
    uint8_t phase8_led_drv2_cha_sel = LED3_LDR_SEL;
	uint8_t phase8_pd_sel = LED3_PD_SEL;
    uint8_t phase8_als_offset_idac = 0;
	uint8_t phase8_led_offset_idac = 0;
    uint8_t phase8_ldr1_cur = 20;
	uint8_t phase8_ldr2_cur = 0;
    uint8_t phase8_ad_inr_ctrl = 3;
    uint8_t phase8_ad_of_idac_ctrl = 0;
    uint8_t phase8_led_en = 1;
	uint8_t phase8_dsp_mov_avg_num = 0;
    uint8_t phase8_dsp_iir_lp_coeff = 0;

    uint8_t init_wait_delay = 5;
    uint8_t afe_reset = 3;
	uint8_t afe_rst_cnvt = 1;
    uint8_t led_en_idac_pre	= 2;
	uint8_t led_en_idac	= 1;
    uint8_t led_on_time = 3;

    hx3695_write_reg(0x02,0x00);
    hx3695_delay_ms(10);

    hx3695_write_reg(0X2a, (uint8_t)prf_clk_num);
    hx3695_write_reg(0X2b, (uint8_t)(prf_clk_num>>8));
    hx3695_write_reg(0X2c, (uint8_t)(prf_clk_num>>16));

    hx3695_write_reg(0X25, (afe_rst_cnvt<<4| led_en_idac_pre<<2|led_en_idac));
	hx3695_write_reg(0X2d, phase7_8_internal);
	hx3695_write_reg(0X2e, 0x33);
    hx3695_write_reg(0X2f, ((afe_reset<<4)| init_wait_delay));
	hx3695_write_reg(0X30, (phase8_led_en<<7 | phase7_led_en<<6 |phase6_led_en<<5 |phase5_led_en<<4 |phase4_led_en<<3 | phase3_led_en<<2 | phase2_led_en<<1 | phase1_led_en) );
    hx3695_write_reg(0X72, (phase8_led_en<<7 | phase7_led_en<<6 |phase6_led_en<<5 |phase5_led_en<<4 |phase4_led_en<<3 | phase3_led_en<<2 | phase2_led_en<<1 | phase1_led_en) );

    hx3695_write_reg(0X31, led_on_time<<4);
    hx3695_write_reg(0X33, 0x83);
    hx3695_write_reg(0X34, 0x00);
    hx3695_write_reg(0X36, 0x00);
    hx3695_write_reg(0X38, 0x00);

    hx3695_write_reg(0X3a, (phase1_inner_avg | (phase2_inner_avg<<4)));
    hx3695_write_reg(0X3b, (phase3_inner_avg | (phase4_inner_avg<<4)));
    hx3695_write_reg(0X3c, (phase5_inner_avg | (phase6_inner_avg<<4)));
    hx3695_write_reg(0X3d, (phase7_inner_avg | (phase8_inner_avg<<4)));
    hx3695_write_reg(0X3e, adc_rst_post_num<<6 | samp_delay_leden_num<<4 | samp_copy_avg);
    hx3695_write_reg(0X3f, data_avg_num<<4 | data_avg_num );
    hx3695_write_reg(0X40, data_avg_num<<4 | data_avg_num );
    hx3695_write_reg(0X41, data_avg_num<<4 | data_avg_num );
    hx3695_write_reg(0X42, data_avg_num<<4 | data_avg_num );

    hx3695_write_reg(0X43, phase1_mux_sel);
    hx3695_write_reg(0X44, phase1_tia_cap<<4|phase1_tia_res);
    hx3695_write_reg(0X45, phase1_ldr1_cur);
    hx3695_write_reg(0x70, phase1_ldr2_cur);
    hx3695_write_reg(0X46, phase1_pd_sel<<4);
    hx3695_write_reg(0X47, phase1_als_offset_idac);
    hx3695_write_reg(0X48, phase1_led_offset_idac);
    hx3695_write_reg(0X49, phase1_ad_inr_ctrl<<4|phase1_ad_of_idac_ctrl);
    hx3695_write_reg(0X4a, phase1_led_drv1_cha_sel);
    hx3695_write_reg(0X4b, phase1_led_drv2_cha_sel);
    hx3695_write_reg(0X71, phase1_dsp_mov_avg_num | phase1_dsp_iir_lp_coeff<<4);

    hx3695_write_reg(0X43, phase2_mux_sel);
    hx3695_write_reg(0X44, phase2_tia_cap<<4|phase2_tia_res);
    hx3695_write_reg(0X45, phase2_ldr1_cur);
    hx3695_write_reg(0x70, phase2_ldr2_cur);
    hx3695_write_reg(0X46, phase2_pd_sel<<4);
    hx3695_write_reg(0X47, phase2_als_offset_idac);
    hx3695_write_reg(0X48, phase2_led_offset_idac);
    hx3695_write_reg(0X49, phase2_ad_inr_ctrl<<4|phase2_ad_of_idac_ctrl);
    hx3695_write_reg(0X4a, phase2_led_drv1_cha_sel);
    hx3695_write_reg(0X4b, phase2_led_drv2_cha_sel);
    hx3695_write_reg(0X71, phase2_dsp_mov_avg_num | phase2_dsp_iir_lp_coeff<<4);

    hx3695_write_reg(0X43, phase3_mux_sel);
    hx3695_write_reg(0X44, phase3_tia_cap<<4|phase3_tia_res);
    hx3695_write_reg(0X45, phase3_ldr1_cur);
    hx3695_write_reg(0x70, phase3_ldr2_cur);
    hx3695_write_reg(0X46, phase3_pd_sel<<4);
    hx3695_write_reg(0X47, phase3_als_offset_idac);
    hx3695_write_reg(0X48, phase3_led_offset_idac);
    hx3695_write_reg(0X49, phase3_ad_inr_ctrl<<4|phase3_ad_of_idac_ctrl);
    hx3695_write_reg(0X4a, phase3_led_drv1_cha_sel);
    hx3695_write_reg(0X4b, phase3_led_drv2_cha_sel);
    hx3695_write_reg(0X71, phase3_dsp_mov_avg_num | phase3_dsp_iir_lp_coeff<<4);

    hx3695_write_reg(0X43, phase4_mux_sel);
	hx3695_write_reg(0X44, phase4_tia_cap<<4|phase4_tia_res);
	hx3695_write_reg(0X45, phase4_ldr1_cur);
	hx3695_write_reg(0x70, phase4_ldr2_cur);
	hx3695_write_reg(0X46, phase4_pd_sel<<4);
	hx3695_write_reg(0X47, phase4_als_offset_idac);
	hx3695_write_reg(0X48, phase4_led_offset_idac);
    hx3695_write_reg(0X49, phase4_ad_inr_ctrl<<4|phase4_ad_of_idac_ctrl);
	hx3695_write_reg(0X4a, phase4_led_drv1_cha_sel);
	hx3695_write_reg(0X4b, phase4_led_drv2_cha_sel);
	hx3695_write_reg(0X71, phase4_dsp_mov_avg_num | phase4_dsp_iir_lp_coeff<<4);

	hx3695_write_reg(0X43, phase5_mux_sel);
	hx3695_write_reg(0X44, phase5_tia_cap<<4|phase5_tia_res);
	hx3695_write_reg(0X45, phase5_ldr1_cur);
	hx3695_write_reg(0x70, phase5_ldr2_cur);
	hx3695_write_reg(0X46, phase5_pd_sel<<4);
	hx3695_write_reg(0X47, phase5_als_offset_idac);
	hx3695_write_reg(0X48, phase5_led_offset_idac);
    hx3695_write_reg(0X49, phase5_ad_inr_ctrl<<4|phase5_ad_of_idac_ctrl);
	hx3695_write_reg(0X4a, phase5_led_drv1_cha_sel);
	hx3695_write_reg(0X4b, phase5_led_drv2_cha_sel);
	hx3695_write_reg(0X71, phase5_dsp_mov_avg_num | phase5_dsp_iir_lp_coeff<<4);

	hx3695_write_reg(0X43, phase6_mux_sel);
	hx3695_write_reg(0X44, phase6_tia_cap<<4|phase6_tia_res);
	hx3695_write_reg(0X45, phase6_ldr1_cur);
	hx3695_write_reg(0x70, phase6_ldr2_cur);
	hx3695_write_reg(0X46, phase6_pd_sel<<4);
	hx3695_write_reg(0X47, phase6_als_offset_idac);
	hx3695_write_reg(0X48, phase6_led_offset_idac);
    hx3695_write_reg(0X49, phase6_ad_inr_ctrl<<4|phase6_ad_of_idac_ctrl);
	hx3695_write_reg(0X4a, phase6_led_drv1_cha_sel);
	hx3695_write_reg(0X4b, phase6_led_drv2_cha_sel);
	hx3695_write_reg(0X71, phase6_dsp_mov_avg_num | phase6_dsp_iir_lp_coeff<<4);

	hx3695_write_reg(0X43, phase7_mux_sel);
	hx3695_write_reg(0X44, phase7_tia_cap<<4|phase7_tia_res);
	hx3695_write_reg(0X45, phase7_ldr1_cur);
	hx3695_write_reg(0x70, phase7_ldr2_cur);
	hx3695_write_reg(0X46, phase7_pd_sel<<4);
	hx3695_write_reg(0X47, phase7_als_offset_idac);
	hx3695_write_reg(0X48, phase7_led_offset_idac);
    hx3695_write_reg(0X49, phase7_ad_inr_ctrl<<4|phase7_ad_of_idac_ctrl);
	hx3695_write_reg(0X4a, phase7_led_drv1_cha_sel);
	hx3695_write_reg(0X4b, phase7_led_drv2_cha_sel);
	hx3695_write_reg(0X71, phase7_dsp_mov_avg_num | phase7_dsp_iir_lp_coeff<<4);

	hx3695_write_reg(0X43, phase8_mux_sel);
	hx3695_write_reg(0X44, phase8_tia_cap<<4|phase8_tia_res);
	hx3695_write_reg(0X45, phase8_ldr1_cur);
	hx3695_write_reg(0x70, phase8_ldr2_cur);
	hx3695_write_reg(0X46, phase8_pd_sel<<4);
	hx3695_write_reg(0X47, phase8_als_offset_idac);
	hx3695_write_reg(0X48, phase8_led_offset_idac);
    hx3695_write_reg(0X49, phase8_ad_inr_ctrl<<4|phase8_ad_of_idac_ctrl);
	hx3695_write_reg(0X4a, phase8_led_drv1_cha_sel);
	hx3695_write_reg(0X4b, phase8_led_drv2_cha_sel);
	hx3695_write_reg(0X71, phase8_dsp_mov_avg_num | phase8_dsp_iir_lp_coeff<<4);

    hx3695_write_reg(0X60, 0x14);
    hx3695_write_reg(0X61, 0xf0);
    hx3695_write_reg(0X62, 0x22);
    hx3695_write_reg(0X63, 0x15);
    hx3695_write_reg(0X64, 0x08);
    hx3695_write_reg(0X65, 0x88);
    hx3695_write_reg(0X66, 0x30);
    hx3695_write_reg(0X67, 0x23);
    hx3695_write_reg(0X68, 0x46);
    hx3695_write_reg(0X69, 0x42);
    hx3695_write_reg(0X39, 0xff);
    hx3695_write_reg(0x73, 0xff);
    hx3695_write_reg(0X6a, 0X0f);
    hx3695_write_reg(0X6b, 0X22);
    hx3695_write_reg(0X6c, 0X00);
    hx3695_write_reg(0X6d, 0X37);
    hx3695_write_reg(0X6e, 0X82);
    hx3695_write_reg(0X6f, 0X00);

#if defined(INT_MODE)
    hx3695_write_reg(0X1b, 0x91);
    hx3695_write_reg(0X1c, watermark);
    hx3695_write_reg(0X33, 0x83);
    hx3695_write_reg(0X21, 0x00);
    hx3695_write_reg(0X38, 0x04);
#else
    hx3695_write_reg(0X1b, 0x91);
    hx3695_write_reg(0X38, 0x01);
#endif

    hx3695_write_reg(0X26,(phase1_enable<<3)|(phase1_adc_osr)|(phase2_enable<<7)|(phase2_adc_osr<<4) );
    hx3695_write_reg(0X27,(phase3_enable<<3)|(phase3_adc_osr)|(phase4_enable<<7)|(phase4_adc_osr<<4) );
    hx3695_write_reg(0X28,(phase5_enable<<3)|(phase5_adc_osr)|(phase6_enable<<7)|(phase6_adc_osr<<4) );
    hx3695_write_reg(0X29,(phase7_enable<<3)|(phase7_adc_osr)|(phase8_enable<<7)|(phase8_adc_osr<<4) );

    hx3695_write_reg(0X5c, 0x02);
    hx3695_delay_ms(1);
    hx3695_write_reg(0X1b, 0x10);
    hx3695_delay_ms(1);
    hx3695_write_reg(0X1b, 0x91);
    hx3695_write_reg(0X5c, 0x00);
    read_fifo_first_flg = 1;
    return ;
}

void hx3695_set_fs(ppg_sensor_data_t *ppg_sensor_dat)
{
    uint32_t prf_clk_num = 32000/ppg_sensor_dat->fs;

	hx3695_write_reg(0X2a, (uint8_t)prf_clk_num);
    hx3695_write_reg(0X2b, (uint8_t)(prf_clk_num>>8));
    hx3695_write_reg(0X2c, (uint8_t)(prf_clk_num>>16));

	hx3695_write_reg(0X5c, 0x02);
    hx3695_delay_ms(1);
    hx3695_write_reg(0X5c, 0x00);
}

void hx3695_set_watermark(ppg_sensor_data_t *ppg_sensor_dat)
{
	uint8_t watermark = ppg_sensor_dat->watermark + ppg_sensor_dat->led_activated;
	hx3695_write_reg(0X1c, watermark);
	hx3695_write_reg(0X5c, 0x02);
    hx3695_delay_ms(1);
    hx3695_write_reg(0X1b, 0x10);
    hx3695_delay_ms(1);
    hx3695_write_reg(0X1b, 0x91);
    hx3695_write_reg(0X5c, 0x00);
    read_fifo_first_flg = 1;
}

void hx3695_led_control(ppg_sensor_data_t *ppg_sensor_dat, uint8_t index)
{
	if(ppg_sensor_dat->led_data[index].led_state == Activating)
	{
		hx3695_write_reg(0X26+index,0xbb);
		ppg_sensor_dat->led_data[index].led_state = Activated;
	}
	else if(ppg_sensor_dat->led_data[index].led_state == InActivating)
	{
		hx3695_write_reg(0X26+index,0x33);
		ppg_sensor_dat->led_data[index].led_state = InActivated;
	}
}

void hx3695_updata_sensor(ppg_sensor_data_t *ppg_sensor_dat)
{
	uint8_t ii;
	hx3695_set_fs(ppg_sensor_dat);
	for(ii=0;ii<4;ii++)
	{
		hx3695_led_control(ppg_sensor_dat, ii);
	}
	hx3695_set_watermark(ppg_sensor_dat);
}

void hx3695_hrs_enable(ppg_sensor_data_t *ppg_sensor_dat)
{

	tyhx_hrs_alg_open();
	ppg_sensor_data.mode = HRS_MODE;
	hx3695_ppg_set_mode(PPG_INIT);
	#ifdef DOUBLE_GREEN
	ppg_sensor_dat->led_activated = 3;
	ppg_sensor_dat->fs = 100;
	ppg_sensor_dat->watermark = 30;
	ppg_sensor_dat->led_data[0].led_state = Activating;
	ppg_sensor_dat->led_data[1].led_state = Activating;
	ppg_sensor_dat->led_data[2].led_state = InActivating;
	ppg_sensor_dat->led_data[3].led_state = Activating;
	ppg_sensor_dat->led_data[0].isneed_cal_start = true;
	ppg_sensor_dat->led_data[1].isneed_cal_start = true;
	ppg_sensor_dat->led_data[2].isneed_cal_start = false;
	ppg_sensor_dat->led_data[3].isneed_cal_start = false;
	#else
	ppg_sensor_dat->led_activated = 2;
	ppg_sensor_dat->fs = 25;
	ppg_sensor_dat->watermark = 20;
	ppg_sensor_dat->led_data[0].led_state = Activating;
	ppg_sensor_dat->led_data[1].led_state = InActivating;
	ppg_sensor_dat->led_data[2].led_state = InActivating;
	ppg_sensor_dat->led_data[3].led_state = Activating;
	ppg_sensor_dat->led_data[0].isneed_cal_start = true;
	ppg_sensor_dat->led_data[1].isneed_cal_start = false;
	ppg_sensor_dat->led_data[2].isneed_cal_start = false;
	ppg_sensor_dat->led_data[3].isneed_cal_start = false;
	#endif
	hx3695_updata_sensor(ppg_sensor_dat);
}

void hx3695_hrsdaily_enable(ppg_sensor_data_t *ppg_sensor_dat)
{
	if (alg_ram == NULL)
	{
		return;
	}
	
	tyhx_hrs_alg_open();
	ppg_sensor_data.mode = HRSDAILY_MODE;
	hx3695_ppg_set_mode(PPG_INIT);
    
	ppg_sensor_dat->led_activated = 2;
	ppg_sensor_dat->fs = 25;
	ppg_sensor_dat->watermark = 20;
	ppg_sensor_dat->led_data[0].led_state = Activating;
	ppg_sensor_dat->led_data[1].led_state = InActivating;
	ppg_sensor_dat->led_data[2].led_state = InActivating;
	ppg_sensor_dat->led_data[3].led_state = Activating;
	ppg_sensor_dat->led_data[0].isneed_cal_start = true;
	ppg_sensor_dat->led_data[1].isneed_cal_start = false;
	ppg_sensor_dat->led_data[2].isneed_cal_start = false;
	ppg_sensor_dat->led_data[3].isneed_cal_start = false;

	hx3695_updata_sensor(ppg_sensor_dat);
	DEBUG_PRINTF("hx3695_hrs_enable\n");
}

void hx3695_spo2_enable(ppg_sensor_data_t *ppg_sensor_dat)
{

	tyhx_spo2_alg_open();
	ppg_sensor_data.mode = SPO2_MODE;
	hx3695_ppg_set_mode(PPG_INIT);
	ppg_sensor_dat->led_activated = 4;
	ppg_sensor_dat->fs = 25;
	ppg_sensor_dat->watermark = 40;
	ppg_sensor_dat->led_data[0].led_state = Activating;
	ppg_sensor_dat->led_data[1].led_state = Activating;
	ppg_sensor_dat->led_data[2].led_state = Activating;
	ppg_sensor_dat->led_data[3].led_state = Activating;
	ppg_sensor_dat->led_data[0].isneed_cal_start = true;
	ppg_sensor_dat->led_data[1].isneed_cal_start = true;
	ppg_sensor_dat->led_data[2].isneed_cal_start = true;
	ppg_sensor_dat->led_data[3].isneed_cal_start = false;
	hx3695_updata_sensor(ppg_sensor_dat);
}

void hx3695_hrv_enable(ppg_sensor_data_t *ppg_sensor_dat)
{   
	tyhx_hrv_alg_open();
	ppg_sensor_data.mode = HRV_MODE;
	hx3695_ppg_set_mode(PPG_INIT);
	ppg_sensor_dat->led_activated = 2;
	ppg_sensor_dat->fs = 100;
	ppg_sensor_dat->watermark = 50;
	ppg_sensor_dat->led_data[0].led_state = Activating;
	ppg_sensor_dat->led_data[1].led_state = InActivating;
	ppg_sensor_dat->led_data[2].led_state = InActivating;
	ppg_sensor_dat->led_data[3].led_state = Activating;
	ppg_sensor_dat->led_data[0].isneed_cal_start = true;
	ppg_sensor_dat->led_data[1].isneed_cal_start = false;
	ppg_sensor_dat->led_data[2].isneed_cal_start = false;
	ppg_sensor_dat->led_data[3].isneed_cal_start = false;
	hx3695_updata_sensor(ppg_sensor_dat);
}

void hx3695_wear_low_power(ppg_sensor_data_t *ppg_sensor_dat)
{
	ppg_sensor_data.mode = WEAR_MODE;
	hx3695_ppg_set_mode(PPG_INIT);
	ppg_sensor_dat->led_activated = 1;
	ppg_sensor_dat->fs = 5;
	ppg_sensor_dat->watermark = 5;
	ppg_sensor_dat->led_data[0].led_state = InActivating;
	ppg_sensor_dat->led_data[1].led_state = InActivating;
	ppg_sensor_dat->led_data[2].led_state = InActivating;
	ppg_sensor_dat->led_data[3].led_state = Activating;
	ppg_sensor_dat->led_data[0].isneed_cal_start = false;
	ppg_sensor_dat->led_data[1].isneed_cal_start = false;
	ppg_sensor_dat->led_data[2].isneed_cal_start = false;
	ppg_sensor_dat->led_data[3].isneed_cal_start = false;
	hx3695_updata_sensor(ppg_sensor_dat);
}

void hx3695_ppg_set_mode(uint8_t mode_cmd)
{
    switch (mode_cmd)
    {
		case PPG_INIT:
			hx3695_ppg_para_init(&ppg_sensor_data);
        	s_ppg_state = 1;
			s_cal_state = 0;
			#if defined(TIMMER_MODE)
			hx3695_320ms_timer_cfg(true);       // heart_rate_meas_timeout_handler
			#else
			hx3695_gpioint_cfg(true);
			#endif
			DEBUG_PRINTF("ppg init mode");
			break;

		case PPG_OFF:
			#if defined(TIMMER_MODE)
			hx3695_320ms_timer_cfg(false);
			hx3695_40ms_timer_cfg(false);
			#elif defined(INT_MODE)
			hx3695_gpioint_cfg(false);
			#endif
			hx3695_ppg_off();
			s_ppg_state = 0;
			s_cal_state = 0;
			DEBUG_PRINTF("ppg off mode");
			break;

		case PPG_LED_OFF:
			hx3695_wear_low_power(&ppg_sensor_data);
			s_ppg_state = 1;
			s_cal_state = 0;
			DEBUG_PRINTF("ppg led off mode");
			break;

		case CAL_INIT:
			hx3695_cal_init();
        	s_ppg_state = 0;
			s_cal_state = 1;
			#if defined(TIMMER_MODE)
            hx3695_320ms_timer_cfg(false);
			hx3695_40ms_timer_cfg(true);
			#endif
			DEBUG_PRINTF("cal init mode");
			break;

		case RECAL_INIT:
			hx3695_recal_init();
        	s_ppg_state = 0;
			s_cal_state = 1;
			#if defined(TIMMER_MODE)
            hx3695_320ms_timer_cfg(false);
            hx3695_40ms_timer_cfg(true);
			#endif
			DEBUG_PRINTF("recal init mode");
			break;

		case CAL_OFF:
			#if defined(TIMMER_MODE)
            hx3695_320ms_timer_cfg(true);
            hx3695_40ms_timer_cfg(false);
			#endif
			hx3695_cal_off();
			s_ppg_state = 1;
			s_cal_state = 0;
			DEBUG_PRINTF("cal off mode");
			break;

		default:
			break;
    }
}

bool hx3695_init(WORK_MODE_T mode)
{
    work_mode_flag = mode;
    // hx3695_software_reset();
    hx3695_ppg_on();
	if (!hx3695_chip_check())
	{
		return false;
	}
    switch (work_mode_flag)
    {
		case HRS_MODE:
			tyhx_hrs_set_alg_para(hrs_sports_mode, static_thre_val, gsen_lv_val);
			hx3695_hrs_enable(&ppg_sensor_data);
			break;
        
     case HRSDAILY_MODE:
			hx3695_hrsdaily_enable(&ppg_sensor_data);
			break;

		case LIVING_MODE:

			break;

		case SPO2_MODE:
			hx3695_spo2_enable(&ppg_sensor_data);
			tyhx_spo2_para_usuallyadjust(SPO2_LOW_XCORR_THRE, SPO2_LOW_SNR_THRE, COUNT_BLOCK_NUM, SPO2_BASE_LINE_INIT, SPO2_SLOPE, SPO2_GSEN_POW_THRE);
			tyhx_spo2_para_barelychange(MEAN_NUM, SOP2_DEGLITCH_THRE, SPO2_REMOVE_JUMP_THRE, SPO2_LOW_CLIP_END_TIME, SPO2_LOW_CLIP_DN, SPO2_NORMAL_CLIP_DN,
										IR_AC_TOUCH_THRE, IR_FFT_POW_THRE, SPO2_CALI, SLOPE_PARA_MAX, SLOPE_PARA_MIN);

			break;

		case HRV_MODE:
			hx3695_hrv_enable(&ppg_sensor_data);
			break;

		case WEAR_MODE:
			hx3695_ppg_set_mode(PPG_LED_OFF);
			break;

		case FT_LEAK_LIGHT_MODE:
			if (!hx3695_chip_check())
			{
				return false;
			}
			hx3695_factroy_test(LEAK_LIGHT_TEST);
			return true;
			break;

		case FT_GRAY_CARD_MODE:
			if (!hx3695_chip_check())
			{
				return false;
			}
			hx3695_factroy_test(GRAY_CARD_TEST);
		  return true;
			break;

		case FT_INT_TEST_MODE:
			//hx3695_factroy_test(FT_INT_TEST);
			break;

		case FT_SINGLE_CHECK_MODE:
			//hx3695_factroy_test(SINGLE_CHECK_TEST);
			break;

		case FT_LED_OFF_MODE:
			//hx3695_factroy_test(LED_OFF_TEST);
			break;

		case FT_WEAR_MODE:
			//hx3695_factroy_test(WEAR_MODE_TEST);
			break;

		default:
			break;
    }
	
	hx3695_agc_Int_handle(&ppg_sensor_data);
    return true;
}

void hx3695_agc_Int_handle(ppg_sensor_data_t *ppg_sensor_dat)
{
	uint8_t ii = 0;
	if(s_ppg_state==1 && s_cal_state==0)
	{
		for(ii=0;ii<4;ii++)
		{
			if(ppg_sensor_dat->led_data[ii].isneed_cal_start == true)
			{
				hx3695_ppg_set_mode(CAL_INIT);
				return;
			}
		}
	}
	hx3695_agc_process(ppg_sensor_dat);
}

void agc_timeout_handler(void *p_context)
{
    hx3695_agc_Int_handle(&ppg_sensor_data);
}

void heart_rate_meas_timeout_handler(void *p_context)
{
    hx3695_ppg_Int_handle(&ppg_sensor_data);
    hx3695_agc_Int_handle(&ppg_sensor_data);
}

void nrf_gpioint_handle(void)
{
	hx3695_ppg_Int_handle(&ppg_sensor_data);
    hx3695_agc_Int_handle(&ppg_sensor_data);
}

void hx3695_alg_send_data(ppg_sensor_data_t *ppg_sensor_dat, WORK_MODE_T mode, uint8_t data_count)
{
	uint16_t gcc_len = 8;
	hrs_results_t hrs_result = {MSG_HRS_ALG_NOT_OPEN,MSG_LIVING_INITIAL,0,0,0,0,0,NORMAL_MODE,0,0};
	spo2_results_t spo2_result = {MSG_SPO2_ALG_NOT_OPEN,MSG_SPO2_LIVING_INITIAL,0,0,0,0,0,0,0,0,0,0,0};
	hrv_results_t hrv_alg_results = {MSG_HRV_ALG_NOT_OPEN,0,0,0,0,0,0,0,0};

	switch(mode)
	{
		case HRS_MODE:
		{
			tyhx_acc_send_data(gsen_data_x_fifo, gsen_data_y_fifo, gsen_data_z_fifo, gcc_len, 25);
			tyhx_hrs_alg_send_data(ppg_sensor_dat->led_data[0].data_val, ppg_sensor_dat->led_data[1].data_val, data_count, 25);
			
			//tyhx_hrs_alg_send_data(ppg_sensor_dat->led_data[0].data_val,data_count, gsen_data_x_fifo, gsen_data_y_fifo, gsen_data_z_fifo, ppg_sensor_dat->led_data[0].offset_idac);
			//tyhx_hrs_alg_send_data(ppg_sensor_dat->led_data[0].data_val,data_count, gsen_data_x_fifo, gsen_data_y_fifo, gsen_data_z_fifo);
			hrs_result = tyhx_hrs_alg_get_results();
			g_hr_data.hr_result = hrs_result.hr_result;
			DEBUG_PRINTF("HRS: %d %d",hrs_result.data_cnt, hrs_result.hr_result);
			break;
		}
		case SPO2_MODE:
		{
			tyhx_spo2_alg_send_data(ppg_sensor_dat->led_data[1].data_val, ppg_sensor_dat->led_data[2].data_val, ppg_sensor_dat->led_data[0].data_val, \
			ppg_sensor_dat->led_data[1].offset_idac, ppg_sensor_dat->led_data[2].offset_idac,ppg_sensor_dat->led_data[0].offset_idac, \
			gsen_data_x_fifo, gsen_data_y_fifo, gsen_data_z_fifo, data_count);
			spo2_result = tyhx_spo2_alg_get_results();
			g_hr_data.spo2_result = spo2_result.spo2_result;
			g_hr_data.hr_result = spo2_result.hr_result;
			DEBUG_PRINTF("count:%d HRS: %d SPO2: %d",spo2_result.data_cnt, spo2_result.hr_result, spo2_result.spo2_result);
			break;
		}
		case HRV_MODE:
		{
			hrv_alg_results=tyhx_hrv_alg_send_bufdata(ppg_sensor_dat->led_data[0].data_val, data_count,gsen_data_x_fifo,gsen_data_y_fifo, gsen_data_z_fifo);
			g_hr_data.hrv_result = hrv_alg_results.hrv_result;
			DEBUG_PRINTF("data_cnt:%d hrv_result:%d \r\n", hrv_alg_results.data_cnt,hrv_alg_results.hrv_result);
			break;
		}
		default:
		{
			break;
		}
	}
}


void hx3695_ppg_Int_handle(ppg_sensor_data_t *ppg_sensor_dat)
{
	uint16_t count = 0;
	uint16_t ii = 0;
	if(s_ppg_state == 0)
	{
		return;
	}
	count = hx3695_read_fifo_data(ppg_sensor_dat);
	DEBUG_PRINTF("count:%d", count);
    if(count > 0)
    {
        hx3695_check_wear(ppg_sensor_dat);
		g_hr_data.wear_status = wear_status;
        if(wear_status == MSG_WEAR)
        {
			gsensor_xyz_info xyz_data[count];
			tjd_service_gs_get_xyz_fifo(GSEN_SERVICE_HR, xyz_data, count);
			for (int i=0; i<count; i++) {
				gsen_data_x_fifo[i] = xyz_data[i].gsensor_x.raw_data;
				gsen_data_y_fifo[i] = xyz_data[i].gsensor_y.raw_data;
				gsen_data_z_fifo[i] = xyz_data[i].gsensor_z.raw_data;
			}
			// for (int i=0; i<count; i++) {
			// 	DEBUG_PRINTF("gsen_data_xyz: %d, %d, %d", gsen_data_x_fifo[i], gsen_data_y_fifo[i], gsen_data_z_fifo[i]);
			// }
            hx3695_alg_send_data(ppg_sensor_dat,work_mode_flag,count);
        }
    }

	for(ii=0;ii<count;ii++)
	{
		DEBUG_PRINTF("hxdata:%d/%d, %d, %d, %d, %d, %d, %d, %d, %d, %d, %d", ii+1, count, ppg_sensor_dat->led_data[0].data_val[ii], ppg_sensor_dat->led_data[1].data_val[ii],ppg_sensor_dat->led_data[2].data_val[ii], ppg_sensor_dat->led_data[3].data_val[ii],  \
		ppg_sensor_dat->led_data[0].led_current, ppg_sensor_dat->led_data[0].offset_idac, ppg_sensor_dat->led_data[1].led_current, ppg_sensor_dat->led_data[1].offset_idac,ppg_sensor_dat->led_data[2].led_current, ppg_sensor_dat->led_data[2].offset_idac);
	}
}