/**
 ******************************************************************************
 * @file           : heading_diff_ctrl_c.cpp
 * @author         : wangyingjie
 * @brief          : None
 * @attention      : None
 * @date           : 2025/5/23
 ******************************************************************************
 */

#include "heading_diff_ctrl_c.h"


//// 小船控制结构体
//typedef struct
//{
//    studio_pid heading_pid;  // 方位角PID控制器
//    studio_point_c target;   // 目标点
//    double base_throttle;    // 基础油门
//    double left_throttle;    // 左侧油门
//    double right_throttle;   // 右侧油门
//} diff_ctrl;

// 初始化小船控制器
void init_diff_ctrl(diff_ctrl *controller,
                          const studio_point_c *target,
                          double base_throttle,  // 输入范围0~1.0
                          double Kp,
                          double Ki,
                          double Kd)
{
    init_pid(&controller->heading_pid, 0, 0, Kp, Ki, Kd, 1.0, 0.3);
    controller->target = *target;
    controller->base_throttle = base_throttle;
    controller->left_throttle = base_throttle;
    controller->right_throttle = base_throttle;
}

void reset_diff_ctrl_goal(diff_ctrl* controller, const studio_point_c* target)
{
    controller->target = *target;
    set_goal(&controller->heading_pid, 0);
}

// 正确计算最短路径误差（返回-180~180°）
double calculate_heading_error(double target_deg, double current_deg)
{
    double error = target_deg - current_deg;
    error = fmod(error + 360.0, 360.0);  // 转0~360°
    if (error > 180.0)
    {
        error -= 360.0;
    }
    return error;
}

double update_boat(diff_ctrl *controller, const studio_point_c *current_pos, double current_heading_deg)
{
    // 1. 计算目标方位角（确保函数正确）
    const double target_deg = azimuth_angle_gcs_c(current_pos, &controller->target);

    // 2. 计算最短路径误差
    double error = calculate_heading_error(target_deg, current_heading_deg);
    double steer = 0.0;

    // 3. PID控制
    if (fabs(error) > 1.0)  // 仅当误差超过1度时进行纠偏
    {
        set_goal(&controller->heading_pid, 0);
        steer = compute_pid(&controller->heading_pid, error);
    }
    else
    {

    }

    // 4. 差速控制（关键修正）
    controller->left_throttle = controller->base_throttle - steer;
    controller->right_throttle = controller->base_throttle + steer;

    // 5. 限幅保护
    controller->left_throttle = fmax(0.0, fmin(1.0, controller->left_throttle));
    controller->right_throttle = fmax(0.0, fmin(1.0, controller->right_throttle));

    return distance_squared_gcs_c(current_pos, &controller->target);
}

void simulate_movement(studio_point_c *current_pos, double *current_heading, double left_throttle, double right_throttle)
{
    const double dt = 0.1;         // 时间间隔（秒）
    const double max_speed = 2;  // 最大速度（米/秒）
    const double turn_rate = 40;  // 转向速率（度/秒 per 差速百分比）

    // 保存原始位置用于计算位移
    const studio_point_c original_pos = *current_pos;

    // 1. 计算航向变化 ---------------------------------------------
    double steer_diff = left_throttle - right_throttle;
    *current_heading += turn_rate * steer_diff * dt;
    *current_heading = fmod(*current_heading + 360.0, 360.0);

    // 2. 计算位移量（米）------------------------------------------
    double avg_throttle = (left_throttle + right_throttle) / 2.0;
    double speed = max_speed * avg_throttle;

    // 将航向角转换为弧度
    double radian = *current_heading * AO_M_PI / 180.0;

    // 计算东向和北向位移（单位：米）
    const double delta_east = speed * sin(radian) * dt;   // 东向位移（+东/-西）
    const double delta_north = speed * cos(radian) * dt;  // 北向位移（+北/-南）

    // 3. 通过投影转换更新位置 -------------------------------------
    *current_pos = lonlat_move_by(&original_pos, delta_east, delta_north);
}