XNSim/XNCore/XNDDSInterface.h

#pragma once

#include "XNFramework.h"
#include "XNDDSManager.h"
#include "XNByteArray.h"
#include "XNTypeTraits.h"
#include <stdexcept>

// 定义UDP包的最大大小
constexpr size_t MAX_UDP_PACKET_SIZE = 40000;

class XNDDSInterface
{
public:
	XNDDSInterface()		  = default;
	virtual ~XNDDSInterface() = default;

public:
	/**
	 * @brief 初始化
	 * @param framework: 框架
	 */
	virtual void Initialize(XNFrameworkPtr framework, uint32_t modelID, uint32_t DDS_type) = 0;

	/**
	 * @brief 获取该接口的UDP包
	 * @return 字节数组
	 */
	XNByteArray getUDPPackage();

	/**
	 * @brief 通过UDP包设置数据
	 * @param package: UDP包
	 */
	void setDataByUDPPackage(const XNByteArray &package);

	/**
	 * @brief 批量获取指定变量的数据
	 * @param varNames: 变量名列表
	 * @return: 变量名到数据的映射
	 */
	std::unordered_map<std::string, std::string>
	getStringData(const std::vector<std::string> &varNames);

	/**
	 * @brief 批量设置指定变量的数据
	 * @param data: 变量名到数据的映射
	 */
	void setDataByString(const std::unordered_map<std::string, std::string> &data);

protected:
	/**
	 * @brief 获取指定变量的字节数据，用于网络通信
	 * @param data: 数据
	 * @return 字节数组
	 */
	template <typename T>
	XNByteArray getByteArray(const eprosima::fastcdr::optional<T> &data)
	{
		XNByteArray result(getTypeSize<T>());

		if constexpr (std::is_arithmetic_v<T>) {
			if (data) {
				std::memcpy(result.data(), &data.value(), sizeof(T));
			} else {
				T zero = 0;
				std::memcpy(result.data(), &zero, sizeof(T));
			}
		} else if constexpr (is_std_array_v<T>) {
			if (data) {
				return getByteArrayFromStdArray(data.value());
			} else {
				T zero = {};
				return getByteArrayFromStdArray(zero);
			}
		} else {
			static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
						  "T 必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 78");
		}

		return result;
	}

	/**
	 * @brief 设置指定变量的字节数据，用于网络通信
	 * @param data: 数据
	 * @param byteArray: 字节数组
	 */
	template <typename T>
	void setByteArray(eprosima::fastcdr::optional<T> &data, const XNByteArray &byteArray,
					  uint32_t &pos)
	{
		uint32_t thisSize = getTypeSize<T>();
		XNByteArray thisArray(thisSize);
		if (pos + thisSize > byteArray.size()) {
			return;
		}
		std::memcpy(thisArray.data(), byteArray.data() + pos, thisSize);
		pos += thisSize;

		if constexpr (std::is_arithmetic_v<T>) {
			T temp;
			std::memcpy(&temp, thisArray.data(), sizeof(T));
			data = temp;
		} else if constexpr (is_std_array_v<T>) {
			if (!data) {
				data = T{};
			}
			setByteArrayFromStdArray(data.value(), thisArray);
		} else {
			static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
						  "T 必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 112");
		}
	}

	/**
	 * @brief 获取指定数组变量的字节数据，用于网络通信
	 * @param data: 数据
	 * @return 字节数组
	 */
	template <typename T, std::size_t N>
	XNByteArray getByteArrayFromStdArray(const std::array<T, N> &data)
	{
		XNByteArray result(getTypeSize<T>() * N);

		for (std::size_t i = 0; i < N; ++i) {
			if constexpr (std::is_arithmetic_v<T>) {
				std::memcpy(result.data() + i * getTypeSize<T>(), &data[i], getTypeSize<T>());
			} else if constexpr (is_std_array_v<T>) {
				XNByteArray subArray = getByteArrayFromStdArray(data[i]);
				std::memcpy(result.data() + i * getTypeSize<T>(), subArray.data(),
							getTypeSize<T>());
			} else {
				static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
							  "T 必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 135");
			}
		}

		return result;
	}

	/**
	 * @brief 设置指定数组变量的字节数据，用于网络通信
	 * @param data: 数据
	 * @param byteArray: 字节数组
	 */
	template <typename T, std::size_t N>
	void setByteArrayFromStdArray(std::array<T, N> &data, const XNByteArray &byteArray)
	{
		if (byteArray.size() < getTypeSize<T>() * N)
			return;

		for (std::size_t i = 0; i < N; ++i) {
			if constexpr (std::is_arithmetic_v<T>) {
				std::memcpy(&data[i], byteArray.data() + i * getTypeSize<T>(), getTypeSize<T>());
			} else if constexpr (is_std_array_v<T>) {
				XNByteArray subArray(byteArray.data() + i * getTypeSize<T>(), getTypeSize<T>());
				setByteArrayFromStdArray(data[i], subArray);
			} else {
				static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
							  "T 必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 160");
			}
		}
	}

	/**
	 * @brief 获取指定变量的字符串数据，用于JSON前端
	 * @param data: 数据
	 * @return: 数据（字符串格式）
	 */
	template <typename T>
	std::string getString(const eprosima::fastcdr::optional<T> &data)
	{
		if constexpr (std::is_arithmetic_v<T>) {
			if (data) {
				return std::to_string(data.value());
			} else {
				return "Unknown";
			}
		} else if constexpr (is_std_array_v<T>) {
			if (data) {
				return getStringFromStdArray(data.value());
			} else {
				T zero = {};
				return getStringFromStdArray(zero);
			}
		} else {
			static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
						  "T 必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 188");
		}
		return std::string();
	}

	/**
	 * @brief 通过字符串数据设置指定变量，用于JSON前端
	 * @param data: 数据
	 * @param value: 字符串数据
	 */
	template <typename T>
	void setDataFromString(eprosima::fastcdr::optional<T> &data, const std::string &value)
	{
		if constexpr (std::is_arithmetic_v<T>) {
			if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
				data = std::stod(value);
			} else {
				data = std::stoll(value);
			}
		} else if constexpr (is_std_array_v<T>) {
			// 解析输入字符串
			std::stringstream ss(value);
			std::string item;
			std::vector<std::string> items;
			while (std::getline(ss, item, ',')) {
				items.push_back(item);
			}
			T temp;
			setStdArrayFromString(temp, items, 0);
			data = temp;
		} else {
			static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
						  "T 必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 220");
		}
	}

	/**
	 * @brief 获取指定数组变量的字符串数据，用于JSON前端
	 * @param data: 数据
	 * @return: 数据（字符串格式）
	 */
	template <typename T, std::size_t N>
	std::string getStringFromStdArray(const std::array<T, N> &data)
	{
		std::stringstream ss;
		for (std::size_t i = 0; i < N; ++i) {
			if (i > 0)
				ss << ",";
			if constexpr (std::is_arithmetic_v<T>) {
				ss << data[i];
			} else if constexpr (is_std_array_v<T>) {
				ss << getStringFromStdArray(data[i]);
			} else {
				static_assert(std::is_arithmetic_v<T> || is_std_array_v<T>,
							  "T 必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 243");
			}
		}
		return ss.str();
	}

	/**
	 * @brief 通过字符串数据设置指定数组变量，用于JSON前端
	 * @param data: 数据
	 * @param value: 字符串数据，格式为"数字,数字,..."
	 * @param start_pos: 当前数组在输入字符串中的起始位置
	 * @return 处理完当前数组后，下一个数组的起始位置
	 * @throw std::runtime_error: 当输入数据格式不正确时抛出异常
	 */
	template <typename T, std::size_t N>
	size_t setStdArrayFromString(std::array<T, N> &data, const std::vector<std::string> &value,
								 size_t start_pos = 0)
	{
		// 递归处理每个元素
		for (std::size_t i = 0; i < N; ++i) {
			try {
				if constexpr (std::is_arithmetic_v<T>) {
					// 对于基本类型，直接转换
					if constexpr (std::is_same_v<T, float> || std::is_same_v<T, double>) {
						data[i] = static_cast<T>(std::stod(value[start_pos + i]));
					} else {
						data[i] = static_cast<T>(std::stoll(value[start_pos + i]));
					}
				} else if constexpr (is_std_array_v<T>) {
					// 对于嵌套数组，递归处理
					start_pos = setStdArrayFromString(data[i], value, start_pos + i);
				} else {
					static_assert(
						std::is_arithmetic_v<T> || is_std_array_v<T>,
						"T 必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 275");
				}
			} catch (const std::exception &e) {
				throw std::runtime_error("无法解析第 " + std::to_string(i)
										 + " 个元素: " + e.what());
			}
		}
		return start_pos + N;
	}

	template <typename T1, typename T2>
	void assign_value_get(const eprosima::fastcdr::optional<T1> &data, T2 &model_data)
	{
		if (data) {
			auto temp = data.value();
			if constexpr (std::is_arithmetic_v<T1>) {
				static_assert(std::is_arithmetic_v<T2>,
							  "模板参数T2必须是算术类型，详见XNDDSInterface.cpp：line 293");
				static_assert(std::is_convertible_v<T1, T2>,
							  "模板参数T1必须可以转换为T2类型，详见XNDDSInterface.cpp：line 295");
				model_data = temp;
			} else if constexpr (is_std_array_v<T1>) {
				size_t arraySize = array_size_v<T1>;
				for (size_t i = 0; i < arraySize; ++i) {
					auto temp2		 = temp[i];
					using array_type = typename T1::value_type;
					if constexpr (std::is_arithmetic_v<array_type>) {
						model_data[i] = temp2;
					} else if constexpr (is_std_array_v<array_type>) {
						size_t arraySize2	 = array_size_v<array_type>;
						using sub_array_type = typename array_type::value_type;
						if constexpr (std::is_arithmetic_v<sub_array_type>) {
							for (size_t j = 0; j < arraySize2; ++j) {
								model_data[i][j] = temp2[j];
							}
						} else {
							static_assert(std::is_arithmetic_v<sub_array_type>,
										  "模板参数T1是std::"
										  "array类型时，它的数组嵌套不能超过两层，详见XNDDSInterfac"
										  "e.cpp：line 313");
						}
					} else {
						static_assert(
							std::is_arithmetic_v<array_type> || is_std_array_v<array_type>,
							"模板参数T1是std::array类型时，它的value_"
							"type必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 320");
					}
				}
			} else {
				static_assert(
					std::is_arithmetic_v<T1> || is_std_array_v<T1>,
					"模板参数T1必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 326");
			}
		}
	}

	template <typename T1, typename T2>
	void assign_value_set(eprosima::fastcdr::optional<T1> &data, const T2 &model_data)
	{
		if constexpr (std::is_arithmetic_v<T1>) {
			static_assert(std::is_arithmetic_v<T2>,
						  "模板参数T2必须是算术类型，详见XNDDSInterface.cpp：line 337");
			static_assert(std::is_convertible_v<T2, T1>,
						  "模板参数T2必须可以转换为T1类型，详见XNDDSInterface.cpp：line 339");
			data = model_data;
		} else if constexpr (is_std_array_v<T1>) {
			T1 temp;
			size_t arraySize = array_size_v<T1>;
			using array_type = typename T1::value_type;
			for (size_t i = 0; i < arraySize; ++i) {
				if constexpr (std::is_arithmetic_v<array_type>) {
					temp[i] = model_data[i];
				} else if constexpr (is_std_array_v<array_type>) {
					size_t arraySize2	 = array_size_v<array_type>;
					using sub_array_type = typename array_type::value_type;
					if constexpr (std::is_arithmetic_v<sub_array_type>) {
						for (size_t j = 0; j < arraySize2; ++j) {
							temp[i][j] = model_data[i][j];
						}
					} else {
						static_assert(std::is_arithmetic_v<sub_array_type>,
									  "模板参数T1是std::"
									  "array类型时，它的数组嵌套不能超过两层，详见XNDDSInterface."
									  "cpp：line 357");
					}
				} else {
					static_assert(
						std::is_arithmetic_v<array_type> || is_std_array_v<array_type>,
						"模板参数T1是std::array类型时，它的value_"
						"type必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 364");
				}
			}
			data = temp;
		} else {
			static_assert(
				std::is_arithmetic_v<T1> || is_std_array_v<T1>,
				"模板参数T1必须是算术类型或std::array类型，详见XNDDSInterface.cpp：line 371");
		}
	}

	virtual void clearOutData() {}
	virtual void sendOutData() {}

protected:
	std::unordered_map<std::string, std::function<std::string()>> getDataFunction;
	std::unordered_map<std::string, std::function<void(std::string)>> setDataFunction;
	std::vector<std::function<XNByteArray(void)>> getByteArrayFunction;
	std::vector<std::function<void(XNByteArray, uint32_t &)>> setByteArrayFunction;
	std::mutex dataMutex;
	std::mutex outDataMutex;
	uint8_t header[8]{0}; // 固定大小的头部
	size_t headerSize = 8;
	FAST_DDS_MACRO::DataWriter *dataWriter;
};

#define MAP_DATA_FUNC(NAME)                                                                        \
	getDataFunction[#NAME] = [this]() { return getString(data.NAME()); };                          \
	setDataFunction[#NAME] = [this](const std::string &value) {                                    \
		setDataFromString(out_data.NAME(), value);                                                 \
	};                                                                                             \
	getByteArrayFunction.push_back([this]() { return getByteArray(data.NAME()); });                \
	setByteArrayFunction.push_back([this](const XNByteArray &byteArray, uint32_t &pos) {           \
		setByteArray(out_data.NAME(), byteArray, pos);                                             \
	});