#ifndef CMVR_ES_BIOHEAD_ESP32_H
#define CMVR_ES_BIOHEAD_ESP32_H

#include "devices/abstract_biohead.h"
#include "../../../../include/hardware/serial/RS485/esp32_serial_port.h"
#include <vector>
#include <string>
#include <memory>
#include <mutex>

namespace cmvr::device {

    struct ServoChannel {
        uint8_t addr;
        uint8_t channel;
    };

    class BioHeadRobot : public AbstractBiohead {
    public:
        explicit BioHeadRobot(const XmlNode &config);
        ~BioHeadRobot() override = default;

        // 接口实现
        void getState(RobotState &state) override;
        void eStop() override;
        void setExpressionPose(FacialExpressionState& expression_state, double vel = 0.5, double acc = 0.1) override;
        void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) override;


    private:
        // 内部方法
        void parseXmlConfig(const XmlNode &node);
        void sendServoCommands( const std::vector<double>& targets, uint16_t duration_ms);
        uint16_t angleToRaw(double angle);
        double normalizeToAngle(double normalized, size_t index);

        std::shared_ptr<SerialPort> serial_;
        std::string port_name_;

        std::vector<ServoChannel> channels_;
        std::vector<double> current_joints_;
        std::mutex stateMutex_;  // 状态保护锁

        std::vector<double> min_angles_;
        std::vector<double> max_angles_;


        double angle_deadband_{36};
        std::vector<double> last_joints_;        // 上次下发角度缓存


    };

} // namespace cmvr::device

#endif // CMVR_ES_BIOHEAD_ESP32_H