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

extern execute_rsp_hd_t rsp; // 执行结果

/*PID控制器重启后的初始化*/

static void initialize(struct PID_HD *self)
{
    self->pri_u.position.pre_feed_back = mode_pwmp_hd->control.ctrl_feedback;
    self->pri_u.position.ki_error = (float32)((rsp.dac - mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin) * (1 / (mode_pwmp_hd->pwmp_save->storage.pwmp_max_origin - mode_pwmp_hd->pwmp_save->storage.pwmp_min_origin)) * 100);
    if (self->pri_u.position.ki_error > 100)
    {
        self->pri_u.position.ki_error = 100;
    }
    else if (self->pri_u.position.ki_error < 0)
    {
        self->pri_u.position.ki_error = 0;
    }
}
/*模式设置*/
static void _set_mode(struct PID_HD *self, pid_mode_e mode)
{
    self->pri_u.position.pid_mode = mode;
    if (mode != INAUTO)
    {
        initialize(&_pid.pid_u.hd);
    }
}
#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));

    /*设置模式*/
    self->pri_u.position.pid_mode = INAUTO;
    /*观测传进来的Kp、Ki、Kd*/
    pri->kp = kp;
    pri->ki = ki;
    pri->kd = kd;
    pri->ki_limit = 20;  // 积分分离界限
    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;
}

/*杭电：位置式PID控制算法*/
static float32 _pid_position(struct PID_HD *self, float32 err)
{
    /*模式设置*/
    self->pri_u.position.pid_mode = (pid_mode_e)mode_pwmp_hd->pid_mode;
    _set_mode(self, self->pri_u.position.pid_mode);
    /*判断是否在自动模式*/
    if (self->pri_u.position.pid_mode != INAUTO)
        return self->pri_u.position.pre_out;
    /*计算控制的运行时间*/
    // 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;
    }
    self->pri_u.position.err = err;
    self->pri_u.position.feed_back = mode_pwmp_hd->control.ctrl_feedback;

    /*抗积分饱和*/
#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.ki * 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.ki * 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.ki * 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 (self->pri_u.position.ki_error > 100)
    {
        self->pri_u.position.ki_error = 100;
    }
    else if (self->pri_u.position.ki_error < 0)
    {
        self->pri_u.position.ki_error = 0;
    }

    /*输出*/
    if (fabs(err) < self->pri_u.position.err_dead)
    {
        /*输出上一次的值*/
        // self->pri_u.position.out = self->pri_u.position.pre_out;
        self->pri_u.position.x[0] = self->pri_u.position.err;
        self->pri_u.position.x[1] = self->pri_u.position.ki_error;
        self->pri_u.position.out = self->pri_u.position.kp * self->pri_u.position.x[0] + self->pri_u.position.x[1] + self->pri_u.position.kd * self->pri_u.position.x[2];
    }
    else
    {
        self->pri_u.position.x[0] = self->pri_u.position.err;
        self->pri_u.position.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);
        self->pri_u.position.x[2] = -(td_derivative(&_pid.pid_u.hd, self->pri_u.position.feed_back, self->pri_u.position.pre_feed_back, self->pri_u.position.control_time));
#else
        // 普通的微分环节
        self->pri_u.position.x[2] = self->pri_u.position.err - self->pri_u.position.pre_error;
#endif

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

    /*输出限幅*/
    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; /*上一次输出值*/
    // 更新反馈历史
    self->pri_u.position.pre_feed_back = self->pri_u.position.feed_back; /*上一次反馈值*/

    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;
    self->set_mode = _set_mode;
}