/*******************************************************************************
 * 文件名称 : motion_control_task.c
 * 文件说明 : 割草机运动控制 FreeRTOS 任务封装
 * 创建日期 : 2025-11-22
 *******************************************************************************/

#include "motion_control_task.h"

#include "FreeRTOS.h"
#include "task.h"

#include "stm32h7xx_hal.h"
#include <string.h>

#include "DBG.h"
#include "GPS.h"
#include "PythonLink.h"
#include "geo_utils.h"
#include "motion_config.h"
#include "motion_control.h"
#include "motion_path_data.h"
#include "pwm_ctrol.h"

#define RAD2DEG                      (57.29577951308232f)

#define MC_TASK_STACK_WORDS         (2048U)
#define MC_TASK_PRIORITY            (tskIDLE_PRIORITY + 4U)
#define MC_TASK_PERIOD_TICKS        (pdMS_TO_TICKS((HIDO_UINT32)(1000.0f / MC_CFG_CONTROL_HZ)))
#define MC_CLAMP(v, lo, hi)         ((v) < (lo) ? (lo) : (((v) > (hi)) ? (hi) : (v)))

extern TaskHandle_t g_app_task_handle;

static TaskHandle_t g_motion_task_handle = NULL;
static MC_State g_motion_state;
static MC_Config g_motion_config;
static ST_GeoOrigin g_motion_origin;
static HIDO_UINT32 g_last_gps_ms = 0U;
static HIDO_UINT32 g_last_imu_ms = 0U;
static HIDO_UINT32 g_last_gprmi_tow = 0U;
static HIDO_UINT32 g_last_control_report_ms = 0U;
static HIDO_UINT32 g_last_pose_report_ms = 0U;
static HIDO_UINT32 g_last_state_report_ms = 0U;
static HIDO_UINT32 g_last_stack_report_ms = 0U;
static HIDO_FLOAT g_control_freq_hz = 0.0f;
static HIDO_UINT32 g_freq_sample_count = 0U;
static HIDO_UINT32 g_freq_sample_start_ms = 0U;
static HIDO_FLOAT g_last_enu[3] = {0.0f, 0.0f, 0.0f};
static HIDO_FLOAT g_last_heading_deg = 0.0f;
static HIDO_FLOAT g_last_pitch_deg = 0.0f;
static HIDO_FLOAT g_last_roll_deg = 0.0f;
static HIDO_BOOL g_last_pose_valid = HIDO_FALSE;
static HIDO_FLOAT g_last_target_xy[2] = {0.0f, 0.0f};
static HIDO_BOOL g_last_target_valid = HIDO_FALSE;
static HIDO_UINT16 g_last_steering_pwm = MC_CFG_PWM_CENTER_US;
static HIDO_UINT16 g_last_throttle_pwm = MC_CFG_PWM_CENTER_US;

static HIDO_UINT32 g_last_sensor_timestamp_ms = 0U;

static void MotionControl_TaskEntry(void *argument);
static void MotionControl_ApplyOutput(const MC_Output *output, HIDO_UINT16 *_pu16Steering, HIDO_UINT16 *_pu16Throttle);
static void MotionControl_StopOutputs(void);
static const HIDO_CHAR *MotionControl_StageLabel(E_MCStage stage);

/* 初始化控制任务并创建 FreeRTOS 线程（只做一次） */
HIDO_VOID MotionControl_TaskInit(HIDO_VOID)
{
    if (g_motion_task_handle != NULL)
    {
        return;
    }

    MC_DefaultConfig(&g_motion_config);
    Geo_OriginInit(&g_motion_origin,
                   MC_CFG_ORIGIN_LAT_DEG,
                   MC_CFG_ORIGIN_LON_DEG,
                   MC_CFG_ORIGIN_ALT_M);

    MC_Init(&g_motion_state,
            &g_motion_config,
            g_motion_path_xy,
            g_motion_path_point_count);

    BaseType_t ret = xTaskCreate(
        MotionControl_TaskEntry,
        "MotionCtrl",
        MC_TASK_STACK_WORDS,
        NULL,
        MC_TASK_PRIORITY,
        &g_motion_task_handle);

    if (ret != pdPASS)
    {
        g_motion_task_handle = NULL;
        DBG_Printf("[MotionCtrl] Task create failed\r\n");
    }
}

/* 查询控制任务是否已经创建 */
HIDO_BOOL MotionControl_IsRunning(HIDO_VOID)
{
    return (g_motion_task_handle != NULL) ? HIDO_TRUE : HIDO_FALSE;
}

/* 高优先级任务：采集 → 控制 → PWM 输出 → 反馈 */
static void MotionControl_TaskEntry(void *argument)
{
    (void)argument;
    const TickType_t period = (MC_TASK_PERIOD_TICKS == 0U) ? pdMS_TO_TICKS(13U) : MC_TASK_PERIOD_TICKS;
    TickType_t last_wake = xTaskGetTickCount();
    ST_GPRMI gprmi;
    ST_GPIMU gpimu;
    float enu[3];

    DBG_Printf("[MotionCtrl] Task started (%.1f Hz)\r\n", MC_CFG_CONTROL_HZ);

    for (;;)
    {
        vTaskDelayUntil(&last_wake, period);

        /* --- 1) 采集最新 GPS/IMU 数据（轮询缓存，含超时保护） --- */
        HIDO_BOOL gps_valid = (GPS_GetGPRMI(&gprmi) == HIDO_OK);
        HIDO_BOOL imu_valid = (GPS_GetGPIMU(&gpimu) == HIDO_OK);
        HIDO_UINT32 now = HAL_GetTick();

        if (gps_valid == HIDO_TRUE && gprmi.m_u32TimeOfWeek != g_last_gprmi_tow)
        {
            /* 新的GPS数据到来（时间戳变化），才更新航向 */
            Geo_GprmiToENU(&gprmi, &g_motion_origin, enu);
            MC_UpdateGps(&g_motion_state, enu, &gprmi);
            memcpy(g_last_enu, enu, sizeof(enu));
            g_last_heading_deg = gprmi.m_fHeadingAngle;
            g_last_pitch_deg = gprmi.m_fPitchAngle;
            g_last_roll_deg = gprmi.m_fRollAngle;
            g_last_pose_valid = HIDO_TRUE;
            g_last_gps_ms = now;
            g_last_gprmi_tow = gprmi.m_u32TimeOfWeek;
            g_last_sensor_timestamp_ms = gprmi.m_u32TimeOfWeek;
        }
        else if (gps_valid == HIDO_FALSE && (now - g_last_gps_ms) > 200U)
        {
            g_motion_state.pose_valid = HIDO_FALSE;
            g_last_pose_valid = HIDO_FALSE;
        }

        if (imu_valid == HIDO_TRUE)
        {
            MC_UpdateImu(&g_motion_state, &gpimu);
            g_last_imu_ms = now;
            if (gpimu.m_u32UTCTime != 0U)
            {
                g_last_sensor_timestamp_ms = gpimu.m_u32UTCTime;
            }
        }
        else if ((now - g_last_imu_ms) > 200U)
        {
            g_motion_state.imu_valid = HIDO_FALSE;
        }

        /* --- 2) 调用运动控制器（固定 75 Hz dt） --- */
        MC_Output output;
        MC_Compute(&g_motion_state, 1.0f / MC_CFG_CONTROL_HZ, &output);

        /* --- 3) 根据控制量更新 PWM，并把 forward/turn 回传给 Python --- */
        HIDO_UINT16 applied_steering = MC_CFG_PWM_CENTER_US;
        HIDO_UINT16 applied_throttle = MC_CFG_PWM_CENTER_US;
        if (output.active == HIDO_TRUE)
        {
            MotionControl_ApplyOutput(&output, &applied_steering, &applied_throttle);
        }
        else
        {
            MotionControl_StopOutputs();
            applied_steering = MC_CFG_PWM_CENTER_US;
            applied_throttle = MC_CFG_PWM_CENTER_US;
        }
        g_last_steering_pwm = applied_steering;
        g_last_throttle_pwm = applied_throttle;

        memcpy(g_last_enu, output.pos_enu, sizeof(g_last_enu));
        g_last_heading_deg = output.heading_deg;
        g_last_pitch_deg = output.pitch_deg;
        g_last_roll_deg = output.roll_deg;
        g_last_pose_valid = g_motion_state.pose_valid;
        g_last_target_valid = output.target_valid;
        if (output.target_valid == HIDO_TRUE)
        {
            g_last_target_xy[0] = output.target_xy[0];
            g_last_target_xy[1] = output.target_xy[1];
        }

        g_freq_sample_count++;
        if (g_freq_sample_start_ms == 0U)
        {
            g_freq_sample_start_ms = now;
        }
        else
        {
            HIDO_UINT32 window = now - g_freq_sample_start_ms;
            if (window >= 1000U)
            {
                if (window > 0U)
                {
                    g_control_freq_hz = (HIDO_FLOAT)g_freq_sample_count * 1000.0f / (HIDO_FLOAT)window;
                }
                g_freq_sample_count = 0U;
                g_freq_sample_start_ms = now;
            }
        }

        const HIDO_CHAR *stage_label = MotionControl_StageLabel(output.stage);
        const HIDO_FLOAT *target_ptr = (output.target_valid == HIDO_TRUE) ? output.target_xy : HIDO_NULL;
        PythonLink_ReportControl(output.forward_mps,
                                 output.turn_rate,
                                 g_control_freq_hz,
                                 g_last_steering_pwm,
                                 g_last_throttle_pwm,
                                 stage_label,
                                 g_last_sensor_timestamp_ms,
                                 output.pos_enu,
                                 output.heading_deg,
                                 output.target_heading_deg,
                                 target_ptr);
        if ((now - g_last_state_report_ms) >= 1000U)
        {
            g_last_state_report_ms = now;
            PythonLink_ReportState(stage_label,
                                   output.cross_track_error,
                                   output.heading_error * RAD2DEG,
                                   g_last_sensor_timestamp_ms);
        }

        if ((now - g_last_pose_report_ms) >= 1000U && g_last_pose_valid == HIDO_TRUE)
        {
            g_last_pose_report_ms = now;
            PythonLink_ReportPose(g_last_enu,
                                  g_last_heading_deg,
                                  g_last_pitch_deg,
                                  g_last_roll_deg,
                                  g_last_target_valid ? g_last_target_xy : NULL,
                                  g_last_sensor_timestamp_ms);
        }

        if ((now - g_last_stack_report_ms) >= 10000U)
        {
            g_last_stack_report_ms = now;
            UBaseType_t motion_hw = uxTaskGetStackHighWaterMark(g_motion_task_handle);
            UBaseType_t app_hw = (g_app_task_handle != NULL) ? uxTaskGetStackHighWaterMark(g_app_task_handle) : 0U;
            UBaseType_t heap_free = xPortGetFreeHeapSize();
            UBaseType_t heap_min = xPortGetMinimumEverFreeHeapSize();
            PythonLink_ReportStack("Motion", (HIDO_UINT32)motion_hw, (HIDO_UINT32)heap_free, (HIDO_UINT32)heap_min);
            if (g_app_task_handle != NULL)
            {
                PythonLink_ReportStack("App", (HIDO_UINT32)app_hw, (HIDO_UINT32)heap_free, (HIDO_UINT32)heap_min);
            }
        }
    }
}

/* 将控制输出映射为 PWM，并发回 Python 端 */
static void MotionControl_ApplyOutput(const MC_Output *output,
                                      HIDO_UINT16 *_pu16Steering,
                                      HIDO_UINT16 *_pu16Throttle)
{
    float forward_ratio = output->forward_mps / g_motion_config.max_forward_mps;
    float turn_ratio = output->turn_rate / g_motion_config.max_turn_rate;
    forward_ratio = MC_CLAMP(forward_ratio, -1.0f, 1.0f);
    turn_ratio = MC_CLAMP(turn_ratio, -1.0f, 1.0f);

    int32_t throttle = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(forward_ratio * (float)MC_CFG_PWM_SPAN_US);
    int32_t steering = (int32_t)MC_CFG_PWM_CENTER_US + (int32_t)(turn_ratio * (float)MC_CFG_PWM_SPAN_US);

    if (throttle < 1000)
    {
        throttle = 1000;
    }
    else if (throttle > 2000)
    {
        throttle = 2000;
    }

    if (steering < 1000)
    {
        steering = 1000;
    }
    else if (steering > 2000)
    {
        steering = 2000;
    }

    Set_Motor_Pulse((uint32_t)throttle);
    Set_Steering_Pulse((uint32_t)steering);

    if (_pu16Steering != NULL)
    {
        *_pu16Steering = (HIDO_UINT16)steering;
    }
    if (_pu16Throttle != NULL)
    {
        *_pu16Throttle = (HIDO_UINT16)throttle;
    }
}

/* 失效保护：回中 PWM 并上报零控制量 */
static void MotionControl_StopOutputs(void)
{
    Set_Motor_Pulse(MC_CFG_PWM_CENTER_US);
    Set_Steering_Pulse(MC_CFG_PWM_CENTER_US);
    g_last_steering_pwm = MC_CFG_PWM_CENTER_US;
    g_last_throttle_pwm = MC_CFG_PWM_CENTER_US;
}

static const HIDO_CHAR *MotionControl_StageLabel(E_MCStage stage)
{
    switch (stage)
    {
    case MC_STAGE_GOTO_START:
        return "goto_start";
    case MC_STAGE_FOLLOW_PATH:
        return "follow_path";
    case MC_STAGE_FINISHED:
        return "finished";
    default:
        return "idle";
    }
}