Queue/weigh/MsgHandler.cxx

#include <iostream>
#include <iomanip>
#include <algorithm>
#include <unistd.h>
#include <sys/ioctl.h>
#include "MsgHandler.hpp"

constexpr uint8_t STX = 0x02;
constexpr uint8_t ETX = 0x03;
constexpr uint8_t CR = 0x0D;
constexpr uint8_t LF = 0x0A;

MsgHandler::MsgHandler() : fd(-1) {}

bool MsgHandler::OpenPort(const std::string& port, const std::string& baudrate)
{
    return true;
}

void MsgHandler::ClosePort()
{
    if (fd != -1)
    {
        close(fd);
        fd = -1;
    }
}

bool MsgHandler::SetDevice(const std::string& device)
{
    this->device = device;

    return true;
}

bool MsgHandler::SetEmptyWeight(const int& empty_weight)
{
    this->empty_weight = empty_weight;

    return true;
}

int MsgHandler::SendDeviceMsg(const std::vector<uint8_t>& data)
{
    std::cout << "write to " << this->device << std::endl;
    for (int num : data) {
        std::cout << std::hex << std::setw(2) << std::setfill('0')
                  << num << " ";
    }
    std::cout << std::endl;

    int bytesWritten =  SendMsg(data);

    return bytesWritten;
}

int MsgHandler::RecvDeviceMsg(std::vector<uint8_t>& data, int timeoutMs)
{
    int bytesRead = RecvMsg(data, timeoutMs);

    if (bytesRead > 0)
    {
        std::cout << "read from " << this->device << std::endl;
        for (int num : data)
        {
            std::cout << std::hex << std::setw(2) << std::setfill('0')
                      << num << " ";
        }
        std::cout << std::endl;
    }

    return bytesRead;
}

int MsgHandler::SendMsg(const std::vector<uint8_t>& data)
{
    if (fd == -1)
    {
        perror("设备未打开");
        return -1;
    }

    int bytesWritten = write(fd, data.data(), data.size());
    if (bytesWritten < 0)
    {
        perror("发送数据失败");
    }

    return bytesWritten;
}

int MsgHandler::RecvMsg(std::vector<uint8_t>& data, int timeoutMs)
{
    if (fd == -1)
    {
        perror("设备未打开\n");
        return -1;
    }

    // 使用select实现超时
    fd_set readfds;
    struct timeval tv;

    FD_ZERO(&readfds);
    FD_SET(fd, &readfds);

    tv.tv_sec = timeoutMs / 1000;
    tv.tv_usec = (timeoutMs % 1000) * 1000;

    int ret = select(fd + 1, &readfds, NULL, NULL, &tv);
    if (ret == -1)
    {
        perror("select错误");
        return -1;
    }
    else if (ret == 0)
    {
        //无数据可读
        return 0;
    }

    // 有数据可读
    uint8_t bytes;
    if (ioctl(fd, FIONREAD, &bytes) < 0)
    {
        perror("ioctl FIONREAD失败");
        return -1;
    }
    data.resize(bytes);
    int bytesRead = read(fd, data.data(), bytes);
    if (bytesRead < 0)
    {
        perror("读取数据失败");
        return -1;
    }

    return bytesRead;
}

void MsgHandler::HandleDdsMsg(const std::map<std::string, std::string>& msg)
{
    return;
}

bool MsgHandler::get_frame(std::vector<uint8_t>& frame, uint8_t frame_size, uint8_t start, uint8_t end)
{
    bool frame_found = false;

    do
    {
        auto start_it = std::find(m_WeightData.begin(), m_WeightData.end(), start);

        if (start_it == m_WeightData.end())
        {
            // 没有找到起始字节，清空缓冲区
            m_WeightData.clear();
            break;
        }

        // 移除起始字节前的无用数据
        if (start_it != m_WeightData.begin())
        {
            m_WeightData.erase(m_WeightData.begin(), start_it);
        }

        // 检查是否有足够的数据（至少一个完整帧）
        if (m_WeightData.size() >= 12)
        {
            // 检查结束字节
            if (m_WeightData[frame_size - 1] == end)
            {
                // 提取完整帧
                frame.clear();
                frame.insert(frame.begin(), m_WeightData.begin(), m_WeightData.begin() + frame_size);

                // 从缓冲区移除已处理的数据
                m_WeightData.erase(m_WeightData.begin(), m_WeightData.begin() + frame_size);
                frame_found = true;
                break;
            }
            else
            {
                // 结束字节不匹配，可能是数据错误或粘包
                // 查找下一个起始字节
                auto next_start = std::find(m_WeightData.begin() + 1, m_WeightData.end(), start);
                if (next_start != m_WeightData.end())
                {
                    // 移除到下一个起始字节之前的数据
                    m_WeightData.erase(m_WeightData.begin(), next_start);
                }
                else
                {
                    // 没有找到下一个起始字节，清空缓冲区
                    m_WeightData.clear();
                    break;
                }
            }
        }
        else
        {
            // 数据不完整，等待更多数据
            break;
        }
    } while (true);

    return frame_found;
}

bool MsgHandler::get_weight_1(std::string& weight, std::vector<uint8_t>& frame)
{
    // 1. 解析符号
    char sign = '+';
    if (frame[1] == 0x2D)
    {
        sign = '-';
    }
    else if (frame[1] != 0x2B)
    {
        return false;
    }

    // 2. 解析称量数据（6位数字）
    std::string weightDigits;
    for (int i = 2; i <= 7; i++)
    {
        if (frame[i] < 0x30 || frame[i] > 0x39)
        {
            return false;
        }
        weightDigits += static_cast<char>(frame[i]);
    }

    // 3. 解析小数点位置
    if (frame[8] < 0x30 || frame[8] > 0x34)
    {
        return false;
    }
    int decimalPos = frame[8] - 0x30;

    // 4. 构建结果字符串
    std::string weightStr;

    if (decimalPos == 0)
    {
        // 情况1：没有小数点，去除所有前导零，但保留至少一位数字
        std::string integerPart = weightDigits;

        // 去除前导零
        size_t firstNonZero = 0;
        while (firstNonZero < integerPart.length() && integerPart[firstNonZero] == '0')
        {
            firstNonZero++;
        }

        if (firstNonZero == integerPart.length())
        {
            // 如果全部是零，保留一个零
            weightStr = "0";
        }
        else
        {
            weightStr = integerPart.substr(firstNonZero);
        }
    }
    else
    {
        // 情况2：有小数点
        if (decimalPos > weightDigits.length())
        {
            return false;
        }

        // 从右向左插入小数点
        int insertPos = weightDigits.length() - decimalPos;

        // 分离整数部分和小数部分
        std::string integerPart = weightDigits.substr(0, insertPos);
        std::string decimalPart = weightDigits.substr(insertPos, decimalPos);

        // 处理整数部分：去除前导零，但如果全为零则保留一个0
        bool allZero = true;
        for (char c : integerPart)
        {
            if (c != '0')
            {
                allZero = false;
                break;
            }
        }

        if (allZero && !integerPart.empty())
        {
            integerPart = "0";
        }
        else
        {
            // 去除整数部分的前导零
            size_t firstNonZero = 0;
            while (firstNonZero < integerPart.length() && integerPart[firstNonZero] == '0')
            {
                firstNonZero++;
            }

            if (firstNonZero > 0 && firstNonZero < integerPart.length())
            {
                integerPart = integerPart.substr(firstNonZero);
            }
        }

        // 构建最终字符串，小数部分保留完整位数
        weightStr = integerPart + "." + decimalPart;
    }

    // 5. 添加符号
    weight.clear();
    weight += sign;
    weight += weightStr;

    return true;
}

bool MsgHandler::get_weight_1(float& weight, std::vector<uint8_t>& frame)
{
    float raw_value;
    float divisors[] = {1.0f, 10.0f, 100.0f, 1000.0f, 10000.0f};

    std::string weightDigits;
    for (int i = 2; i <= 7; i++)
    {
        if (frame[i] < 0x30 || frame[i] > 0x39)
        {
            return false;
        }
        weightDigits += static_cast<char>(frame[i]);
    }

    int decimalPos = frame[8] - 0x30;
    if (decimalPos < 0)
    {
        decimalPos = 0;
    }

    raw_value = std::stof(weightDigits);
    weight = raw_value / divisors[decimalPos];

    if (frame[1] == 0x2D)
    {
        weight = 0 - weight;
    }

    return true;
}

bool MsgHandler::get_weight_2(std::string& weight, std::vector<uint8_t>& frame)
{
    // 1. 解析符号
    char sign = '+';
    if (frame[2] & 0x02)
    {
        sign = '-';
    }

    // 2. 解析称量数据（6位数字）
    std::string weightDigits;
    for (int i = 4; i <= 9; i++)
    {
        if ((frame[i] >= 0x30) && (frame[i] <= 0x39))
        {
            weightDigits += static_cast<char>(frame[i]);
        }
        else if (frame[i] == 0x20)
        {
            weightDigits += '0';
        }
    }

    // 3. 解析小数点位置
    int decimalPos = frame[1]&0x07 - 2;
    if (decimalPos < 0)
    {
        decimalPos = 0;
    }

    // 4. 构建结果字符串
    std::string weightStr;

    if (decimalPos == 0)
    {
        // 情况1：没有小数点，去除所有前导零，但保留至少一位数字
        std::string integerPart = weightDigits;

        // 去除前导零
        size_t firstNonZero = 0;
        while (firstNonZero < integerPart.length() && integerPart[firstNonZero] == '0')
        {
            firstNonZero++;
        }

        if (firstNonZero == integerPart.length())
        {
            // 如果全部是零，保留一个零
            weightStr = "0";
        }
        else
        {
            weightStr = integerPart.substr(firstNonZero);
        }
    }
    else
    {
        // 情况2：有小数点
        if (decimalPos > weightDigits.length())
        {
            return false;
        }

        // 从右向左插入小数点
        int insertPos = weightDigits.length() - decimalPos;

        // 分离整数部分和小数部分
        std::string integerPart = weightDigits.substr(0, insertPos);
        std::string decimalPart = weightDigits.substr(insertPos, decimalPos);

        // 处理整数部分：去除前导零，但如果全为零则保留一个0
        bool allZero = true;
        for (char c : integerPart)
        {
            if (c != '0')
            {
                allZero = false;
                break;
            }
        }

        if (allZero && !integerPart.empty())
        {
            integerPart = "0";
        }
        else
        {
            // 去除整数部分的前导零
            size_t firstNonZero = 0;
            while (firstNonZero < integerPart.length() && integerPart[firstNonZero] == '0')
            {
                firstNonZero++;
            }

            if (firstNonZero > 0 && firstNonZero < integerPart.length())
            {
                integerPart = integerPart.substr(firstNonZero);
            }
        }

        // 构建最终字符串，小数部分保留完整位数
        weightStr = integerPart + "." + decimalPart;
    }

    // 5. 添加符号
    weight.clear();
    weight += sign;
    weight += weightStr;

    std::cout << weight << std::endl;

    return true;
}

bool MsgHandler::get_weight_2(float& weight, std::vector<uint8_t>& frame)
{
    float raw_value;
    float divisors[] = {1.0f, 10.0f, 100.0f, 1000.0f, 10000.0f, 100000.0f};

    std::string weightDigits;
    for (int i = 4; i <= 9; i++)
    {
        if ((frame[i] >= 0x30) && (frame[i] <= 0x39))
        {
            weightDigits += static_cast<char>(frame[i]);
        }
        else if (frame[i] == 0x20)
        {
            weightDigits += '0';
        }
    }

    int decimalPos = frame[1]&0x07 - 2;
    if (decimalPos < 0)
    {
        decimalPos = 0;
    }

    raw_value = std::stof(weightDigits);
    weight = raw_value / divisors[decimalPos];

    if (frame[2] & 0x02)
    {
        weight = 0 - weight;
    }

    return true;
}

template<typename T>
bool MsgHandler::HandleDeviceMsg(T& msg)
{
    std::vector<uint8_t> m_Data;
    std::vector<uint8_t> frame;

    if (RecvDeviceMsg(m_Data, 100) > 0)
    {
        m_WeightData.insert(m_WeightData.end(), m_Data.begin(), m_Data.end());
    }

    if (m_WeightData.empty() != true)
    {
        if (device == "Keli")
        {
            if (get_frame(frame, 12, STX, ETX) == true)
            {
                return get_weight_1(msg, frame);
            }
        }
        else if (device == "Toledo")
        {
            if (get_frame(frame, 17, STX, CR) == true)
            {
                return get_weight_2(msg, frame);
            }
        }
    }

    return false;
}
template bool MsgHandler::HandleDeviceMsg<std::string>(std::string&);
template bool MsgHandler::HandleDeviceMsg<float>(float&);