#include "pid.h"
#include <math.h>

// 模糊集合
#define NL -3
#define NM -2
#define NS -1
#define ZE 0
#define PS 1
#define PM 2
#define PL 3

// 定义偏差E的范围，因为设置了非线性区间，误差在10时才开始进行PID调节，这里E的范围为10
#define MAXE (10)
#define MINE (-MAXE)
// 定义EC的范围，因为变化非常缓慢！，每次的EC都非常小，这里可以根据实际需求来调整，
#define MAXEC (10)
#define MINEC (-MAXEC)
// 定义e,ec的量化因子
#define KE 3 / MAXE
#define KEC 3 / MAXEC

// 定义输出量比例因子
#define KUP 3.0f // 这里只使用了模糊PID的比例增益
#define KUI 0.0f
#define KUD 0.0f

static const float32 fuzzyRuleKp[7][7] = {
    PL, PL, PM, PL, PS, PM, PL,
    PL, PM, PM, PM, PS, PM, PL,
    PM, PS, PS, PS, PS, PS, PM,
    PM, PS, ZE, ZE, ZE, PS, PM,
    PS, PS, PS, PS, PS, PM, PM,
    PM, PM, PM, PM, PL, PL, PL,
    PM, PL, PL, PL, PL, PL, PL};

static const float32 fuzzyRuleKi[7][7] = {
    NL, NL, NL, NL, NM, NL, NL,
    NL, NL, NM, NM, NM, NL, NL,
    NM, NM, NS, NS, NS, NM, NM,
    NM, NS, ZE, ZE, ZE, NS, NM,
    NM, NS, NS, NS, NS, NM, NM,
    NM, NM, NS, NM, NM, NL, NL,
    NM, NL, NM, NL, NL, NL, NL};

static const float32 fuzzyRuleKd[7][7] = {
    PS, PS, ZE, ZE, ZE, PL, PL,
    NS, NS, NS, NS, ZE, NS, PM,
    NL, NL, NM, NS, ZE, PS, PM,
    NL, NM, NM, NS, ZE, PS, PM,
    NL, NM, NS, NS, ZE, PS, PS,
    NM, NS, NS, NS, ZE, PS, PS,
    PS, ZE, ZE, ZE, ZE, PL, PL};

static void fuzzy(float32 e, float32 ec, FUZZY_PID_t *fuzzy_pid)
{

    float32 etemp, ectemp;
    float32 eLefttemp, ecLefttemp; // ec,e，左隶属度
    float32 eRighttemp, ecRighttemp;

    int eLeftIndex, ecLeftIndex; // 模糊位置标号
    int eRightIndex, ecRightIndex;
    e = RANGE(e, fuzzy_pid->mine, fuzzy_pid->maxe);
    ec = RANGE(ec, MINEC, MAXEC);
    e = e * KE;
    ec = ec * KEC;

    etemp = e > 3.0f ? 0.0f : (e < -3.0f ? 0.0f : (e >= 0.0f ? (e >= 2.0f ? 2.5f : (e >= 1.0f ? 1.5f : 0.5f)) : (e >= -1.0f ? -0.5f : (e >= -2.0f ? -1.5f : (e >= -3.0f ? -2.5f : 0.0f)))));
    eLeftIndex = (int)((etemp - 0.5f) + 3); //[-3,3] -> [0,6]
    eRightIndex = (int)((etemp + 0.5f) + 3);
    eLefttemp = etemp == 0.0f ? 0.0f : ((etemp + 0.5f) - e); //
    eRighttemp = etemp == 0.0f ? 0.0f : (e - (etemp - 0.5f));
    ectemp = ec > 3.0f ? 0.0f : (ec < -3.0f ? 0.0f : (ec >= 0.0f ? (ec >= 2.0f ? 2.5f : (ec >= 1.0f ? 1.5f : 0.5f)) : (ec >= -1.0f ? -0.5f : (ec >= -2.0f ? -1.5f : (ec >= -3.0f ? -2.5f : 0.0f)))));
    ecLeftIndex = (int)((ectemp - 0.5f) + 3); //[-3,3] -> [0,6]
    ecRightIndex = (int)((ectemp + 0.5f) + 3);

    ecLefttemp = ectemp == 0.0f ? 0.0f : ((ectemp + 0.5f) - ec);
    ecRighttemp = ectemp == 0.0f ? 0.0f : (ec - (ectemp - 0.5f));

    /*************************************反模糊*************************************/

    fuzzy_pid->kp = (eLefttemp * ecLefttemp * fuzzyRuleKp[eLeftIndex][ecLeftIndex] + eLefttemp * ecRighttemp * fuzzyRuleKp[eLeftIndex][ecRightIndex] + eRighttemp * ecLefttemp * fuzzyRuleKp[eRightIndex][ecLeftIndex] + eRighttemp * ecRighttemp * fuzzyRuleKp[eRightIndex][ecRightIndex]);

    fuzzy_pid->ki = (eLefttemp * ecLefttemp * fuzzyRuleKi[eLeftIndex][ecLeftIndex] + eLefttemp * ecRighttemp * fuzzyRuleKi[eLeftIndex][ecRightIndex] + eRighttemp * ecLefttemp * fuzzyRuleKi[eRightIndex][ecLeftIndex] + eRighttemp * ecRighttemp * fuzzyRuleKi[eRightIndex][ecRightIndex]);

    fuzzy_pid->kd = (eLefttemp * ecLefttemp * fuzzyRuleKd[eLeftIndex][ecLeftIndex] + eLefttemp * ecRighttemp * fuzzyRuleKd[eLeftIndex][ecRightIndex] + eRighttemp * ecLefttemp * fuzzyRuleKd[eRightIndex][ecLeftIndex] + eRighttemp * ecRighttemp * fuzzyRuleKd[eRightIndex][ecRightIndex]);
    // 对解算出的KP,KI,KD进行量化映射

    fuzzy_pid->kp = fuzzy_pid->kp * fuzzy_pid->kup;
    fuzzy_pid->ki = fuzzy_pid->ki * fuzzy_pid->kui;
    fuzzy_pid->kd = fuzzy_pid->kd * fuzzy_pid->kud;
}

/**
 * @brief SV平滑给定,步长默认为0.1，范围0-1之间，越大平滑性越差
 * @param {PID_FUZZY} *self
 * @param {float32} target_sv
 * @return {*}
 * @note
 */
static void smooth_setpoint(struct PID_FUZZY *self, float32 target_sv)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        float32 stepIn = (pri->sv_range) * 0.1f;
        float32 kFactor = 0.0f;
        if (fabs(pri->ref - target_sv) <= stepIn)
        {
            pri->ref = target_sv;
        }
        else
        {
            if (pri->ref - target_sv > 0)
            {
                kFactor = -1.0f;
            }
            else if (pri->ref - target_sv < 0)
            {
                kFactor = 1.0f;
            }
            else
            {
                kFactor = 0.0f;
            }
            pri->ref = pri->ref + kFactor * stepIn;
        }
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        float32 stepIn = (pri->sv_range) * 0.1f;
        float32 kFactor = 0.0f;
        if (fabs(pri->ref - target_sv) <= stepIn)
        {
            pri->ref = target_sv;
        }
        else
        {
            if (pri->ref - target_sv > 0)
            {
                kFactor = -1.0f;
            }
            else if (pri->ref - target_sv < 0)
            {
                kFactor = 1.0f;
            }
            else
            {
                kFactor = 0.0f;
            }
            pri->ref = pri->ref + kFactor * stepIn;
        }
    }
}

// 变速积分
static float32 changing_integral_rate(struct PID_FUZZY *self)
{
    float32 err = 0, iout = 0;
    float32 err_1 = 1, // 误差下限
        err_2 = 10;    // 误差上限
    float32 index = 0;
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        err = pri->e_0;
        iout = pri->iout;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        err = pri->e_0;
        iout = pri->iout;
    }

    if (err * iout > 0) // 判断积分是否为积累趋势
    {
        if (ABS(err) <= err_1)
        {
            index = 1; // 完整积分
        }
        else if (ABS(err) <= (err_1 + err_2))
        {
            // 使用线性函数过渡
            index = (float)(err_2 - ABS(err) + err_1) / err_2;
        }
        else
        {
            index = 0;
        }
    }

    return index;
}

/*封装模糊接口*/
static void compensate(float32 e, float32 ec, FUZZY_PID_t *fuzzy_d)
{
    fuzzy(e, ec, fuzzy_d);
}

/**
 * @brief 更新最大最小值
 * @param {PID_FUZZY} *self
 * @param {float32} out_min
 * @param {float32} out_max
 * @return {*}
 * @note
 */
static void _set_range(struct PID_FUZZY *self, float32 out_min, float32 out_max)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->out_max = out_max;
        pri->out_min = out_min;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->out_max = out_max;
        pri->out_min = out_min;
    }
}

/**
 * @brief 设置死区
 * @param {PID_FUZZY} *self
 * @param {float32} err_dead
 * @return {*}
 * @note
 */
static void _set_err_dead(struct PID_FUZZY *self, float32 err_dead)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->err_dead = err_dead;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->err_dead = err_dead;
    }
}

static void _set_iout(struct PID_FUZZY *self, float32 iout)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->iout = iout;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->iout = iout;
    }
}

static void _set_kp(struct PID_FUZZY *self, float32 kp)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->kp = kp;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->kp = kp;
    }
}

/**
 * @brief  使能积分控制
 * @param {PID_FUZZY} *self
 * @param {BOOL} enable
 * @return {*}
 * @note
 */
// static void _set_ki_enable(struct PID_FUZZY *self, BOOL enable)
// {
//     pri->ki_enable = enable;
// }

/**
 * @brief  使能微分控制
 * @param {PID_FUZZY} *self
 * @param {BOOL} enable
 * @return {*}
 * @note
 */
static void _set_kd_enable(struct PID_FUZZY *self, BOOL enable)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->kd_enable = enable;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->kd_enable = enable;
    }
}

static void _set_kd(struct PID_FUZZY *self, float32 kd)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->kd = kd;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->kd = kd;
    }
}

/**
 * @brief 配置不完全微分系数
 * @param {PID_FUZZY} *self
 * @param {float32} alpha
 * @return {*}
 * @note alpha范围0-1，系数越大,不完全微分的作用越强
 */
static void _set_kd_dev(struct PID_FUZZY *self, float32 alpha)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->alpha = alpha;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->alpha = alpha;
    }
}

static void _set_ki_enable(struct PID_FUZZY *self, BOOL enable)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->ki_enable = enable;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->ki_enable = enable;
    }
}

static void _set_ki(struct PID_FUZZY *self, float32 ki)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->ki = ki;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->ki = ki;
    }
}

/*
 * Function:使能平滑控制
 * parameter:*pid需要配，PID参数结构指针，sv_range控制范围sv的范围
 * return:无
 */
static void _set_smooth_enable(struct PID_FUZZY *self, BOOL enable, float32 sv_range)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->sm = enable;
        pri->sv_range = sv_range;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->sm = enable;
        pri->sv_range = sv_range;
    }
}

// 设置控制参数
static void _set_ctrl_prm(struct PID_FUZZY *self, float32 kp, float32 ki, float32 kd, float32 err_dead, float32 deviation,
                          float32 out_min, float32 out_max)
{
    self->open = TRUE;
    self->fuzzy.kup = KUP;
    self->fuzzy.kui = KUI;
    self->fuzzy.kud = KUD;
    self->fuzzy.mine = MINE;
    self->fuzzy.maxe = MAXE;
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        osel_memset((uint8_t *)pri, 0, sizeof(pid_common_position_t));
        pri->kp = kp;
        pri->ki = ki;
        pri->kd = kd;
        pri->err_dead = err_dead;
        pri->deviation = deviation;
        pri->out_max = out_max;
        pri->out_min = out_min;
        pri->detach = FALSE;
        pri->sm = FALSE;
        pri->ki_enable = TRUE;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        osel_memset((uint8_t *)pri, 0, sizeof(pid_common_increment_t));
        pri->kp = kp;
        pri->ki = ki;
        pri->kd = kd;
        pri->err_dead = err_dead;
        pri->deviation = deviation;
        pri->out_max = out_max;
        pri->out_min = out_min;
        pri->ki_enable = TRUE;
    }
}

static void _set_error_max_min(struct PID_FUZZY *self, float32 mine, float32 maxe)
{
    self->fuzzy.mine = mine;
    self->fuzzy.maxe = maxe;
}

static void _update_ctrl_prm(struct PID_FUZZY *self, float32 kp, float32 ki, float32 kd, float32 err_dead,
                             float32 out_min, float32 out_max)
{

    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->kp = kp;
        pri->ki = ki;
        pri->kd = kd;
        pri->err_dead = err_dead;
        pri->out_max = out_max;
        pri->out_min = out_min;
        pri->detach = FALSE;
        pri->sm = FALSE;

        if (kd > 0)
        {
            pri->kd_enable = TRUE;
        }
        else
        {
            pri->kd_enable = FALSE;
        }
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->kp = kp;
        pri->ki = ki;
        pri->kd = kd;
        pri->err_dead = err_dead;
        pri->out_max = out_max;
        pri->out_min = out_min;

        if (kd > 0)
        {
            pri->kd_enable = TRUE;
        }
        else
        {
            pri->kd_enable = FALSE;
        }
    }
}

/**
 * @brief 非0时配置为积分分离+抗积分饱和PID,否则为普通抗积分饱和PID
 * @param {PID_FUZZY} *self
 * @param {float32} max_err
 * @param {BOOL} mode
 * @return {*}
 */
static void _set_cfg(struct PID_FUZZY *self, float32 max_err, BOOL mode)
{

    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->err_limit = max_err;
        pri->detach = mode == FALSE ? FALSE : TRUE;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->err_limit = max_err;
        pri->detach = mode == FALSE ? FALSE : TRUE;
    }
}

/**
 * @brief 判断是否处于死区范围内
 *
 * 根据给定的 PID_FUZZY 结构体，判断当前值是否处于死区范围内。
 *
 * @param self PID_FUZZY 结构体指针
 *
 * @return 如果处于死区范围内，返回 TRUE；否则返回 FALSE
 */
static BOOL _in_dead_zone(struct PID_FUZZY *self)
{
    float32 deviation = 0.0f;
    float32 err_dead = 0.0f;
    float32 err = 0.0f;
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        deviation = pri->deviation;
        err_dead = pri->err_dead;
        err = pri->feedback - pri->ref;
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        deviation = pri->deviation;
        err_dead = pri->err_dead;
        err = pri->feedback - pri->ref;
    }

    if (ABS(err + deviation) <= err_dead)
    {
        return TRUE;
    }
    else
    {
        return FALSE;
    }
}

static float32 position_pid(struct PID_FUZZY *self, float32 target, float32 feedback)
{
    float32 error = 0;
    float32 ec = 0;
    float32 kd = 0;
    float32 thisdev = 0;

    pid_common_position_t *pri = &self->pri_u.position;

    /*获取期望值与实际值,进行偏差计算*/
    if (pri->sm == 1)
    {
        smooth_setpoint(self, target);
    }
    else
    {
        pri->ref = target;
    }

    pri->feedback = feedback;
    error = pri->ref - pri->feedback;

    if (self->in_dead_zone(self) == TRUE)
    {
        error = 0;
    }

    pri->e_0 = error;

    /* fuzzy control caculate */
    ec = error - pri->e_1;
    if (self->open == TRUE)
    {
        compensate(error, ec, &self->fuzzy);
    }

    /*根据PID配置的模式,获取积分数据,进行积分累加*/
    if (self->speed_integral_enable == TRUE)
    {
        pri->iout = (pri->ki + self->fuzzy.ki) * error * changing_integral_rate(self);
    }
    else
    {
        float32 temp_iterm = 0.0f;
        float32 insert = 0;
        if (pri->out >= pri->out_max)
        {
            if (fabs(error) > pri->err_limit && pri->detach)
            {
                insert = 0;
            }
            else
            {
                insert = 1;
                if (error < 0)
                {
                    temp_iterm = (pri->ki + self->fuzzy.ki) * error;
                }
            }
        }
        else if (pri->out <= pri->out_min)
        {
            if (fabs(error) > pri->err_limit && pri->detach)
            {
                insert = 0;
            }
            else
            {
                insert = 1;
                if (error > 0)
                {
                    temp_iterm = (pri->ki + self->fuzzy.ki) * error;
                }
            }
        }
        else
        {
            if (fabs(error) > pri->err_limit && pri->detach)
            {
                insert = 0;
            }
            else
            {
                insert = 1;
                temp_iterm = (pri->ki + self->fuzzy.ki) * error;
            }
        }

        pri->iout += temp_iterm;

        /* limt integral */
        if (pri->iout > pri->out_max)
        {
            pri->iout = pri->out_max;
        }
        else if (pri->iout < pri->out_min)
        {
            pri->iout = pri->out_min;
        }
        pri->iout = pri->iout * insert;
    }

#if INCOMPLETE_DIFFEREN == 1
    /*不完全微分*/
    thisdev = kd * (1.0 - pri->alpha) * (error - pri->e_1) + pri->alpha * pri->lastdev;
    /*record last dev result*/
    pri->lastdev = thisdev;
#else
    thisdev = (error - pri->e_1) * (kd);
#endif

    if (pri->kd_enable == TRUE)
    {
        thisdev = 0;
    }

    if (pri->ki_enable == FALSE)
    {
        pri->iout = 0;
    }

    pri->out = (pri->kp + self->fuzzy.kp) * error + pri->iout + thisdev;
    pri->e_1 = error;
    /*record last feedback sensor result*/
    pri->pre_feedback = pri->feedback;
    /*limt pid output*/
    pri->out = RANGE(pri->out, pri->out_min, pri->out_max);
    return pri->out;
}

static float32 increment_pid(struct PID_FUZZY *self, float32 target, float32 feedback)
{
    float32 ep, ei, ed;
    float32 inc_out;
    float32 thisdev = 0;
    pid_common_increment_t *pri = &self->pri_u.increment;

    pri->feedback = feedback;
    pri->e_0 = pri->ref - pri->feedback;

    if (pri->e_0 >= MAXE)
    {
        return pri->out_max;
    }
    else if (pri->e_0 <= MINE)
    {
        return pri->out_min;
    }

    if (pri->sm == 1)
    {
        smooth_setpoint(self, target);
    }
    else
    {
        pri->ref = target;
    }

    if (fabs(pri->e_0) <= pri->err_dead)
    {
        pri->e_0 = 0;
    }

    ep = pri->e_0 - pri->e_1;
    ei = pri->e_0;
    ed = pri->e_0 - 2 * pri->e_1 + pri->e_2;
    if (self->open == TRUE)
    {
        compensate(pri->e_0, ep, &self->fuzzy);
    }

#if INCOMPLETE_DIFFEREN == 1
    /*不完全微分*/
    thisdev = (1.0 - pri->alpha) * (pri->kd + self->fuzzy.kd) * ed + pri->alpha * pri->lastdev;
#else
    ed = ed;
#endif

    if (self->speed_integral_enable == TRUE)
    {

        if (ABS(pri->e_0) > MAXE)
        {
            pri->iout = (pri->ki + self->fuzzy.ki) * ei;
        }
        else
        {
            // 变速积分
            pri->iout = (pri->ki + self->fuzzy.ki) * ei * changing_integral_rate(self);
        }
    }
    else
    {
        pri->iout = (pri->ki + self->fuzzy.ki) * ei;
    }

    if (pri->kd_enable == FALSE)
    {
        thisdev = 0;
    }

    if (pri->ki_enable == FALSE)
    {
        pri->iout = 0;
    }

    inc_out = (pri->kp + self->fuzzy.kp) * ep + pri->iout + thisdev;
    pri->e_2 = pri->e_1;
    pri->e_1 = pri->e_0;
    pri->lastdev = thisdev;
    pri->out = pri->out + inc_out;
    pri->out = RANGE(pri->out, pri->out_min, pri->out_max);
    return pri->out;
}

static float32 _pid(struct PID_FUZZY *self, float32 target, float32 feedback)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        return position_pid(self, target, feedback);
    }
    else
    {
        return increment_pid(self, target, feedback);
    }
}

/**
 * @brief 复位PID积分及微分控制数据
 * @param {PID_FUZZY} *self
 * @return {*}
 */
static void _restctrl(struct PID_FUZZY *self, float32 out)
{
    if (self->sub_type == PID_SUB_TYPE_POSITION)
    {
        pid_common_position_t *pri = NULL;
        pri = &self->pri_u.position;
        pri->e_1 = 0;
        pri->iout = 0;
        pri->out = out;
        pri->iout = out;
#if INCOMPLETE_DIFFEREN == 1
        pri->lastdev = 0;
#endif
    }
    else
    {
        pid_common_increment_t *pri = NULL;
        pri = &self->pri_u.increment;
        pri->e_0 = 0;
        pri->e_1 = 0;
        pri->e_2 = 0;
        pri->lastdev = 0;
        pri->out = out;
        pri->iout = out;
    }
}

void pid_fuzzy_constructor(struct PID_FUZZY *self)
{
    self->set_ctrl_prm = _set_ctrl_prm;
    self->set_error_max_min = _set_error_max_min;
    self->update_ctrl_prm = _update_ctrl_prm;
    self->set_cfg = _set_cfg;
    self->set_smooth_enable = _set_smooth_enable;
    self->set_err_dead = _set_err_dead;
    self->set_kp = _set_kp;
    self->set_ki_enable = _set_ki_enable;
    self->set_ki = _set_ki;
    self->set_kd_enable = _set_kd_enable;
    self->set_kd = _set_kd;
    self->set_kd_dev = _set_kd_dev;
    self->set_range = _set_range;
    self->restctrl = _restctrl;
    self->set_iout = _set_iout;
    self->in_dead_zone = _in_dead_zone;
    self->execute = _pid;
}