XNSim/XNCore/XNDDSInterface.h

#pragma once

#include <string>
#include <vector>
#include <unordered_map>
#include <mutex>
#include <sstream>
#include <iomanip>
#include <array>
#include <functional>
#include <cstring>
#include <iostream>
#include <fastcdr/xcdr/optional.hpp>

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

template <typename T>
struct is_std_array : std::false_type {
};

template <typename T, std::size_t N>
struct is_std_array<std::array<T, N>> : std::true_type {
};

// 变量模板简化使用
template <typename T>
inline constexpr bool is_std_array_v = is_std_array<T>::value;

// 获取类型大小的辅助函数
template <typename T>
constexpr size_t getTypeSize()
{
	if constexpr (is_std_array_v<T>) {
		// 对于std::array，计算所有元素的总大小
		return getTypeSize<typename T::value_type>() * std::tuple_size<T>::value;
	} else {
		return sizeof(T);
	}
}

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

	template <typename T>
	void getByteArray(eprosima::fastcdr::optional<T> data, uint8_t *buffer, size_t bufferSize)
	{
		if (bufferSize < getTypeSize<T>())
			return;

		if constexpr (std::is_arithmetic_v<T>) {
			if (data) {
				std::memcpy(buffer, &data.value(), sizeof(T));
			} else {
				T zero = 0;
				std::memcpy(buffer, &zero, sizeof(T));
			}
		} else if constexpr (is_std_array_v<T>) {
			if (data) {
				getByteArrayFromStdArray(data.value(), buffer, bufferSize);
			} else {
				T zero = {};
				getByteArrayFromStdArray(zero, buffer, bufferSize);
			}
		}
	}

	template <typename T, std::size_t N>
	void getByteArrayFromStdArray(std::array<T, N> data, uint8_t *buffer, size_t bufferSize)
	{
		if (bufferSize < getTypeSize<T>() * N)
			return;

		for (std::size_t i = 0; i < N; ++i) {
			if constexpr (std::is_arithmetic_v<T>) {
				std::memcpy(buffer + i * getTypeSize<T>(), &data[i], getTypeSize<T>());
			} else {
				getByteArrayFromStdArray(data[i], buffer + i * getTypeSize<T>(), getTypeSize<T>());
			}
		}
	}

	void getUDPPackage(uint8_t *buffer, size_t bufferSize)
	{
		if (bufferSize < MAX_UDP_PACKET_SIZE)
			return;

		size_t currentPos = 0;

		// 复制头部
		if (headerSize >= 5) {
			std::memcpy(buffer, header, 5);
			currentPos = 5;
		}

		// 复制数据
		for (auto func : getByteArrayFunction) {
			if (currentPos + func.size <= MAX_UDP_PACKET_SIZE) {
				func.func(buffer + currentPos, func.size);
				currentPos += func.size;
			} else {
				break; // 超出最大包大小
			}
		}

		// 更新包大小
		if (currentPos >= 5) {
			buffer[4] = static_cast<uint8_t>(currentPos);
		}
	}

	template <typename T>
	std::string getString(eprosima::fastcdr::optional<T> data)
	{
		if constexpr (std::is_arithmetic_v<T>) {
			if (data) {
				return std::to_string(data.value());
			} else {
				return std::to_string(0);
			}
		} else if constexpr (std::is_same_v<T, std::string>) {
			if (data) {
				return getStringFromStdArray(data.value());
			} else {
				T zero = {};
				return getStringFromStdArray(zero);
			}
		}
		return std::string();
	}

	template <typename T, std::size_t N>
	std::string getStringFromStdArray(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 {
				ss << getStringFromStdArray(data[i]);
			}
		}
		return ss.str();
	}

	std::string getData(const std::string &varName)
	{
		int index1				= -1;
		int index2				= -1;
		std::string trueVarName = varName;

		size_t startPos = varName.find('[');
		if (startPos != std::string::npos) {
			size_t midPos = varName.find("][", startPos);
			size_t endPos = varName.find_last_of(']');

			if (midPos != std::string::npos) {
				try {
					index1 = std::stoi(varName.substr(startPos + 1, midPos - startPos - 1));
					index2 = std::stoi(varName.substr(midPos + 2, endPos - midPos - 2));
				} catch (...) {
					std::cerr << "无法解析数组索引: " << varName << std::endl;
					index1 = 0;
					index2 = 0;
				}
			} else if (endPos != std::string::npos) {
				try {
					index1 = std::stoi(varName.substr(startPos + 1, endPos - startPos - 1));
				} catch (...) {
					std::cerr << "无法解析数组索引: " << varName << std::endl;
					index1 = 0;
				}
			}
			trueVarName = varName.substr(0, startPos);
		}

		auto it = getDataFunction.find(trueVarName);
		if (it == getDataFunction.end()) {
			return std::string();
		}

		std::lock_guard<std::mutex> lock(mutex);
		std::string result = it->second();

		if (index1 < 0) {
			return result;
		}

		std::vector<std::string> list;
		std::stringstream ss(result);
		std::string item;
		while (std::getline(ss, item, ',')) {
			list.push_back(item);
		}

		if (index1 >= static_cast<int>(list.size())) {
			std::cerr << "数组索引超出范围: " << varName << std::endl;
			return std::string();
		}

		if (index2 < 0) {
			return list[index1];
		}

		std::vector<std::string> list2;
		std::stringstream ss2(list[index1]);
		while (std::getline(ss2, item, ' ')) {
			list2.push_back(item);
		}

		if (index2 >= static_cast<int>(list2.size())) {
			std::cerr << "数组索引超出范围: " << varName << std::endl;
			return std::string();
		}

		return list2[index2];
	}

protected:
	struct ByteArrayFunc {
		std::function<void(uint8_t *, size_t)> func;
		size_t size;
	};

	std::unordered_map<std::string, std::function<std::string()>> getDataFunction;
	std::vector<ByteArrayFunc> getByteArrayFunction;
	std::mutex mutex;
	uint8_t header[5]{}; // 固定大小的头部
	size_t headerSize = 0;
};

#define MAP_GET_DATA_FUNC(NAME)                                                                    \
	getDataFunction[#NAME] = [this]() { return getString(data.NAME()); };                          \
	getByteArrayFunction.push_back(                                                                \
		{[this](uint8_t *buffer, size_t size) { getByteArray(data.NAME(), buffer, size); },        \
		 getTypeSize<decltype(data.NAME())>()})