#include <sys/time.h>
#include <rtthread.h>

#include "pid.h"

void PID_Init(PID_TypeDef *pid, float kp, float ki, float kd)
{
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->setpoint = 0;
    pid->integral = 0;
    pid->prev_error = 0;
    pid->output = 0;
    pid->last_time = 0;
}

float PID_Calculate(PID_TypeDef *pid, float measured, unsigned long now_time)
{
    float error = pid->setpoint - measured;
    float delta_time = (now_time - pid->last_time) / 1000.0f; // 转换为秒

    if (delta_time <= 0)
    {
        return pid->output; // 如果时间间隔无效，返回上次输出
    }

    pid->integral += error * delta_time;
    float derivative = (error - pid->prev_error) / delta_time;

    pid->output = pid->kp * error + pid->ki * pid->integral + pid->kd * derivative;

    pid->prev_error = error;
    pid->last_time = now_time;

    return pid->output;
}

extern unsigned long get_timestamp(void);

void pid_demo(void)
{
    PID_TypeDef pid;
    PID_Init(&pid, 1.0f, 0.5f, 0.1f);
    pid.setpoint = 100.0f; // 设置目标值
    // unsigned long now_time = get_timestamp(); // 获取当前时间戳

    // float output = PID_Calcuate(&pid, measured, now);
    
}