/**
 * @file    control_task.c
 * @brief   二轴云台增稳控制任务
 * @note    目标：保持 Roll=0, Pitch=0，Yaw 用于方向控制
 *
 * 控制架构：
 *   IMU 数据 → PID 控制器 → 电机补偿角度 → 抵消基座倾斜
 */

#include "app.h"
#include "cmsis_os2.h"
#include "mf4010v2.h"
#include <stdio.h>

/* ============================================================================
 * 外部变量 (来自 sensor_task)
 * ============================================================================ */
extern float fAcc[3], fGyro[3], fAngle[3];  /* fAngle[0]=Roll, fAngle[1]=Pitch, fAngle[2]=Yaw */

/* ============================================================================
 * 云台配置
 * ============================================================================ */

/* 目标角度：保持水平 */
static float g_target_roll = 0.0f;
static float g_target_pitch = 0.0f;
static float g_target_yaw = 0.0f;  /* Yaw 目标可通过外部修改 */

/* 电机实例 */
static mf4010v2_t motor_yaw;    /* Yaw 轴电机（外框） */
static mf4010v2_t motor_pitch;  /* Pitch 轴电机（内框） */

/* PID 控制器 */
typedef struct {
    float Kp, Ki, Kd;
    float integral;
    float prev_error;
    float output_max;
} PID_t;

static PID_t g_roll_pid = {0};
static PID_t g_pitch_pid = {0};

/* ============================================================================
 * PID 控制函数
 * ============================================================================ */

void PID_Init(PID_t *pid, float kp, float ki, float kd, float max)
{
    pid->Kp = kp;
    pid->Ki = ki;
    pid->Kd = kd;
    pid->output_max = max;
    pid->integral = 0;
    pid->prev_error = 0;
}

float PID_Compute(PID_t *pid, float target, float actual)
{
    float error = target - actual;
    float output;

    /* 比例项 */
    output = pid->Kp * error;

    /* 积分项（带抗饱和） */
    pid->integral += error;
    if (pid->integral > pid->output_max) {
        pid->integral = pid->output_max;
    } else if (pid->integral < -pid->output_max) {
        pid->integral = -pid->output_max;
    }
    output += pid->Ki * pid->integral;

    /* 微分项 */
    float derivative = error - pid->prev_error;
    output += pid->Kd * derivative;
    pid->prev_error = error;

    /* 输出限幅 */
    if (output > pid->output_max) {
        output = pid->output_max;
    } else if (output < -pid->output_max) {
        output = -pid->output_max;
    }

    return output;
}

/* ============================================================================
 * 控制任务
 * ============================================================================ */

void control_task(void)
{
    // int32_t yaw_cmd, pitch_cmd;
    // float roll_comp, pitch_comp;

    printf("[control_task] starting stabilization control\n");

    /* 1. 初始化 PID 参数 */
    /* Roll 轴 PID：控制 Yaw 电机来补偿 Roll 倾斜 */
    // PID_Init(&g_roll_pid, 0.5f, 0.01f, 0.1f, 18000.0f);  /* 最大补偿 180 度 */

    /* Pitch 轴 PID：控制 Pitch 电机补偿 Pitch 倾斜 */
    // PID_Init(&g_pitch_pid, 0.5f, 0.01f, 0.1f, 9000.0f);  /* 最大补偿 90 度 */

    /* 2. 初始化电机 */
    // osDelay(pdMS_TO_TICKS(1000));
    mf4010v2_init(&motor_yaw, 0x141, 1);    /* Yaw 电机 ID: 0x141, CAN2 */
    mf4010v2_init(&motor_pitch, 0x142, 1);  /* Pitch 电机 ID: 0x142, CAN2 */

    /* 3. 电机关机状态启动 */
    mf4010v2_close(&motor_yaw);
    mf4010v2_close(&motor_pitch);

    printf("[control_task] motor yaw is fault: %d\n", mf4010v2_is_fault(&motor_yaw));
    printf("[control_task] motor pitch is fault: %d\n", mf4010v2_is_fault(&motor_pitch));
    /* 等待 IMU 稳定 */
    osDelay(pdMS_TO_TICKS(5000));

    /* 4. 使能电机 */
    mf4010v2_run(&motor_yaw);
    mf4010v2_run(&motor_pitch);

    printf("[control_task] stabilization enabled\n");

    /* 主控制循环 (100Hz) */
    while (1) {
        /* 读取 IMU 数据 */
        // float current_roll = fAngle[0];   /* Roll 角 */
        // float current_pitch = fAngle[1];  /* Pitch 角 */

        // /* PID 计算补偿量 */
        // /* 目标：Roll=0, Pitch=0 */
        // roll_comp = PID_Compute(&g_roll_pid, g_target_roll, current_roll);
        // pitch_comp = PID_Compute(&g_pitch_pid, g_target_pitch, current_pitch);

        // /* 运动学映射：补偿量 -> 电机角度 */
        // /* 对于 Pitch-Yaw 云台：
        //  * - Pitch 倾斜 → Pitch 电机反向补偿
        //  * - Roll 倾斜 → Yaw 电机 + Pitch 电机联合补偿（简化为 Yaw 电机）
        //  */
        // yaw_cmd = (int32_t)(roll_comp * 100.0f);      /* 转为 0.01 度 */
        // pitch_cmd = (int32_t)(pitch_comp * 100.0f);

        // /* 发送位置指令到电机 */
        // /* 使用增量位置控制，直接叠加补偿量 */
        // mf4010v2_set_angle(&motor_yaw, motor_yaw.current_angle + yaw_cmd);
        // mf4010v2_set_angle(&motor_pitch, motor_pitch.current_angle + pitch_cmd);

        /* 100Hz 控制循环 */
        osDelay(pdMS_TO_TICKS(10));
    }
}

/* ============================================================================
 * 外部 API
 * ============================================================================ */

/* 设置 Yaw 目标方向 */
void gimbal_set_yaw_target(float yaw_deg)
{
    g_target_yaw = yaw_deg;
}