#include "ladrc.h"
#include "f4_can_app.h"

#define LADRC_CONTROL_DT_S          0.005f
#define LADRC_RPM_FILTER_ALPHA      0.20f

/* 这组只是保守起始值，b0 仍建议后续辨识 */
#define LADRC_DEFAULT_WC            12.0f
#define LADRC_DEFAULT_WO            25.0f
#define LADRC_DEFAULT_B0            0.80f
#define LADRC_DEFAULT_OUT_MAX       1000.0f

/* 把目标爬坡放到控制循环内部，避免依赖外部调用频率 */
#define LADRC_TARGET_SLEW_RPM_PER_S 100.0f

static float LADRC_Abs(float x);
static float LADRC_Clamp(float x, float min_value, float max_value);
static float Ramp_To_Target(float curr, float target, float step);
static void LADRC_ResetStates(LADRC_TypeDef *ladrc);

static float s_filtered_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f};

LADRC_TypeDef ladrc_motors[4];

static float LADRC_Abs(float x)
{
    return (x >= 0.0f) ? x : -x;
}

static float LADRC_Clamp(float x, float min_value, float max_value)
{
    if (x > max_value)
    {
        return max_value;
    }
    if (x < min_value)
    {
        return min_value;
    }
    return x;
}

static float Ramp_To_Target(float curr, float target, float step)
{
    if (target > curr + step)
    {
        return curr + step;
    }
    if (target < curr - step)
    {
        return curr - step;
    }
    return target;
}

static void LADRC_ResetStates(LADRC_TypeDef *ladrc)
{
    if (ladrc == NULL)
    {
        return;
    }

    ladrc->v_cmd   = 0.0f;
    ladrc->v       = 0.0f;
    ladrc->y       = 0.0f;
    ladrc->z1      = 0.0f;
    ladrc->z2      = 0.0f;
    ladrc->u       = 0.0f;
    ladrc->eu_prev = 0.0f;
}

void LADRC_Init(LADRC_TypeDef *ladrc, float wc, float wo, float b0, float h, float max)
{
    float h_beta1_2;
    float h2_beta2_4;
    float delta;

    if (ladrc == NULL)
    {
        return;
    }

    if (h <= 0.0f)
    {
        h = LADRC_CONTROL_DT_S;
    }
    if (LADRC_Abs(b0) < 1e-6f)
    {
        b0 = LADRC_DEFAULT_B0;
    }
    if (b0 < 0.0f)
    {
        b0 = -b0;
    }
    if (wc < 0.0f)
    {
        wc = -wc;
    }
    if (wo < 0.0f)
    {
        wo = -wo;
    }
    if (max <= 0.0f)
    {
        max = LADRC_DEFAULT_OUT_MAX;
    }

    LADRC_ResetStates(ladrc);

    ladrc->wc      = wc;
    ladrc->wo      = wo;
    ladrc->b0      = b0;
    ladrc->h       = h;
    ladrc->out_max = max;

    ladrc->beta1 = 2.0f * wo;
    ladrc->beta2 = wo * wo;

    h_beta1_2  = h * ladrc->beta1 * 0.5f;
    h2_beta2_4 = h * h * ladrc->beta2 * 0.25f;
    delta      = 1.0f + h_beta1_2 + h2_beta2_4;

    /* x(k+1) = Ad*x(k) + Bdu*u(k) + Bdy*y(k) + Bdaw*eu(k-1) */
    ladrc->Ad11 = (1.0f - h_beta1_2 - h2_beta2_4) / delta;
    ladrc->Ad12 = h / delta;
    ladrc->Ad21 = (-h * ladrc->beta2) / delta;
    ladrc->Ad22 = (1.0f + h_beta1_2 - h2_beta2_4) / delta;

    ladrc->Bdu1 = (h * ladrc->b0) / delta;
    ladrc->Bdu2 = (-h * h * ladrc->beta2 * ladrc->b0 * 0.5f) / delta;

    ladrc->Bdy1 = (h * (ladrc->beta1 + h * ladrc->beta2 * 0.5f)) / delta;
    ladrc->Bdy2 = (h * ladrc->beta2) / delta;

    /* e_u = u_sat - u_raw; 取负增益让饱和时对 z2 形成恢复作用 */
    ladrc->kaw   = -(ladrc->wo * ladrc->b0);
    ladrc->Bdaw1 = (0.5f * h * h * ladrc->kaw) / delta;
    ladrc->Bdaw2 = (h * (1.0f + h_beta1_2) * ladrc->kaw) / delta;
}

float LADRC_Calc(LADRC_TypeDef *ladrc, float actual_val)
{
    float z1_new;
    float z2_new;
    float u0;
    float u_raw;
    float u_sat;

    if (ladrc == NULL)
    {
        return 0.0f;
    }

    ladrc->y = actual_val;

    /* LESO + anti-windup 输入统一在同一套 Tustin 离散模型里 */
    z1_new = ladrc->Ad11 * ladrc->z1 + ladrc->Ad12 * ladrc->z2
           + ladrc->Bdy1 * ladrc->y  + ladrc->Bdu1 * ladrc->u
           + ladrc->Bdaw1 * ladrc->eu_prev;

    z2_new = ladrc->Ad21 * ladrc->z1 + ladrc->Ad22 * ladrc->z2
           + ladrc->Bdy2 * ladrc->y  + ladrc->Bdu2 * ladrc->u
           + ladrc->Bdaw2 * ladrc->eu_prev;

    ladrc->z1 = z1_new;
    ladrc->z2 = z2_new;

    u0    = ladrc->wc * (ladrc->v - ladrc->z1);
    u_raw = (u0 - ladrc->z2) / ladrc->b0;
    u_sat = LADRC_Clamp(u_raw, -ladrc->out_max, ladrc->out_max);

    /* 当前拍算出的饱和误差，下一拍再经 Bdaw 进入观测器 */
    ladrc->eu_prev = u_sat - u_raw;
    ladrc->u       = u_sat;

    return u_sat;
}

void FourWheel_LADRC_Init(void)
{
    int i;

    Motor_Init();

    for (i = 0; i < 4; ++i)
    {
        LADRC_Init(&ladrc_motors[i],
                   LADRC_DEFAULT_WC,
                   LADRC_DEFAULT_WO,
                   LADRC_DEFAULT_B0,
                   LADRC_CONTROL_DT_S,
                   LADRC_DEFAULT_OUT_MAX);
        s_filtered_rpm[i] = 0.0f;
    }
}

void FourWheel_LADRC_ResetAll(void)
{
    int i;

    for (i = 0; i < 4; ++i)
    {
        LADRC_ResetStates(&ladrc_motors[i]);
        s_filtered_rpm[i] = 0.0f;
    }

    Motor_Brake_All();
}

void FourWheel_Set_Target_RPM(float fl_rpm, float rl_rpm, float fr_rpm, float rr_rpm)
{
    ladrc_motors[MOTOR_FL].v_cmd = fl_rpm;
    ladrc_motors[MOTOR_RL].v_cmd = rl_rpm;
    ladrc_motors[MOTOR_FR].v_cmd = fr_rpm;
    ladrc_motors[MOTOR_RR].v_cmd = rr_rpm;
}

void FourWheel_LADRC_Control_Loop(void)
{
    int i;
    const float ref_step = LADRC_TARGET_SLEW_RPM_PER_S * LADRC_CONTROL_DT_S;

    /* 5ms 调度；内部速度估计窗口在 motor.c 里做成 10ms */
    Motor_Update_RPM(LADRC_CONTROL_DT_S);

    if (g_robot_ctrl.state != SYSTEM_OPERATIONAL)
    {
        FourWheel_LADRC_ResetAll();
        return;
    }

    for (i = 0; i < 4; ++i)
    {
        float raw_rpm;
        float pwm_out;

        ladrc_motors[i].v = Ramp_To_Target(ladrc_motors[i].v,
                                           ladrc_motors[i].v_cmd,
                                           ref_step);

        raw_rpm = Get_Motor_RPM((Motor_ID_t)i);

        /* 外部低通保留，但减为“轻滤波” */
        s_filtered_rpm[i] += LADRC_RPM_FILTER_ALPHA * (raw_rpm - s_filtered_rpm[i]);

        pwm_out = LADRC_Calc(&ladrc_motors[i], s_filtered_rpm[i]);
        Set_Motor_Output((Motor_ID_t)i, (int16_t)pwm_out);
    }
}

float FourWheel_Get_Target_RPM(Motor_ID_t id)
{
    if ((id < MOTOR_FL) || (id > MOTOR_RR))
    {
        return 0.0f;
    }

    return ladrc_motors[id].v_cmd;
}

float FourWheel_Get_Ref_RPM(Motor_ID_t id)
{
    if ((id < MOTOR_FL) || (id > MOTOR_RR))
    {
        return 0.0f;
    }

    return ladrc_motors[id].v;
}

float FourWheel_Get_Control_Output(Motor_ID_t id)
{
    if ((id < MOTOR_FL) || (id > MOTOR_RR))
    {
        return 0.0f;
    }

    return ladrc_motors[id].u;
}