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

#ifndef HEADING_DIFF_CTRL_H
#define HEADING_DIFF_CTRL_H

//#ifdef __cplusplus
//extern "C"
//{
//#endif

#include "control/pid/studio_pid_c.h"
#include "geography/studio_proj_c.h"
#include "geometry/studio_geo_algo_c.h"

//#ifdef __cplusplus
//}
//#endif

// 测试使用

//#include "geometry/studio_geo_utils.h"
//#include "geometry/studio_geo_algo.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);

void reset_diff_ctrl_goal(diff_ctrl *controller, const studio_point_c *target);

// 正确计算最短路径误差（返回-180~180°）
double calculate_heading_error(double target_deg, double current_deg);

double update_boat(diff_ctrl *controller, const studio_point_c *current_pos, double current_heading_deg);

void simulate_movement(studio_point_c *current_pos, double *current_heading, double left_throttle, double right_throttle);


#endif  // HEADING_DIFF_CTRL_H