cmvr-es/cmvr-es/devices/canbus/canopen/sdo_response_protocol.h

//
// Created by lgv on 2025/7/17.
//

#pragma once

#include "canbus/can_comm/protocol_data.h"
#include "cmvr/msgs/robot_detail.pb.h"
#include "sdo_response_protocol.h"
#include "common/base/logging/logger.h"
namespace cmvr {
    namespace device {
        template<typename SensorType>
        class SdoResponseProtocol : public ProtocolData<SensorType> {
        public:
            static constexpr uint32_t BASE_ID = 0x580;

            static uint32_t ID(uint8_t node_id) {
                return BASE_ID + node_id;
            }
            uint32_t ID() const{
                return BASE_ID + node_id_;
            }

            explicit SdoResponseProtocol(uint8_t node_id) : node_id_(node_id), message_id_(BASE_ID + node_id) {
            }

            void Parse(const std::uint8_t *bytes, int32_t length, SensorType *sensor_data) const override;

            virtual void ParseSdoData(const msgs::SdoFrame &sdo_response, SensorType *sensor_data) const {}

            void UpdateData(uint8_t *data) override {}

        protected:
            uint8_t node_id_;
            uint32_t message_id_;
            mutable msgs::SdoFrame sdo_response_; // 成员变量
        };


        template<typename SensorType>
        void SdoResponseProtocol<SensorType>::Parse(const std::uint8_t *bytes, int32_t length,SensorType *sensor_data) const {
            if (length < 8) {
                CMVR_LOG(WARNING) << "MotorSdoResponseProtocol: data length too short: " << length;
                return;
            }

            // 解析 command 字段（第0字节）
            auto command = static_cast<msgs::CommandSpecifier>(bytes[0]);

            // 解析 index（字节1和字节2，低字节优先）
            const uint32_t index = bytes[1] + (bytes[2] << 8);

            // 解析 subindex（字节3）
            const uint32_t subindex = bytes[3];

            // 根据 command 解析 data（字节4~7）
            uint32_t data = 0;
            switch (command) {
                case msgs::CS_READ_RESPONSE_ONE_BYTE:
                    data = bytes[4];
                    break;
                case msgs::CS_READ_RESPONSE_TWO_BYTES:
                    data = bytes[5] + (bytes[4] << 8);
                    break;
                case msgs::CS_READ_RESPONSE_THREE_BYTES:
                    data = bytes[4] + (bytes[5] << 8) + (bytes[6] << 16);
                           (static_cast<uint32_t>(bytes[5]) << 8) |
                           (static_cast<uint32_t>(bytes[6]) << 16);
                    break;
                case msgs::CS_READ_RESPONSE_FOUR_BYTES:
                    data = bytes[4] + (bytes[5] << 8) + (bytes[6] << 16) + (bytes[7] << 24);
                    break;

                case msgs::CS_WRITE_SUCCESS_RESPONSE:
                    data = 0;
                    break;
                case msgs::CS_EXCEPTION_RESPONSE:
                    break;

                default:
                    CMVR_LOG(WARNING) << "Unknown command specifier: " << std::hex << static_cast<int>(command);
                    data = 0;
                    break;
            }

            sdo_response_.set_node_id(node_id_);
            sdo_response_.set_cs(command);
            sdo_response_.set_index(index);
            sdo_response_.set_sub_index(subindex);
            sdo_response_.set_data(data);

            // 子类实现的扩展解析data逻辑
            ParseSdoData(sdo_response_, sensor_data);
        }
    }
}