#include "pid_hd.h"
#include <math.h>
#include "sys.h"
#include "app.h"

float32 out_pos;  // 位置式pid输出
float32 Kp_watch; // 观测Kp的大小
float32 Ki_watch; // 观测Kp的大小
float32 Kd_watch; // 观测Kp的大小

#if INCOMPLETE_DIFFEREN_HD == 1 // 积分分离
/*计算微分项，使用追随误差微分项*/
static float32 td_derivative(struct PID_HD *self, float32 current_err, float32 pre_err, float32 dt)
{
    pid_hd_position_t *pri = &self->pri_u.position;
    float32 derivative = (current_err - pre_err) / dt;                                                  // 计算积分项
    derivative = pri->td_alpha * derivative + (1 - pri->td_alpha) * pri->td_beta * pri->pre_derivative; // 追随误差微分器平滑输出
    pri->pre_derivative = derivative;                                                                   // 更新上一次误差
    return derivative;
}
#endif

/*杭电：设置增量式PID参数*/
static void _set_ctrl_prm_position(struct PID_HD *self, float32 kp, float32 ki, float32 kd)
{
    pid_hd_position_t *pri = &self->pri_u.position;
    osel_memset((uint8_t *)pri, 0, sizeof(pid_hd_position_t));

    /*观测传进来的Kp、Ki、Kd*/
    Kp_watch = kp;
    Ki_watch = ki;
    Kd_watch = kd;

    pri->kp = kp;
    pri->ki = ki;
    pri->kd = kd;
    pri->ki_limit = 10;  // 积分分离界限
    pri->err_dead = 0.5; // 控制死区范围
#if INCOMPLETE_DIFFEREN_HD == 1
    /*不完全微分系数*/
    pri->td_alpha = 0.5;
    pri->td_beta = 0.5;
#endif
}

/*杭电：设置输出限幅参数*/
static void _set_out_prm_position(struct PID_HD *self, float32 maximum, float32 minimum)
{
    self->pri_u.position.out_max = maximum;
    self->pri_u.position.out_min = minimum;
}

float32 out_pos_watch;
/*杭电：位置式PID控制算法*/
static float32 _pid_position(struct PID_HD *self, float32 err)
{
    /*计算控制的运行时间*/
    sys_millis_reset();
    self->pri_u.position.control_time = sys_millis();
    self->pri_u.position.tmp_time = 0;

    /*测试：4.18*/
    if (fabs(err) < 0.1)
    {
        err = 0;
    }

    float32 x[3];
    self->pri_u.position.err = err;

    /*抗积分饱和*/
#if INTEGRAL_SEPARATION == 1 // 积分分离
    if (self->pri_u.position.out > self->pri_u.position.out_max)
    {
        if (self->pri_u.position.err > self->pri_u.position.ki_limit) // 积分分离
        {
            self->pri_u.position.ki_error += 0;
        }
        else
        {
            if (self->pri_u.position.err < 0) // 若偏差为负值，执行负偏差的累加
            {
                self->pri_u.position.ki_error += self->pri_u.position.err;
            }
        }
    }
    else if (self->pri_u.position.out < self->pri_u.position.out_min)
    {
        if (self->pri_u.position.err > self->pri_u.position.ki_limit) // 若偏差为负值，停止积分
        {
            self->pri_u.position.ki_error += 0;
        }
        else
        {
            if (self->pri_u.position.err > 0) // 若偏差为正值，执行正偏差的累加
            {
                self->pri_u.position.ki_error += self->pri_u.position.err;
            }
        }
    }
    else
    {
        if (fabs(err) > self->pri_u.position.ki_limit || fabs(err) < 0.5)
        {
            self->pri_u.position.ki_error += 0;
        }
        else
        {
            self->pri_u.position.ki_error += self->pri_u.position.err;
        }
    }
#else /*无积分分离操作*/
    if (fabs(err) < 0.4)
    {
        self->pri_u.position.ki_error += 0;
    }
    else
    {
        self->pri_u.position.ki_error += self->pri_u.position.err;
    }
#endif

    /*输出*/
    if (fabs(err) < self->pri_u.position.err_dead)
    {
        /*输出上一次的值*/
        // self->pri_u.position.out = self->pri_u.position.pre_out;
        x[0] = self->pri_u.position.err;
        x[1] = self->pri_u.position.ki_error;
        out_pos = self->pri_u.position.kp * x[0] + self->pri_u.position.ki * x[1] + self->pri_u.position.kd * x[2];
        out_pos_watch = out_pos;
        self->pri_u.position.out = out_pos;
    }
    else
    {
        x[0] = self->pri_u.position.err;
        x[1] = self->pri_u.position.ki_error;

#if INCOMPLETE_DIFFEREN_HD == 1
        /*不完全微分项，为了解决普通PID为微分环节容易振荡的问题*/
        self->pri_u.position.tmp_time = sys_millis();
        self->pri_u.position.control_time -= self->pri_u.position.tmp_time;
        self->pri_u.position.control_time /= 1000.0; // 将单位转换为秒
        x[2] = td_derivative(&_pid.pid_u.hd, err, self->pri_u.position.pre_error, self->pri_u.position.control_time);
#else
        // 普通的微分环节
        x[2] = self->pri_u.position.err - self->pri_u.position.pre_error;
#endif

        out_pos = self->pri_u.position.kp * x[0] + self->pri_u.position.ki * x[1] + self->pri_u.position.kd * x[2];
        out_pos_watch = out_pos;
        self->pri_u.position.out = out_pos;
    }

    /*输出限幅*/
    if (self->pri_u.position.out > self->pri_u.position.out_max)
    {
        self->pri_u.position.out = self->pri_u.position.out_max;
    }
    if (self->pri_u.position.out < self->pri_u.position.out_min)
    {
        self->pri_u.position.out = self->pri_u.position.out_min;
    }

    // 更新误差历史
    self->pri_u.position.pre_error = self->pri_u.position.err; /*上一次误差值*/
    // 更新输出历史
    self->pri_u.position.pre_out = self->pri_u.position.out; /*上一次输出值*/

    out_pos = self->pri_u.position.out;
    return self->pri_u.position.out;
}

/*杭电：参数控制器*/
void pid_hd_constructor(struct PID_HD *self)
{
    self->set_ctrl_prm_position = _set_ctrl_prm_position;
    self->set_out_prm_position = _set_out_prm_position;
    self->pid_position = _pid_position;
}