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

#include "hx3695.h"
#include "hx3695_agc.h"
#include "tyhx_spo2_alg.h"
#include "tyhx_hrs_alg.h"

extern uint8_t read_fifo_first_flg;
extern uint8_t s_ppg_state;
extern uint8_t s_cal_state;
static int32_t agc_buf[8] = {0};
static agc_set_t calReg;

void hx3695_agc_reset(void)
{
    calReg.STATE = sCalStart;
    calReg.FLAG = CAL_FLG_LED_DR|CAL_FLG_LED_DAC|CAL_FLG_AMB_DAC|CAL_FLG_RF;

    calReg.LED0_IDAC = 0;
    calReg.LED1_IDAC = 0;
    calReg.LED2_IDAC = 0;
    calReg.LED3_IDAC = 0;
    calReg.AMB0_IDAC = 0;
    calReg.AMB1_IDAC = 0;
    calReg.AMB2_IDAC = 0;
    calReg.AMB3_IDAC = 0;

    calReg.LED0_RF = 0;
    calReg.LED1_RF = 0;
    calReg.LED2_RF = 0;
    calReg.LED3_RF = 0;

    calReg.LED0_CUR = 0;
    calReg.LED1_CUR = 0;
    calReg.LED2_CUR = 0;
    calReg.LED3_CUR = 0;

	calReg.CAL_DELAY_CNT = 0;
}

void Init_PPG_Calibration_Routine(agc_set_t *calR)
{
    calR->STATE = sCalStart;
    calR->FLAG = CAL_FLG_LED_DR|CAL_FLG_LED_DAC|CAL_FLG_AMB_DAC|CAL_FLG_RF;

	if(AGC_SWI_LED0)
	{
		calR->LED0_IDAC = 0;
		calR->AMB0_IDAC = 0;
		calR->LED0_RF = 0;
		calR->LED0_CUR = AGC_CUR_INIT_LED0;
	}
	if(AGC_SWI_LED1)
	{
		calR->LED1_IDAC = 0;
		calR->AMB1_IDAC = 0;
		calR->LED1_RF = 0;
		calR->LED1_CUR = AGC_CUR_INIT_LED1;
	}
	if(AGC_SWI_LED2)
	{
		calR->LED2_IDAC = 0;
		calR->AMB2_IDAC = 0;
		calR->LED2_RF = 0;
		calR->LED2_CUR = AGC_CUR_INIT_LED2;
	}
	if(AGC_SWI_LED3)
	{
		calR->LED3_IDAC = 0;
		calR->AMB3_IDAC = 0;
		calR->LED3_RF = 0;
		calR->LED3_CUR = AGC_CUR_INIT_LED3;
	}
}

void Restart_PPG_Calibration_Routine(agc_set_t *calR)
{
    calR->FLAG = CAL_FLG_LED_DAC;
    calR->STATE = sCalOffset1;
}

void PPG_Calibration_Routine(ppg_sensor_data_t *ppg_sensor_dat, agc_set_t *calR, int32_t amb0_val, int32_t led0_val, \
    int32_t amb1_val, int32_t led1_val, int32_t amb2_val, int32_t led2_val, int32_t amb3_val, int32_t led3_val)
{
    int32_t led0_cur_temp = 0;
    int32_t led1_cur_temp = 0;
    int32_t led2_cur_temp = 0;
    int32_t led3_cur_temp = 0;

    switch(calR->STATE)
    {
        case sCalStart:
        {
            calR->STATE = sCalCur;
            break;
        }
        case sCalCur:
        {
            if(AGC_SWI_LED0 && ppg_sensor_dat->led_data[0].isneed_cal_start==1)
            {
                ppg_sensor_dat->led_data[0].isneed_cal_start = 0;
                if(amb0_val > 535000)
                {
                    calR->AMB0_IDAC = (amb0_val - 535000)/1250;
                }
                else
                {
                    calR->AMB0_IDAC = 0;
                }
                if(led0_val > (amb0_val+256))
                {
                    calR->LED0_STEP = (led0_val - amb0_val) / AGC_CUR_INIT_LED0;
                    led0_cur_temp = 1250 * (AGC_IDAC_TARGET_LED0 + calR->AMB0_IDAC) / calR->LED0_STEP;

                    if(led0_cur_temp > AGC_CUR_MAX_LED0)
                    {
                        calR->LED0_CUR = AGC_CUR_MAX_LED0;
                    }
                    else
                    {
                        calR->LED0_CUR = led0_cur_temp;
                    }

                    if(calR->LED0_CUR != AGC_CUR_MAX_LED0)
                    {
                        calR->LED0_IDAC = (AGC_IDAC_TARGET_LED0*3)>>2;
                    }
                    else
                    {
                        calR->LED0_IDAC = AGC_IDAC_TARGET_LED0>>1;
                    }
                }
                calR->LED0_RF = AGC_RF_LED0;
            }
			#ifdef DOUBLE_GREEN
			if(ppg_sensor_dat->mode == HRS_MODE)
			{
				if(AGC_SWI_LED1 && ppg_sensor_dat->led_data[1].isneed_cal_start==1)
				{
						ppg_sensor_dat->led_data[1].isneed_cal_start = 0;
						if(amb1_val > 535000)
						{
								calR->AMB1_IDAC = (amb1_val - 535000)/1250;
						}
						else
						{
								calR->AMB1_IDAC = 0;
						}
						if(led1_val > (amb1_val+256))
						{
								calR->LED1_STEP = (led1_val - amb1_val) / AGC_CUR_INIT_LED1;
								led1_cur_temp = 1250 * (AGC_IDAC_TARGET_LED1N + calR->AMB1_IDAC) / calR->LED1_STEP;

								if(led1_cur_temp > AGC_CUR_MAX_LED1N)
								{
										calR->LED1_CUR = AGC_CUR_MAX_LED1N;
								}
								else
								{
										calR->LED1_CUR = led1_cur_temp;
								}

								if(calR->LED1_CUR != AGC_CUR_MAX_LED1N)
								{
										calR->LED1_IDAC = (AGC_IDAC_TARGET_LED1N*3)>>2;
								}
								else
								{
										calR->LED1_IDAC = AGC_IDAC_TARGET_LED1N>>1;
								}
						}
						calR->LED1_RF = AGC_RF_LED1N;
				}
			}
			else
			#endif
			{
				if(AGC_SWI_LED1 && ppg_sensor_dat->led_data[1].isneed_cal_start==1)
				{
						ppg_sensor_dat->led_data[1].isneed_cal_start = 0;
						if(amb1_val > 535000)
						{
								calR->AMB1_IDAC = (amb1_val - 535000)/1250;
						}
						else
						{
								calR->AMB1_IDAC = 0;
						}
						if(led1_val > (amb1_val+256))
						{
								calR->LED1_STEP = (led1_val - amb1_val) / AGC_CUR_INIT_LED1;
								led1_cur_temp = 1250 * (AGC_IDAC_TARGET_LED1 + calR->AMB1_IDAC) / calR->LED1_STEP;

								if(led1_cur_temp > AGC_CUR_MAX_LED1)
								{
										calR->LED1_CUR = AGC_CUR_MAX_LED1;
								}
								else
								{
										calR->LED1_CUR = led1_cur_temp;
								}

								if(calR->LED1_CUR != AGC_CUR_MAX_LED1)
								{
										calR->LED1_IDAC = (AGC_IDAC_TARGET_LED1*3)>>2;
								}
								else
								{
										calR->LED1_IDAC = AGC_IDAC_TARGET_LED1>>1;
								}
						}
						calR->LED1_RF = AGC_RF_LED1;
				}
			}

            if(AGC_SWI_LED2 && ppg_sensor_dat->led_data[2].isneed_cal_start==1)
            {
                ppg_sensor_dat->led_data[2].isneed_cal_start = 0;
                if(amb2_val > 535000)
                {
                    calR->AMB2_IDAC = (amb2_val - 535000)/1250;
                }
                else
                {
                    calR->AMB2_IDAC = 0;
                }
                if(led2_val > (amb2_val+256))
                {
                    calR->LED2_STEP = (led2_val - amb2_val) / AGC_CUR_INIT_LED2;
                    led2_cur_temp = 1250 * (AGC_IDAC_TARGET_LED2 + calR->AMB2_IDAC) / calR->LED2_STEP;

                    if(led2_cur_temp > AGC_CUR_MAX_LED2)
                    {
                        calR->LED2_CUR = AGC_CUR_MAX_LED2;
                    }
                    else
                    {
                        calR->LED2_CUR = led2_cur_temp;
                    }

                    if(calR->LED2_CUR != AGC_CUR_MAX_LED2)
                    {
                        calR->LED2_IDAC = (AGC_IDAC_TARGET_LED2*3)>>2;
                    }
                    else
                    {
                        calR->LED2_IDAC = AGC_IDAC_TARGET_LED2>>1;
                    }
                }
                calR->LED2_RF = AGC_RF_LED2;
            }

            if(AGC_SWI_LED3 && ppg_sensor_dat->led_data[3].isneed_cal_start==1)
            {
                ppg_sensor_dat->led_data[3].isneed_cal_start = 0;
                if(amb3_val > 535000)
                {
                    calR->AMB3_IDAC = (amb3_val - 535000)/1250;
                }
                else
                {
                    calR->AMB3_IDAC = 0;
                }
                if(led3_val > (amb3_val+256))
                {
					calR->LED3_STEP = (led3_val - amb3_val) / AGC_CUR_INIT_LED3;
                    led3_cur_temp = 1250 * (AGC_IDAC_TARGET_LED3 + calR->AMB3_IDAC) / calR->LED3_STEP;

                    if(led3_cur_temp > AGC_CUR_MAX_LED3)
                    {
                        calR->LED3_CUR = AGC_CUR_MAX_LED3;
                    }
                    else
                    {
                        calR->LED3_CUR = led3_cur_temp;
                    }

                    if(calR->LED3_CUR != AGC_CUR_MAX_LED3)
                    {
                        calR->LED3_IDAC = (AGC_IDAC_TARGET_LED3*3)>>2;
                    }
                    else
                    {
                        calR->LED3_IDAC =AGC_IDAC_TARGET_LED3>>1;
                    }
                }
                calR->LED3_RF = AGC_RF_LED3;
            }

			calR->FLAG = CAL_FLG_LED_DR|CAL_FLG_LED_DAC|CAL_FLG_AMB_DAC|CAL_FLG_RF;
            calR->STATE = sCalOffset1;
            break;
        }
        case sCalOffset1:
        {
            if(AGC_SWI_LED0 && ppg_sensor_dat->led_data[0].led_state == Activated)
            {
                if(led0_val>850000 && calR->LED0_IDAC < 240)
                {
                   calR->LED0_IDAC = calR->LED0_IDAC+10;
                }
            }
            if(AGC_SWI_LED1 && ppg_sensor_dat->led_data[1].led_state == Activated)
            {
                if(led1_val>850000 && calR->LED1_IDAC < 240)
                {
                   calR->LED1_IDAC = calR->LED1_IDAC+10;
                }
            }
            if(AGC_SWI_LED2 && ppg_sensor_dat->led_data[2].led_state == Activated)
            {
                if(led2_val>850000 && calR->LED2_IDAC < 240)
                {
                   calR->LED2_IDAC = calR->LED2_IDAC+10;
                }
            }
            if(AGC_SWI_LED3 && ppg_sensor_dat->led_data[3].led_state == Activated)
            {
                if(led3_val>850000 && calR->LED3_IDAC < 240)
                {
                   calR->LED3_IDAC = calR->LED3_IDAC+10;
                }
            }

            if ((!AGC_SWI_LED0 || ppg_sensor_dat->led_data[0].led_state == InActivated || led0_val<=850000) \
			  &&(!AGC_SWI_LED1 || ppg_sensor_dat->led_data[1].led_state == InActivated || led1_val<=850000) \
              &&(!AGC_SWI_LED2 || ppg_sensor_dat->led_data[2].led_state == InActivated || led2_val<=850000) \
			  &&(!AGC_SWI_LED3 || ppg_sensor_dat->led_data[3].led_state == InActivated || led3_val<=850000) )
            {
                calR->STATE = sCalOffset2;
            }
            else if(calR->LED0_IDAC >= 240 || calR->LED1_IDAC >= 240 || calR->LED2_IDAC >= 240 || calR->LED3_IDAC >= 240)
            {
                calR->STATE = sCalErr;
            }
            else
            {
              calR->STATE = sCalOffset1;
            }
            calR->FLAG = CAL_FLG_LED_DAC;
            break;
        }
        case sCalOffset2:
        {
            if(AGC_SWI_LED0 && ppg_sensor_dat->led_data[0].led_state == Activated)
            {
                if(led0_val > 650000 && calR->LED0_IDAC < 240)
                {
                   calR->LED0_IDAC = calR->LED0_IDAC+5;
                }
                else if(led0_val <450000 && calR->LED0_IDAC >= 5)
                {
                   calR->LED0_IDAC = calR->LED0_IDAC-5;
                }
                else if(led0_val<450000 && calR->LED0_IDAC > 0)
                {
                   calR->LED0_IDAC = calR->LED0_IDAC-1;
                }
            }
            if(AGC_SWI_LED1 && ppg_sensor_dat->led_data[1].led_state == Activated)
            {
                if(led1_val > 650000 && calR->LED1_IDAC < 240)
                {
                   calR->LED1_IDAC = calR->LED1_IDAC+5;
                }
                else if(led1_val <450000 && calR->LED1_IDAC >= 5)
                {
                   calR->LED1_IDAC = calR->LED1_IDAC-5;
                }
                else if(led1_val<450000 && calR->LED1_IDAC > 0)
                {
                   calR->LED1_IDAC = calR->LED1_IDAC-1;
                }
            }
            if(AGC_SWI_LED2 && ppg_sensor_dat->led_data[2].led_state == Activated)
            {
                if(led2_val > 650000 && calR->LED2_IDAC < 240)
                {
                   calR->LED2_IDAC = calR->LED2_IDAC+5;
                }
                else if(led2_val <450000 && calR->LED2_IDAC >= 5)
                {
                   calR->LED2_IDAC = calR->LED2_IDAC-5;
                }
                else if(led2_val<450000 && calR->LED2_IDAC > 0)
                {
                   calR->LED2_IDAC = calR->LED2_IDAC-1;
                }
            }
            if(AGC_SWI_LED3 && ppg_sensor_dat->led_data[3].led_state == Activated)
            {
                if(led3_val > 650000 && calR->LED3_IDAC < 240)
                {
                   calR->LED3_IDAC = calR->LED3_IDAC+5;
                }
                else if(led3_val <450000 && calR->LED3_IDAC >= 5)
                {
                   calR->LED3_IDAC = calR->LED3_IDAC-5;
                }
                else if(led3_val<450000 && calR->LED3_IDAC > 0)
                {
                   calR->LED3_IDAC = calR->LED3_IDAC-1;
                }
            }

            if ((!AGC_SWI_LED0 || ppg_sensor_dat->led_data[0].led_state == InActivated || (led0_val>=450000 && led0_val<=650000))\
              &&(!AGC_SWI_LED1 || ppg_sensor_dat->led_data[1].led_state == InActivated || (led1_val>=450000 && led1_val<=650000))\
              &&(!AGC_SWI_LED2 || ppg_sensor_dat->led_data[2].led_state == InActivated || (led2_val>=450000 && led2_val<=650000))\
              &&(!AGC_SWI_LED3 || ppg_sensor_dat->led_data[3].led_state == InActivated || (led3_val>=450000 && led3_val<=650000)))
            {
                calR->STATE = sCalFinish;
            }
            else if(calR->LED0_IDAC >= 240 || calR->LED1_IDAC >= 240 || calR->LED2_IDAC >= 240 || calR->LED3_IDAC >= 240)
            {
                calR->STATE = sCalErr;
            }
            else
            {
                calR->STATE = sCalOffset2;
                // calR->STATE = sCalOffset2;
            }
            calR->FLAG = CAL_FLG_LED_DAC;
            break;
        }
        case sCalErr:
        {
            calR->STATE = sCalFinish;
            break;
        }
        default:
        {
            break;
        }

    }
    hx3695_agc_updata_reg(calR);
    // DEBUG_PRINTF("AGC:cur=%d,%d,%d,%d, idac=%d,%d,%d,%d, step=%d,%d,%d,%d, state=%d \r\n",\
    //     calR->LED0_CUR,calR->LED1_CUR,calR->LED2_CUR,calR->LED3_CUR,calR->LED0_IDAC,calR->LED1_IDAC,calR->LED2_IDAC,calR->LED3_IDAC, \
    //     calR->LED0_STEP,calR->LED1_STEP,calR->LED2_STEP,calR->LED3_STEP,calR->STATE);
}

void hx3695_agc_process(ppg_sensor_data_t *ppg_sensor_dat)
{
    if (!s_cal_state && s_ppg_state)
    {
        hx3695_read_led_data(agc_buf);
        
        if ((AGC_SWI_LED0 && ppg_sensor_dat->led_data[0].led_state==Activated && (agc_buf[0]<250000 || agc_buf[0]>850000))\
          ||(AGC_SWI_LED1 && ppg_sensor_dat->led_data[1].led_state==Activated && (agc_buf[1]<250000 || agc_buf[1]>850000))\
          ||(AGC_SWI_LED2 && ppg_sensor_dat->led_data[2].led_state==Activated && (agc_buf[2]<250000 || agc_buf[2]>850000))\
          ||(AGC_SWI_LED3 && ppg_sensor_dat->led_data[3].led_state==Activated && (agc_buf[3]<250000 || agc_buf[3]>850000)))
        {
            calReg.CAL_DELAY_CNT++;
            if(calReg.CAL_DELAY_CNT >= AGC_DELAY_COUNT_SET)
            {
                hx3695_ppg_set_mode(RECAL_INIT);
            }
        }
        else
        {
            calReg.CAL_DELAY_CNT = 0;
        }
        return;
    }

    hx3695_read_phase_data(agc_buf);
    PPG_Calibration_Routine(ppg_sensor_dat,&calReg,agc_buf[0],agc_buf[1],agc_buf[2],agc_buf[3],agc_buf[4],agc_buf[5],agc_buf[6],agc_buf[7]);

    if (calReg.STATE == sCalFinish)
    {
        hx3695_ppg_set_mode(CAL_OFF);
    }
}

void hx3695_cal_init(void)
{
    hx3695_write_reg(0x1b,0x10);
    hx3695_write_reg(0x38,0x01);
    hx3695_write_reg(0x60,0x34);

    hx3695_write_reg(0x5c,0x02);
    hx3695_delay_ms(1);
    hx3695_write_reg(0x5c,0x00);
	hx3695_delay_ms(1);
    
    Init_PPG_Calibration_Routine(&calReg);
    hx3695_agc_updata_reg(&calReg);
}

void hx3695_recal_init(void)
{
    hx3695_write_reg(0x1b,0x10);
    hx3695_write_reg(0x38,0x01);
    hx3695_write_reg(0x60,0x34);

    hx3695_write_reg(0x5c,0x02);
    hx3695_delay_ms(1);
    hx3695_write_reg(0x5c,0x00);
	hx3695_delay_ms(1);
    
    Restart_PPG_Calibration_Routine(&calReg);
    hx3695_agc_updata_reg(&calReg);
}

void hx3695_cal_off(void)
{
    hx3695_write_reg(0x38,0x04);
    hx3695_write_reg(0x60,0x14);

    hx3695_write_reg(0x5c,0x02);
    hx3695_delay_ms(1);
    hx3695_write_reg(0x1b,0x91);
    hx3695_write_reg(0x5c,0x00);
	hx3695_delay_ms(1);
    read_fifo_first_flg = 1;
}

void hx3695_agc_updata_reg(agc_set_t *calR)
{
    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;

    if (calR->FLAG & CAL_FLG_LED_DR)
    {
		if(AGC_SWI_LED0)
		{
			hx3695_write_reg(0X43, phase2_mux_sel);
			hx3695_write_reg(0X45, calR->LED0_CUR/2);
			hx3695_write_reg(0X70, calR->LED0_CUR/2);
		}
		if(AGC_SWI_LED1)
		{
			hx3695_write_reg(0X43, phase4_mux_sel);
			hx3695_write_reg(0X45, calR->LED1_CUR/2);
			hx3695_write_reg(0X70, calR->LED1_CUR/2);
		}
		if(AGC_SWI_LED2)
		{
			hx3695_write_reg(0X43, phase6_mux_sel);
			hx3695_write_reg(0X45, calR->LED2_CUR);
		}
		if(AGC_SWI_LED3)
		{
			hx3695_write_reg(0X43, phase8_mux_sel);
			hx3695_write_reg(0X45, calR->LED3_CUR);
		}
    }

    if (calR->FLAG & CAL_FLG_LED_DAC)
    {
		if(AGC_SWI_LED0)
		{
			hx3695_write_reg(0X43, phase2_mux_sel);
			hx3695_write_reg(0X48, calR->LED0_IDAC);
		}
		if(AGC_SWI_LED1)
		{
			hx3695_write_reg(0X43, phase4_mux_sel);
			hx3695_write_reg(0X48, calR->LED1_IDAC);
		}
		if(AGC_SWI_LED2)
		{
			hx3695_write_reg(0X43, phase6_mux_sel);
			hx3695_write_reg(0X48, calR->LED2_IDAC);
		}
		if(AGC_SWI_LED3)
		{
			hx3695_write_reg(0X43, phase8_mux_sel);
			hx3695_write_reg(0X48, calR->LED3_IDAC);
		}
    }

    if (calR->FLAG & CAL_FLG_AMB_DAC)
    {
		if(AGC_SWI_LED0)
		{
			hx3695_write_reg(0X43, phase1_mux_sel);
			hx3695_write_reg(0X48, calR->AMB0_IDAC);
		}
		if(AGC_SWI_LED1)
		{
			hx3695_write_reg(0X43, phase3_mux_sel);
			hx3695_write_reg(0X48, calR->AMB1_IDAC);
		}
		if(AGC_SWI_LED2)
		{
			hx3695_write_reg(0X43, phase5_mux_sel);
			hx3695_write_reg(0X48, calR->AMB2_IDAC);
		}
		if(AGC_SWI_LED3)
		{
			hx3695_write_reg(0X43, phase7_mux_sel);
			hx3695_write_reg(0X48, calR->AMB3_IDAC);
		}
    }

    if (calR->FLAG & CAL_FLG_RF)
    {
		if(AGC_SWI_LED0)
		{
			hx3695_write_reg(0X43, phase1_mux_sel);
			hx3695_write_reg(0X44, (0x40 | calR->LED0_RF));
			hx3695_write_reg(0X43, phase2_mux_sel);
			hx3695_write_reg(0X44, (0x40 | calR->LED0_RF));
		}
		if(AGC_SWI_LED1)
		{
			hx3695_write_reg(0X43, phase3_mux_sel);
			hx3695_write_reg(0X44, (0x40 | calR->LED1_RF));
			hx3695_write_reg(0X43, phase4_mux_sel);
			hx3695_write_reg(0X44, (0x40 | calR->LED1_RF));
		}
		if(AGC_SWI_LED2)
		{
			hx3695_write_reg(0X43, phase5_mux_sel);
			hx3695_write_reg(0X44, (0x40 | calR->LED2_RF));
			hx3695_write_reg(0X43, phase6_mux_sel);
			hx3695_write_reg(0X44, (0x40 | calR->LED2_RF));
		}
		if(AGC_SWI_LED3)
		{
			hx3695_write_reg(0X43, phase7_mux_sel);
			hx3695_write_reg(0X44, (0x40 | calR->LED3_RF));
			hx3695_write_reg(0X43, phase8_mux_sel);
			hx3695_write_reg(0X44, (0x40 | calR->LED3_RF));
		}
    }
}