#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__

#include "Arduino.h"

typedef struct
{
    float per_pulse_distance;   // 每个脉冲的前进的距离
    int16_t motor_speed;        // 单位用mm/s
    int64_t last_encoder_ticks; // 上一次点击的编码器读数
} motor_param_t;


typedef struct {
    float x;
    float y;
    float angle;
    float linear_speed;
    float angular_speed;
}odom_t;

/**
 * 1. 运动学正逆解（两个轮子的实时速度->当前实时的角速度和线速度 / 当前目标的角速度和线速度->两个轮子的目标速度）
 *
 */
class Kinematics
{
private:
    /* data */
    motor_param_t motor_param[2];
    int16_t delta_ticks[2] = {0, 0}; // 用于存储上一次读取的编码器数值
    uint64_t last_update_time = 0; // 用于存储上一次更新电机速度的时间,计算速度的时候使用
    float wheel_distance = 0.0;    // 两个轮子之间的距离
    odom_t odom; // 用于存储里程计信息
public:
    Kinematics(/* args */) = default;
    ~Kinematics() = default;

    odom_t& get_odom();
    void update_odom(uint16_t dt);
    void TransAngleInPI(float angle,float& out_angle);

    void set_wheel_distance(float distance); // 设置轮子的间距

    void set_motor_param(uint8_t id,float per_pluse_distance);
    // 运动学正解,将左右轮的速度转换成线速度和角速度
    void kinematics_forward(float left_speed,float right_speed,float* out_linear_speed,float* out_angular_speed);
    // 运动学逆解，将线速度和角速度转换成左右轮的速度
    void kinematics_inverse(float linear_speed,float angular_speed,float* out_left_speed,float* out_right_speed);
    // 更新点击速度和编码器数据
    void update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick);
    // 获取电机速度,返回值是速度
    int16_t get_motor_speed(uint8_t id);

};

#endif // __KINEMATICS_H__