#ifndef MODBUS_UTILS_H
#define MODBUS_UTILS_H

#include <stdint.h>
#include <cstddef>
#include <vector>

class ModbusUtils {
public:
    // CRC16计算函数
    static uint16_t modbus_crc16(const std::vector<uint8_t>& data) {
        uint16_t crc = 0xFFFF;
        for (size_t i = 0; i < data.size(); i++) {
            crc ^= data[i];
            for (int j = 0; j < 8; j++) {
                if (crc & 0x0001) {
                    crc = (crc >> 1) ^ 0xA001;
                } else {
                    crc = crc >> 1;
                }
            }
        }
        return crc;
    }

    static uint16_t modbus_crc16(const uint8_t* data, size_t length) {
        return modbus_crc16(std::vector<uint8_t>(data, data + length));
    }

    // MODBUS 06 - 生成写单个寄存器指令
    static bool build_modbus06_command(uint8_t slave_addr, uint16_t reg_addr, uint16_t value, std::vector<uint8_t>& cmd) {

        cmd.clear();
        cmd.push_back(slave_addr);
        cmd.push_back(0x06);
        cmd.push_back(static_cast<uint8_t>(reg_addr >> 8));
        cmd.push_back(static_cast<uint8_t>(reg_addr & 0xFF));
        cmd.push_back(static_cast<uint8_t>(value >> 8));
        cmd.push_back(static_cast<uint8_t>(value & 0xFF));

        uint16_t crc = modbus_crc16(cmd);
        cmd.push_back(static_cast<uint8_t>(crc & 0xFF));        // CRC低字节
        cmd.push_back(static_cast<uint8_t>((crc >> 8) & 0xFF)); // CRC高字节

        return true;
    }

    // MODBUS 10 - 生成写多个寄存器指令
    static bool build_modbus10_command(uint8_t slave_addr, uint16_t start_addr, const std::vector<uint16_t>& values, std::vector<uint8_t>& cmd) {

    	cmd.clear();
		cmd.push_back(slave_addr);
		cmd.push_back(0x10);
		cmd.push_back(start_addr >> 8);
		cmd.push_back(start_addr & 0xFF);
		cmd.push_back(values.size() >> 8);
		cmd.push_back(values.size() & 0xFF);
		cmd.push_back(values.size() * 2);

		// 添加数据
		for (uint16_t val : values) {
			cmd.push_back(val >> 8);
			cmd.push_back(val & 0xFF);
		}

        uint16_t crc = modbus_crc16(cmd);
        cmd.push_back(static_cast<uint8_t>(crc & 0xFF));        // CRC低字节
        cmd.push_back(static_cast<uint8_t>((crc >> 8) & 0xFF)); // CRC高字节

        return true;
    }
    // MODBUS 03 - 生成读多个寄存器指令
	static bool build_modbus03_command(uint8_t slave_addr, uint16_t reg_addr, uint16_t length, std::vector<uint8_t>& cmd) {

		cmd.clear();
		cmd.push_back(slave_addr);
		cmd.push_back(0x03);
		cmd.push_back(static_cast<uint8_t>(reg_addr >> 8));
		cmd.push_back(static_cast<uint8_t>(reg_addr & 0xFF));
		cmd.push_back(static_cast<uint8_t>(length >> 8));
		cmd.push_back(static_cast<uint8_t>(length & 0xFF));

		uint16_t crc = modbus_crc16(cmd);
		cmd.push_back(static_cast<uint8_t>(crc & 0xFF));        // CRC低字节
		cmd.push_back(static_cast<uint8_t>((crc >> 8) & 0xFF)); // CRC高字节

		return true;
    }
};

#endif // MODBUS_UTILS_H