#ifndef CHASSIS_H
#define CHASSIS_H

#include <stdint.h>
#include "dji_motor.h"

/* 底盘模块:麦轮逆解 + 四轮转速 PID + 跟随云台(双 INS 世界系,2026-09 重写)
 * 电机接线(与 legacy 实际发送一致):
 *   0x201 右前  0x202 左前  0x203 左后  0x204 右后 */

extern dji_motor_t motor_wheel_rf;  // 0x201 右前
extern dji_motor_t motor_wheel_lf;  // 0x202 左前
extern dji_motor_t motor_wheel_lr;  // 0x203 左后
extern dji_motor_t motor_wheel_rr;  // 0x204 右后
extern dji_motor_t motor_yaw_follow;  // Yaw GM6020,CAN2 0x205(跟随反馈)

extern void chassis_motor_init(void);

/* 判定任一轮电机掉线 */
extern uint8_t chassis_motor_offline(void);

/* 底盘运动控制(2ms 调用)
 * vx:前进(+)/后退(-)  vy:左(+)/右(-)  w:逆时针(+)旋转
 * power_scale:功率限制系数(0~1),直接缩放输出电流 */
extern void chassis_control(float vx, float vy, float w, float power_scale);

/* 底盘自检:小 w 脉冲+底盘 INS 测"w 正向=底盘 CCW/CW",与云台下发的
 * 编码器方向综合出跟随符号(阻塞约 200ms,整车会轻转)。
 * 返回 1-成功 0-失败 */
extern uint8_t chassis_detect_follow_sign(void);

/* 综合后的跟随符号(+1/-1;自检未完成时为参数回退值) */
extern float chassis_follow_sign(void);

/* 跟随一步(2ms,FOLLOW 模式调用;反馈为编码器相对角,零漂移) */
extern float chassis_follow_w(void);

/* 请求下个周期重锁基准并清积分 */
extern void chassis_follow_reset(void);

/* 底盘输出清零(停车) */
extern void chassis_output_zero(void);

/* 跟随环诊断导出(debug_task 打印) */
extern volatile float dbg_follow_err_deg;
extern volatile float dbg_follow_out;
extern volatile float dbg_follow_sign;

/* 将以云台为基准的平移速度(vx_g, vy_g)旋转投影到底盘自身坐标系(vx_c, vy_c)
 * 无论底盘是否处于小陀螺自旋状态，使 vx_g 始终代表沿云台枪口方向前进 */
extern void chassis_vector_to_body(float vx_g, float vy_g, float w, float * vx_c, float * vy_c);

#endif