/* app */
#include "robot_def.h"
#include "robot_cmd.h"
/* module */
#include "controller.h"
#include "remote_control.h"
#include "ins_task.h"
#include "message_center.h"
#include "general_def.h"
#include "kalman_filter.h"
#include "master_machine.h"
/* bsp */
#include "bsp_dwt.h"
#include "bsp_log.h"

/* ---- 模式 ---- */
static Vision_Mode_e current_mode = VISION_MODE_MANUAL;

/* ---- 视觉追踪 PID (外环) ---- */
static PIDInstance vision_yaw_pid;
static PIDInstance vision_pitch_pid;

/* 视觉误差卡尔曼滤波器 */
static KalmanFilter_t yaw_error_kf;
static KalmanFilter_t pitch_error_kf;
static float filtered_yaw_error;
static float filtered_pitch_error;

/* 内环角度参考累计值 */
static float gimbal_yaw_ref;
static float gimbal_pitch_ref;

/* ---- pub-sub ---- */
static Publisher_t *gimbal_cmd_pub;
static Subscriber_t *gimbal_feed_sub;
static Gimbal_Ctrl_Cmd_s gimbal_cmd_send;
static Gimbal_Upload_Data_s gimbal_fetch_data;

/* ---- 遥控器 ---- */
static RC_ctrl_t *rc_data;

/* ---- 机器人状态 ---- */
static Robot_Status_e robot_state;

/* ---- 激光 ---- */
static void laser_on(void)
{
    HAL_GPIO_WritePin(POWER_5V_GPIO_Port, POWER_5V_Pin, GPIO_PIN_SET);
}

static void laser_off(void)
{
    HAL_GPIO_WritePin(POWER_5V_GPIO_Port, POWER_5V_Pin, GPIO_PIN_RESET);
}

/* ---- 视觉 PID 和卡尔曼滤波器重置 ---- */
static void VisionPIDReset(void)
{
    /* 清零积分项和上次误差 */
    vision_yaw_pid.Iout      = 0;
    vision_yaw_pid.ITerm     = 0;
    vision_yaw_pid.Last_Err  = 0;
    vision_yaw_pid.Err       = 0;
    vision_yaw_pid.Pout      = 0;
    vision_yaw_pid.Dout      = 0;
    vision_yaw_pid.Output    = 0;
    vision_yaw_pid.Last_Dout = 0;

    vision_pitch_pid.Iout      = 0;
    vision_pitch_pid.ITerm     = 0;
    vision_pitch_pid.Last_Err  = 0;
    vision_pitch_pid.Err       = 0;
    vision_pitch_pid.Pout      = 0;
    vision_pitch_pid.Dout      = 0;
    vision_pitch_pid.Output    = 0;
    vision_pitch_pid.Last_Dout = 0;

    /* 重置滤波器状态: x = [0, 0], P = diag(5, 5) */
    memset(yaw_error_kf.xhat_data,   0, yaw_error_kf.xhatSize   * sizeof(float));
    memset(yaw_error_kf.P_data,      0, yaw_error_kf.xhatSize   * yaw_error_kf.xhatSize * sizeof(float));
    memset(pitch_error_kf.xhat_data, 0, pitch_error_kf.xhatSize * sizeof(float));
    memset(pitch_error_kf.P_data,    0, pitch_error_kf.xhatSize * pitch_error_kf.xhatSize * sizeof(float));

    float P_diag[4] = {5.0f, 0.0f, 0.0f, 5.0f};
    memcpy(yaw_error_kf.P_data,   P_diag, sizeof(P_diag));
    memcpy(pitch_error_kf.P_data, P_diag, sizeof(P_diag));

    filtered_yaw_error   = 0;
    filtered_pitch_error = 0;
}

/* ---- 模式切换处理 ---- */
static void SwitchToAutoMode(void)
{
    if (current_mode == VISION_MODE_AUTO)
        return;

    /* 同步角度参考值为当前 IMU 角度 */
    gimbal_yaw_ref   = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
    gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data.Pitch;

    vision_data.manual_updated = 0; // 清除手动数据残留
    VisionPIDReset();
    current_mode = VISION_MODE_AUTO;
    LOGINFO("[CMD] switched to AUTO mode");
}

static void SwitchToManualMode(void)
{
    if (current_mode == VISION_MODE_MANUAL)
        return;

    gimbal_yaw_ref   = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
    gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data.Pitch;

    vision_data.auto_updated = 0; // 清除自动数据残留
    current_mode = VISION_MODE_MANUAL;
    LOGINFO("[CMD] switched to MANUAL mode");
}

/* ---- 命令处理 ---- */
static void ProcessCommand(void)
{
    if (!vision_data.cmd_pending)
        return;

    vision_data.cmd_pending = 0;

    switch (vision_data.cmd)
    {
    case VISION_CMD_SWITCH_AUTO:
        SwitchToAutoMode();
        break;
    case VISION_CMD_SWITCH_MANUAL:
        SwitchToManualMode();
        break;
    case VISION_CMD_LASER_ON:
        laser_on();
        break;
    case VISION_CMD_LASER_OFF:
        laser_off();
        break;
    default:
        break;
    }
}

/* ---- KF + PID 初始化 ---- */
static void VisionKalmanFilterInit()
{
    Kalman_Filter_Init(&yaw_error_kf, 2, 0, 1);
    static float P_init[4] = {5.0f, 0.0f, 0.0f, 5.0f};
    memcpy(yaw_error_kf.P_data, P_init, sizeof(P_init));
    static float F_init[4] = {1.0f, 0.005f, 0.0f, 1.0f};
    memcpy(yaw_error_kf.F_data, F_init, sizeof(F_init));
    static float Q_init[4] = {0.1f, 0.0f, 0.0f, 1.0f};
    memcpy(yaw_error_kf.Q_data, Q_init, sizeof(Q_init));
    static uint8_t measurement_map[1] = {1};
    static float measurement_degree[1] = {1.0f};
    static float mat_R_diagonal[1] = {1.0f};
    static float state_min_variance[2] = {0.001f, 0.001f};
    yaw_error_kf.UseAutoAdjustment = 1;
    memcpy(yaw_error_kf.MeasurementMap,        measurement_map,       sizeof(measurement_map));
    memcpy(yaw_error_kf.MeasurementDegree,     measurement_degree,    sizeof(measurement_degree));
    memcpy(yaw_error_kf.MatR_DiagonalElements, mat_R_diagonal,        sizeof(mat_R_diagonal));
    memcpy(yaw_error_kf.StateMinVariance,      state_min_variance,    sizeof(state_min_variance));

    Kalman_Filter_Init(&pitch_error_kf, 2, 0, 1);
    memcpy(pitch_error_kf.P_data, P_init, sizeof(P_init));
    memcpy(pitch_error_kf.F_data, F_init, sizeof(F_init));
    memcpy(pitch_error_kf.Q_data, Q_init, sizeof(Q_init));
    pitch_error_kf.UseAutoAdjustment = 1;
    memcpy(pitch_error_kf.MeasurementMap,        measurement_map,       sizeof(measurement_map));
    memcpy(pitch_error_kf.MeasurementDegree,     measurement_degree,    sizeof(measurement_degree));
    memcpy(pitch_error_kf.MatR_DiagonalElements, mat_R_diagonal,        sizeof(mat_R_diagonal));
    memcpy(pitch_error_kf.StateMinVariance,      state_min_variance,    sizeof(state_min_variance));
}

void RobotCMDInit()
{
    rc_data = RemoteControlInit(&huart5);

    gimbal_cmd_pub  = PubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
    gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));

    static PID_Init_Config_s yaw_vpid_config = {
        .Kp = 1.5f, .Ki = 3.0f, .Kd = 0.07f,
        .IntegralLimit = 100.0f,
        .CoefA = 20.0f, .CoefB = 20.0f,
        .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
                   PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
        .MaxOut = 200.0f,
    };
    static PID_Init_Config_s pitch_vpid_config = {
        .Kp = 1.0f, .Ki = 2.0f, .Kd = 0.05f,
        .IntegralLimit = 100.0f,
        .CoefA = 10.0f, .CoefB = 30.0f,
        .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
                   PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
        .MaxOut = 200.0f,
    };
    PIDInit(&vision_yaw_pid, &yaw_vpid_config);
    PIDInit(&vision_pitch_pid, &pitch_vpid_config);
    VisionKalmanFilterInit();

    Vision_Uart_Init(&huart10);

    robot_state = ROBOT_READY;
    laser_off();
}

/* ---- 自动 / 视觉追踪模式 ---- */
static void VisionTrackingSet()
{
    gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;

    if (vision_data.auto_updated)
    {
        vision_data.auto_updated = 0;

        yaw_error_kf.MeasuredVector[0] = vision_data.error_x;
        Kalman_Filter_Update(&yaw_error_kf);
        filtered_yaw_error = yaw_error_kf.FilteredValue[0];

        pitch_error_kf.MeasuredVector[0] = vision_data.error_y;
        Kalman_Filter_Update(&pitch_error_kf);
        filtered_pitch_error = pitch_error_kf.FilteredValue[0];

        float yaw_increment   = PIDCalculate(&vision_yaw_pid, filtered_yaw_error, 0);
        float pitch_increment = PIDCalculate(&vision_pitch_pid, filtered_pitch_error, 0);

        gimbal_yaw_ref   += 0.001f * yaw_increment;
        gimbal_pitch_ref += 0.001f * pitch_increment;
    }

    if (gimbal_pitch_ref > 30.0f)
        gimbal_pitch_ref = 30.0f;
    else if (gimbal_pitch_ref < -30.0f)
        gimbal_pitch_ref = -30.0f;

    gimbal_cmd_send.yaw   = gimbal_yaw_ref;
    gimbal_cmd_send.pitch = gimbal_pitch_ref;
}

/* ---- 手动模式 ---- */
static void ManualModeSet()
{
    if (vision_data.manual_updated && vision_data.zero_force)
    {
        /* 零力子模式 */
        vision_data.manual_updated = 0;
        gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
        gimbal_yaw_ref   = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
        gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data.Pitch;
    }
    else if (vision_data.manual_updated)
    {
        /* 上位机角度参考 */
        vision_data.manual_updated = 0;
        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
        gimbal_yaw_ref   = vision_data.yaw_ref;
        gimbal_pitch_ref = vision_data.pitch_ref;
    }
    else
    {
        /* 遥控器 */
        gimbal_cmd_send.gimbal_mode = GIMBAL_GYRO_MODE;
        gimbal_yaw_ref   -= 0.0025f * (float)rc_data[TEMP].rc.rocker_r_;
        gimbal_pitch_ref += 0.00125f * (float)rc_data[TEMP].rc.rocker_r1;
    }

    if (gimbal_pitch_ref > 30.0f)
        gimbal_pitch_ref = 30.0f;
    else if (gimbal_pitch_ref < -30.0f)
        gimbal_pitch_ref = -30.0f;

    gimbal_cmd_send.yaw   = gimbal_yaw_ref;
    gimbal_cmd_send.pitch = gimbal_pitch_ref;
}

/* ---- 急停 ---- */
static void EmergencyHandler()
{
    if (switch_is_down(rc_data[TEMP].rc.switch_right))
    {
        robot_state = ROBOT_STOP;
        gimbal_cmd_send.gimbal_mode = GIMBAL_ZERO_FORCE;
        gimbal_cmd_send.yaw   = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
        gimbal_cmd_send.pitch = gimbal_fetch_data.gimbal_imu_data.Pitch;
        laser_off();
    }

    if (!switch_is_down(rc_data[TEMP].rc.switch_right) && robot_state == ROBOT_STOP)
    {
        robot_state = ROBOT_READY;
        gimbal_yaw_ref   = gimbal_fetch_data.gimbal_imu_data.YawTotalAngle;
        gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data.Pitch;
        LOGINFO("[CMD] reinstate, robot ready");
    }
}

void RobotCMDTask()
{
    SubGetMessage(gimbal_feed_sub, &gimbal_fetch_data);

    /* 优先处理串口命令 (模式切换 / 激光等) */
    ProcessCommand();

    EmergencyHandler();

    if (robot_state == ROBOT_STOP)
    {
        PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
        return;
    }

    /* 根据当前模式执行控制 */
    if (current_mode == VISION_MODE_AUTO)
        VisionTrackingSet();
    else
        ManualModeSet();

    PubPushMessage(gimbal_cmd_pub, (void *)&gimbal_cmd_send);
}