#ifndef __LADRC_H
#define __LADRC_H

#include <stdint.h>
#include "motor.h"

typedef struct {
  /* 外部命令与内部参考 */
  float v_cmd;      /* 上层下发的目标 RPM */
  float v;          /* 内部斜坡后的参考 RPM */
  float y;          /* 实际速度 RPM */

  /* LESO 状态 */
  float z1;
  float z2;

  /* 核心参数 */
  float wc;
  float wo;
  float b0;

  /* LESO 增益 */
  float beta1;
  float beta2;

  /* 采样与限幅 */
  float h;
  float out_max;

  /* 控制输出 */
  float u;

  /* Tustin 离散矩阵 */
  float Ad11;
  float Ad12;
  float Ad21;
  float Ad22;

  float Bdu1;
  float Bdu2;

  float Bdy1;
  float Bdy2;

  /* Anti-windup: 作为 z2_dot 的额外输入，并离散到同一套 Tustin 模型 */
  float kaw;
  float eu_prev;
  float Bdaw1;
  float Bdaw2;

} LADRC_TypeDef;

void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max);
float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val);

extern LADRC_TypeDef ladrc_motors[4];

void FourWheel_LADRC_Init(void);
void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm);
void FourWheel_LADRC_ResetAll(void);
void FourWheel_LADRC_Control_Loop(void);

/* 命令目标与内部参考分开暴露，便于调试 */
float FourWheel_Get_Target_RPM(Motor_ID_t id);     /* 返回 v_cmd */
float FourWheel_Get_Ref_RPM(Motor_ID_t id);        /* 返回 v */
float FourWheel_Get_Control_Output(Motor_ID_t id);

#endif /* __LADRC_H */