#ifndef SIMPLE_QUADRUPED_CONTROL_HPP
#define SIMPLE_QUADRUPED_CONTROL_HPP

#include "rclcpp/rclcpp.hpp"
#include "rc_msgs/msg/ps2_data.hpp"
#include "rc_msgs/msg/motor_cmds.hpp"
#include "rc_msgs/msg/data_motors.hpp"

struct ps2_t
{
    float lx;
    float ly;
    float rx;
    float ry;

    float up_down;
    float left_right;

    bool l1;
    bool l2;
    bool r1;
    bool r2;

    bool x;
    bool y;
    bool a;
    bool b;

    bool select;
    bool start;
};

struct motor_cmd_t
{
    float pos;
    float vw;
    float tau;
    float kp;
    float kd;
};

struct motor_cmds_t
{
    motor_cmd_t motors[3]; // 使用数组替代单独的字段
};

struct data_motor_t
{
    float pos;
    float vw;
    float tau;
};

struct data_motors_t
{
    data_motor_t motors[3]; // 使用数组替代单独的字段
};

struct mech_zero_t
{
    float motors[3]; // 使用数组替代单独的字段
};

struct cmd_t
{
    float pos;
    float vw;
    float tau;
    float kp;
    float kd;
};

struct cmds_t
{
    cmd_t motors[3]; // 使用数组替代单独的字段
};

ps2_t ps2;
motor_cmds_t motor_cmds;
data_motors_t data_motors;
mech_zero_t mech_zero;
cmds_t cmds;

class QuadrupedSubscriber : public rclcpp::Node
{
public:
    QuadrupedSubscriber();

private:
    void ps2_data_callback(const rc_msgs::msg::Ps2Data::SharedPtr msg);
    void data_motors_callback(const rc_msgs::msg::DataMotors::SharedPtr msg);
    float limit_torque(const motor_cmd_t &motor_cmd, const data_motor_t &data_motor, float limit);
    void publish_motor_cmds();

    rclcpp::Subscription<rc_msgs::msg::Ps2Data>::SharedPtr ps2_data_subscriber_;
    rclcpp::Publisher<rc_msgs::msg::MotorCmds>::SharedPtr motor_cmds_publisher_;
    rclcpp::Subscription<rc_msgs::msg::DataMotors>::SharedPtr data_motors_subscriber_;
};

#endif // SIMPLE_QUADRUPED_CONTROL_HPP