// 跟踪微分器结构体
typedef struct {
    float r;          // 速度因子：值越大跟踪越快，但易过冲
    float h0;         // 滤波因子：通常设为积分步长的倍数
    float h;          // 积分步长（控制周期，单位：秒）

    float v1_k1;      // 上一时刻的跟踪输出
    float v2_k1;      // 上一时刻的微分输出
} td_struct_t;

/**
 * @brief TD 计算函数
 * @param td TD 结构体指针
 * @param input 目标设定值
 * @return float 平滑后的位置信号
 */
float td_calc(td_struct_t *td, float input) {
    float v1 = td->v1_k1;
    // float v2 = td->v2_k1; // 变量未使用，注释以消除警告

    // 计算中间变量，用于判断相轨迹位置
    float fh = td->h * td->v2_k1;
    float fv = td->v1_k1 - input + td->v2_k1 / (td->r * td->h0);

    // 离散化最速综合函数 (Fhan 的简化形式)
    float td_out = td->v1_k1 + td->h * td->v2_k1;

    if (fv > td->h0) {
        td->v2_k1 -= td->h * td->r * 1.0f;
    } else if (fv < -td->h0) {
        td->v2_k1 -= td->h * td->r * (-1.0f);
    } else {
        // 线性区，避免高频振荡
        td->v2_k1 -= td->h * td->r * fv / td->h0;
    }

    td->v1_k1 = v1 + td->h * td->v2_k1;

    return td_out;
}

// 扩张状态观测器结构体
typedef struct {
    float b0;         // 扰动补偿增益
    float h;          // 积分步长
    float beta1;      // 观测器增益 1 (对应位置)
    float beta2;      // 观测器增益 2 (对应速度)
    float beta3;      // 观测器增益 3 (对应扰动)

    float z1_k1;      // 位置估计值
    float z2_k1;      // 速度估计值
    float z3_k1;      // 总扰动估计值
} lueso_struct_t;

void lueso_init(lueso_struct_t *eso, float init_state) {
    eso->z1_k1 = init_state;
    eso->z2_k1 = 0.0f;
    eso->z3_k1 = 0.0f;
}

void lueso_update(lueso_struct_t *eso, float y_measure, float u_control) {
    float e = eso->z1_k1 - y_measure;

    // 非线性 fal 函数的简化实现
    // 在小误差区间使用线性增益，大误差区间使用非线性抑制
    float fal_e1 = (e > 0.01f || e < -0.01f) ? e : (e * e * (e > 0 ? 1.0f : -1.0f) / 0.01f);

    // 状态更新
    eso->z1_k1 += eso->h * (eso->z2_k1 - eso->beta1 * e);
    // 注意：此处使用了 fal_e1 进行非线性反馈
    eso->z2_k1 += eso->h * (eso->z3_k1 - eso->beta2 * fal_e1 + eso->b0 * u_control);
    eso->z3_k1 += eso->h * (-eso->beta3 * e); // 原始代码此处仅使用线性误差 e，可根据需求改为 fal_e1
}

// ADRC 控制器总结构体
typedef struct {
    td_struct_t td;      // 跟踪微分器
    lueso_struct_t eso;  // 扩张状态观测器
    float kp;            // 线性反馈比例增益 (原代码中的 r)
    float h;             // 控制周期
} ladrc_struct_t;        // 命名保留 LADRC，实为混合实现

// 初始化
void ladrc_init(ladrc_struct_t *ctl, float init_state) {
    // 1. 初始化 TD
    ctl->td.r = 100.0f;
    ctl->td.h0 = ctl->h * 0.1f;
    ctl->td.h = ctl->h;
    ctl->td.v1_k1 = init_state;
    ctl->td.v2_k1 = 0.0f;

    // 2. 初始化 ESO
    lueso_init(&ctl->eso, init_state);
    ctl->eso.b0 = 1.0f;     // 需根据系统辨识结果修改
    ctl->eso.h = ctl->h;
    // 经验参数，需根据带宽调整
    ctl->eso.beta1 = 10.0f;
    ctl->eso.beta2 = 100.0f;
    ctl->eso.beta3 = 1000.0f;

    // 3. 反馈增益
    ctl->kp = 10.0f;        // 对应 PD 控制中的 P
}

// 核心计算函数
float ladrc_compute(ladrc_struct_t *ctl, float ref_input, float measure_output) {
    // Step 1: TD 安排过渡过程
    float v1 = td_calc(&ctl->td, ref_input);

    // Step 2: ESO 状态与扰动估计
    // 注意：此处 u_control 传入 0.0f 可能导致 ESO 无法正确补偿模型已知部分
    // 建议：应传入上一时刻的实际控制量 u，或者如果不含模型已知项，则保持这种用法
    lueso_update(&ctl->eso, measure_output, 0.0f);

    // Step 3: 计算状态误差
    float e1 = v1 - measure_output;                    // 位置误差
    float e2 = ctl->td.v2_k1 - ctl->eso.z2_k1;         // 速度误差

    // Step 4: 线性反馈与扰动补偿
    // u0 = Kp * error_pos + Kd * error_spd (此处代码省略了 Kd，仅使用了 Kp)
    // 注意：原代码 u0 = ctl->r * e1 + e2 中，e2 相当于 Kd=1 的情况
    float u0 = ctl->kp * e1 + e2;

    // 最终输出：控制律 - 扰动补偿
    float u = (u0 - ctl->eso.z3_k1) / ctl->eso.b0;

    return u;
}

ladrc_struct_t my_adrc;

void robot_init() {
    my_adrc.h = 0.001f; // 1ms 控制周期
    ladrc_init(&my_adrc, get_current_pos());
}

void robot_control_loop() {
    float target = get_remote_control_target();
    float feedback = get_motor_encoder();

    float current = ladrc_compute(&my_adrc, target, feedback);

    motor_set_current(current);
    delay_ms(1);
}