#include "pll.h"

typedef struct
{
    UINT16   rank;      //!<IIR滤波器的阶数
    UINT16   rankadd;
    FLOAT32 *pcoeffan;  //!<IIR滤波器输入各阶系数的地址
    FLOAT32 *pcoeffbn;  //!<IIR滤波器输出各阶系数的地址
    FLOAT32 *pxn;       //!<采样输入的地址
    FLOAT32 *pyn;       //!<滤波输出数组的地址
    UINT16   index;     //!<当前的滤波索引值
}IIR_OBJ;

#ifdef _FLASH
#pragma CODE_SECTION(IirCfg,".TI.ramfunc");
#pragma CODE_SECTION(IirCalc,".TI.ramfunc");
#pragma CODE_SECTION(DigitalFilterCfg,".TI.ramfunc");
#pragma CODE_SECTION(DigitalFilterCalc,".TI.ramfunc");
#pragma CODE_SECTION(PidCfg,".TI.ramfunc");
#pragma CODE_SECTION(PidCalc,".TI.ramfunc");
#pragma CODE_SECTION(ABC2d,".TI.ramfunc");
#pragma CODE_SECTION(ABC2q,".TI.ramfunc");
#pragma CODE_SECTION(ABC2dq,".TI.ramfunc");
#pragma CODE_SECTION(PLLCfg,".TI.ramfunc");
#pragma CODE_SECTION(PLLCalc,".TI.ramfunc");
#endif
IIR_OBJ iir_obj = {0};
FLOAT32 iir_buf[6][2] = {0};
/**
 * @brief  IIR配置函数       H(Z) = Y(Z)/X(Z) = ∑(0 to n)(an * Z^-n) / ∑(0 to n)(bn * Z^-n)
 *         即Yn = a0*X(0) + a1*X(-1) + ... + an*X(-n) - (b1*Y(-1) + b2*Y(-2) + ... + bn*Y(-n))
 * @param  rank             IIR的阶数n (UINT16)
 * @param  coeffan          输入的各阶系数an(FLOAT32型数组)
 * @param  coeffbn          输出的各阶系数bn (FLOAT32型数组)
 * @return 存放IIR配置信息的地址(Handle)
 */
Handle IirCfg(const UINT16 rank, const FLOAT32* coeffan, const FLOAT32* coeffbn)
{
    IIR_OBJ *iir;
    INT16 i;

    iir = &iir_obj;//(IIR_OBJ*)calloc((1), sizeof(IIR_OBJ));

    if(iir != NULL)
	{
		iir->pcoeffan = iir_buf[0];//(FLOAT32*)calloc((rank+1), sizeof(FLOAT32));
		if(iir->pcoeffan == NULL)
		{
			goto ERR;
		}
		iir->pcoeffbn = iir_buf[1];//(FLOAT32*)calloc((rank+1), sizeof(FLOAT32));
		if(iir->pcoeffbn == NULL)
		{
			goto ERR;
		}
		iir->pxn = iir_buf[2];//(FLOAT32*)calloc((rank+1), sizeof(FLOAT32));
		if(iir->pxn == NULL)
		{
			goto ERR;
		}
		iir->pyn = iir_buf[3];//(FLOAT32*)calloc((rank+1), sizeof(FLOAT32));
		if(iir->pyn == NULL)
		{
			goto ERR;
		}
		iir->rank   = rank;
		iir->rankadd  = iir->rank  + 1;

		for(i=0; i<=rank; i++)
		{
			iir->pcoeffan[i] = coeffan[i];
			iir->pcoeffbn[i] = coeffbn[i];
		}
		iir->pcoeffbn[0] = 1;
	}
    return (Handle)iir;

	ERR:
	return NULL;
}

/**
 * @brief  IIR计算函数       H(Z) = Y(Z)/X(Z) = ∑(0 to n)(an * Z^-n) / ∑(0 to n)(bn * Z^-n)
 *         即Yn = a0*X(0) + a1*X(-1) + ... + an*X(-n) - (b1*Y(-1) + b2*Y(-2) + ... + bn*Y(-n))
 * @param  piir             存放IIR配置信息的地址(Handle)
 * @param  xn               当前采样值X(0) (FLOAT32型)
 * @return 滤波后的输出值Yn (FLOAT32型)
 */
FLOAT32 IirCalc(Handle piir, const FLOAT32 xn)
{
    IIR_OBJ *p = (IIR_OBJ*)piir;
    UINT16   ptr,ptr1;
    UINT16 i;

    ptr1 = p->index;
    p->pxn[ptr1] = xn;
    p->pyn[ptr1] = 0;

    for(i=0; i <= p->rank; i++)
    {
        ptr = (ptr1 + p->rankadd -i) % p->rankadd;
        p->pyn[ptr1] += p->pxn[ptr] * p->pcoeffan[i];
        if(i > 0)
        {
            p->pyn[ptr1] -= p->pyn[ptr] * p->pcoeffbn[i];
        }
    }

    p->index = (p->index + 1) % p->rankadd;

    return p->pyn[ptr1];
}


typedef enum DF_Type
{
    LOWPASS_1ST  = 0,     //!<一阶低通滤波器
    HIGHPASS_1ST = 1,     //!<一阶高通滤波器
    LOWPASS_2ND  = 2,     //!<二阶低通滤波器
    HIGHPASS_2ND = 3,     //!<二阶高通滤波器
    BANDPASS_2ND = 4,     //!<二阶带通滤波器
    BANDREJ_2ND  = 5,     //!<二阶带阻滤波器
}DF_TYPE;

typedef struct
{
    IIR_OBJ     *piir;     //!<iir滤波器组件
    DF_TYPE      type;     //!<本次配置的滤波器类型
    FLOAT32      Ts;       //!<配置的滤波器采样周期,单位为s
    FLOAT32      wc;       //!<配置的滤波器截止频率
    FLOAT32      lambda;   //!<配置的滤波器阻尼系数(一阶滤波器无该系数，可写为0)
}DF_OBJ;
DF_OBJ df_obj = {0};
Handle DigitalFilterCfg(const DF_TYPE type, const FLOAT32 Ts, const FLOAT32 wc, const FLOAT32 lambda)
{
    FLOAT32  coeffan[3],coeffbn[3];
	FLOAT32  tswc_1st=0, coeffb0_1st, coeffb0_rcp_1st=0, coeffb1_1st=0;
	FLOAT32  tswc_2nd, tswc2_2nd=0, interVar_2nd=0, coeffb0_2nd, coeffb0_rcp_2nd=0, coeffb1_2nd=0, coeffb2_2nd=0;
	UINT16   rank;
	DF_OBJ  *df;

    df = &df_obj; //(DF_OBJ*)calloc((1), sizeof(DF_OBJ));
	if(df == NULL)
	   goto ERR;

	if((type == LOWPASS_1ST)||(type == HIGHPASS_1ST))
	{
		rank = 1;
		tswc_1st = Ts * wc;
		coeffb0_1st =  tswc_1st + 2;
		coeffb0_rcp_1st = INVF32(coeffb0_1st);
		coeffb1_1st =  1 - 4 * coeffb0_rcp_1st;
	}
	else
	{
        rank = 2;
		tswc_2nd = Ts * wc;
        tswc2_2nd = tswc_2nd * tswc_2nd;
		interVar_2nd = 4 * tswc_2nd * lambda;
        coeffb0_2nd = tswc2_2nd + interVar_2nd + 4;
        coeffb0_rcp_2nd = INVF32(coeffb0_2nd);
        coeffb1_2nd = (2 * tswc2_2nd - 8 ) * coeffb0_rcp_2nd;
        coeffb2_2nd = (tswc2_2nd - interVar_2nd + 4) * coeffb0_rcp_2nd;
	}

	switch(type)
	{
		case LOWPASS_1ST:	                                                            //一阶低通滤波器
			coeffbn[0] = 1;
			coeffbn[1] = coeffb1_1st;
			coeffan[0] = tswc_1st * coeffb0_rcp_1st;
			coeffan[1] = coeffan[0];
			break;
		case HIGHPASS_1ST:                                                             //一阶高通滤波器
		    coeffbn[0] = 1;
			coeffbn[1] = coeffb1_1st;
			coeffan[0] = 2 * coeffb0_rcp_1st;
			coeffan[1] = -2 * coeffb0_rcp_1st;
			break;
		case LOWPASS_2ND:                                                             //二阶低通滤波器
			coeffbn[0] = 1;
			coeffbn[1] = coeffb1_2nd;
			coeffbn[2] = coeffb2_2nd;
			coeffan[0] = tswc2_2nd * coeffb0_rcp_2nd;
			coeffan[1] = 2 * coeffan[0];
			coeffan[2] = coeffan[0];
			break;
		case HIGHPASS_2ND:                                                             //二阶高通滤波器
			coeffbn[0] = 1;
			coeffbn[1] = coeffb1_2nd;
			coeffbn[2] = coeffb2_2nd;
			coeffan[0] = 4 * coeffb0_rcp_2nd;
			coeffan[1] = -8 * coeffb0_rcp_2nd;
			coeffan[2] = coeffan[0];
			break;
		case BANDPASS_2ND:                                                             //二阶带通滤波器
		    coeffbn[0] = 1;
			coeffbn[1] = coeffb1_2nd;
			coeffbn[2] = coeffb2_2nd;
			coeffan[0] = interVar_2nd * coeffb0_rcp_2nd;
			coeffan[1] = 0;
			coeffan[2] = -coeffan[0];
			break;
		case BANDREJ_2ND:                                                             //二阶带阻滤波器
		    coeffbn[0] = 1;
			coeffbn[1] = coeffb1_2nd;
			coeffbn[2] = coeffb2_2nd;
			coeffan[0] = (tswc2_2nd + 4) * coeffb0_rcp_2nd;
			coeffan[1] = (2 * tswc2_2nd - 8)* coeffb0_rcp_2nd;
			coeffan[2] = coeffan[0];
			break;
	}
	df->piir = (IIR_OBJ*)IirCfg(rank, coeffan, coeffbn);
	if(df->piir == NULL)
	   goto ERR;

	df->type = type;
	df->Ts = Ts;
	df->wc = wc;
	df->lambda = lambda;

	return (Handle)df;

	ERR:
	return NULL;
}

/**
 * @brief  数字滤波器计算函数
 * @param  pDF              存放数字滤波器配置信息的地址(Handle)
 * @param  xn               当前采样值 (FLOAT32型)
 * @return 滤波后的输出值 (FLOAT32型)
 */
FLOAT32 DigitalFilterCalc(Handle pdf, const FLOAT32 xn)
{
	DF_OBJ *p = (DF_OBJ*)pdf;
	Handle    piir;
	FLOAT32   yn;

	piir = (Handle)p->piir;
	yn = IirCalc(piir,xn);

	return yn;
}

typedef struct
{
    Handle   pki;
    Handle   pkd;
    FLOAT32  ts_val;   //!<采样周期
    FLOAT32  kp_val;   //!<比例系数
    FLOAT32  ki_val;   //!<积分系数
    FLOAT32  kd_val;   //!<微分系数
    UINT16   pid_rst;  //!<积分器及微分器复位标志
    FLOAT32  xz_val;   //!<PID控制器当前时刻输入
    FLOAT32  yz_val;   //!<PID控制器当前时刻输出
    FLOAT32  previ_xz; //!<积分器上一时刻输入
    FLOAT32  previ_yz; //!<积分器上一时刻输出
    FLOAT32  prevd_xz; //!<微分器上一时刻输入
    FLOAT32  prevd_yz; //!<微分器上一时刻输出
    FLOAT32  intg_thr; //!<积分器限值
    FLOAT32  deri_thr;  //!<微分器限值
    FLOAT32  pid_thr;  //!<PID控制器限值
    FLOAT32  yz_prop;  //!<比例器当前时刻输出
    FLOAT32  yz_intg;  //!<积分器当前时刻输出
    FLOAT32  yz_deri;   //!<微分器当前时刻输出
}PID_OBJ;

PID_OBJ pid_obj = {0};
/**
 * @brief             PID控制器配置函数
 * @param  ts_val     采样周期Ts
 * @param  kp_val     比例系数Kp
 * @param  ki_val     积分系数Ki
 * @param  kd_val     微分系数Kd
 * @param  intg_thr   积分器限值
 * @param  deri_thr   微分器限值
 * @param  pid_thr    PID控制器限值
 * @return Handle     存放PID_OBJ配置信息的地址
 */
Handle PidCfg(FLOAT32 ts_val, FLOAT32 kp_val, FLOAT32 ki_val, FLOAT32 kd_val, FLOAT32 intg_thr, FLOAT32 deri_thr, UINT16 pid_thr)
{
    PID_OBJ *p;
    FLOAT32  an_i[2],bn_i[2];
    FLOAT32  an_d[2],bn_d[2];

    p = &pid_obj;//(PID_OBJ*)calloc((1), sizeof(PID_OBJ));
    p->ts_val   = ts_val;
    p->kp_val   = kp_val;
    p->ki_val   = ki_val;
    p->kd_val   = kd_val;
    p->intg_thr = intg_thr;
    p->deri_thr = deri_thr;
    p->pid_thr  = pid_thr;

    an_i[0] = p->ki_val*p->ts_val;
    an_i[1] = 0;
    bn_i[0] = 1;
    bn_i[1] = -1;
    p->pki = IirCfg(1, an_i, bn_i);

    an_d[0] = DIVF32(p->kd_val, p->ts_val);
    an_d[1] = -DIVF32(p->kd_val, p->ts_val);
    bn_d[0] = 1;
    bn_d[1] = 0;
    p->pkd = IirCfg(1, an_d, bn_d);

    if((p == NULL))
    {
        free(p);
    }

    return (Handle)p;
}

/**
 * @brief             PID控制器计算函数    G(s)= kp + ki/s + kd*s
 * @param  pid_p      存放PID_OBJ配置信息的地址
 * @param  xz_val     PID控制器当前时刻输入
 * @param  pid_rst    PID控制器复位标志位
 * @return FLOAT32    PID控制后的输出值yz_val(FLOAT32)
 */
FLOAT32 PidCalc(Handle pid_p, FLOAT32 xz_val, UINT16 pid_rst)
{
    PID_OBJ *p  = (PID_OBJ*)pid_p;

    //@lst 比例输出 Gp(s) = kp
    p->yz_prop = p->kp_val*xz_val; //比例器离散化

    //@lst 积分输出 Gi(s) = ki/s
    p->yz_intg = IirCalc(p->pki, xz_val);                   //积分器离散化-后向差分
    p->yz_intg = SAT(p->yz_intg, -p->intg_thr, p->intg_thr);//积分器限值

    if(p->pid_rst) //pid复位判断
    {
        p->previ_xz = 0.0;
        p->previ_yz = 0.0;
    }
    else
    {
        p->previ_xz = xz_val;
        p->previ_yz = p->yz_intg;
    }

    //@lst 微分输出 Gd(s) = kd*s
    p->yz_deri = IirCalc(p->pkd, xz_val);                   //微分器离散化-后向差分
    p->yz_deri = SAT(p->yz_deri, -p->deri_thr, p->deri_thr);//微分器限值

    if(p->pid_rst) //pid复位判断
    {
        p->prevd_xz = 0.0;
        p->prevd_yz = 0.0;
    }
    else
    {
        p->prevd_xz = xz_val;
        p->prevd_yz = p->yz_deri;
    }

    //@lst PID输出 = 比例输出 + 积分输出 + 微分输出 Gpid(s) = kp + ki/s + kd*s
    p->yz_val = p->yz_prop + p->yz_intg + p->yz_deri;
    p->yz_val = SAT(p->yz_val, -p->pid_thr, p->pid_thr);   //pid总限值

    return p->yz_val;
}

FLOAT32 ABC2d(const FLOAT32 *abc, const FLOAT32 wt)
{
    FLOAT32 lag,lead;

    lag	 = wt - PI2DIV3;
    lead = wt + PI2DIV3;

    return L2DIV3 * (abc[0] * COSF(wt)  +  abc[1] * COSF(lag)  +  abc[2] * COSF(lead));
}

FLOAT32 ABC2q(const FLOAT32 *abc, const FLOAT32 wt)
{
    FLOAT32 lag,lead;

    lag	 = wt - PI2DIV3;
    lead = wt + PI2DIV3;

    return -L2DIV3 * (abc[0] * SINF(wt)  +  abc[1] * SINF(lag)  +  abc[2] * SINF(lead));
}

#if 1
void ABC2dq(FLOAT32 *outdq, const FLOAT32 *srcabc, const FLOAT32 wt)
{
    outdq[0] = ABC2d(srcabc, wt);
    outdq[1] = ABC2q(srcabc, wt);
}
#else
void ABC2dq(FLOAT32 *outdq, const FLOAT32 *srcabc, const FLOAT32 wt);
#endif

typedef struct
{
    /* data */
    Handle df_p1;
    Handle df_p2;
    Handle df_p3;
    Handle df_p4;
    Handle pid_p;
}PLL_OBJ;

PLL_OBJ pll_obj = {0};
Handle PLLCfg(const FLOAT32 Ts, const FLOAT32 lpf_wc, const FLOAT32 freq_kp, const FLOAT32 freq_ki, const FLOAT32 freq_limit)
{
    PLL_OBJ *pll;

    pll = &pll_obj;//(PLL_OBJ*)calloc(1,sizeof(PLL_OBJ));
    if(pll == NULL)
    goto ERR;

    pll->df_p1 = DigitalFilterCfg(LOWPASS_1ST, Ts, lpf_wc, 0);
    if(pll->df_p1 == NULL)
    goto ERR;

    pll->df_p2 = DigitalFilterCfg(LOWPASS_1ST, Ts, lpf_wc, 0);
    if(pll->df_p2 == NULL)
    goto ERR;

    pll->df_p3 = DigitalFilterCfg(LOWPASS_1ST, Ts, lpf_wc, 0);
    if(pll->df_p3 == NULL)
    goto ERR;

    pll->df_p4 = DigitalFilterCfg(LOWPASS_1ST, Ts, lpf_wc, 0);
    if(pll->df_p4 == NULL)
    goto ERR;

    pll->pid_p = PidCfg(Ts, freq_kp, freq_ki, 0, freq_limit, 0, freq_limit);
    if(pll->pid_p == NULL)
    goto ERR;

    return (Handle)pll;

    ERR:
    return NULL;
}


void PLLCalc(PLLOUT_OBJ *pllout_p, Handle pll_p, const FLOAT32 Ts)
{
    //中间变量
    FLOAT32 vdq_pos_sample[2];
    FLOAT32 vdq_neg_sample[2];
    FLOAT32 vd_pos_filt;
    FLOAT32 vq_pos_filt;
    FLOAT32 vd_neg_filt;
    FLOAT32 vq_neg_filt;
    FLOAT32 grid_omeg;
    FLOAT32 u_inv_sample[3];

    PLL_OBJ *p = (PLL_OBJ*)pll_p;

    u_inv_sample[0] = *(pllout_p->ua_inv_sample);
    u_inv_sample[1] = *(pllout_p->ub_inv_sample);
    u_inv_sample[2] = *(pllout_p->uc_inv_sample);

    ABC2dq(vdq_pos_sample, u_inv_sample, pllout_p->grid_theta);
    ABC2dq(vdq_neg_sample, u_inv_sample, -pllout_p->grid_theta);

    vd_pos_filt = DigitalFilterCalc(p->df_p1, pllout_p->vd_pos_final);
    vq_pos_filt = DigitalFilterCalc(p->df_p2, pllout_p->vq_pos_final);
    vd_neg_filt = DigitalFilterCalc(p->df_p3, pllout_p->vd_neg_final);
    vq_neg_filt = DigitalFilterCalc(p->df_p4, pllout_p->vq_neg_final);

    pllout_p->vd_pos_final = vdq_pos_sample[0] - vd_neg_filt*COSF(2*pllout_p->grid_theta) - vq_neg_filt*SINF(2*pllout_p->grid_theta);
    pllout_p->vq_pos_final = vdq_pos_sample[1] + vd_neg_filt*SINF(2*pllout_p->grid_theta) - vq_neg_filt*COSF(2*pllout_p->grid_theta);
    pllout_p->vd_neg_final = vdq_neg_sample[0] - vd_pos_filt*COSF(2*pllout_p->grid_theta) + vq_pos_filt*SINF(2*pllout_p->grid_theta);
    pllout_p->vq_neg_final = vdq_neg_sample[1] - vd_pos_filt*SINF(2*pllout_p->grid_theta) - vq_pos_filt*COSF(2*pllout_p->grid_theta);

    grid_omeg = PidCalc(p->pid_p, pllout_p->vq_pos_final, 0) + PI2*50;
    pllout_p->grid_theta += grid_omeg * Ts;
    pllout_p->grid_theta = FMODF(pllout_p->grid_theta, PI2);
    pllout_p->grid_freq = DIVF32(grid_omeg, PI2);

}