AdaptiveBrightness/AdaptiveBrightness/sensor.cpp

#include "sensor.hpp"

#include <algorithm>
#include <regex>

#include <QSerialPort>
#include <QtDebug>
#include <QtGlobal>

#include "log_tr.hpp"
#include "utils.hpp"

Sensor::~Sensor()
{
    qDebug(ltr("Destroying sensor on serial port %1").arg(m_serialPortName));
}

Sensor::Sensor(Sensor&& other) noexcept : m_serialPortName(other.m_serialPortName), m_errorOccurred(other.m_errorOccurred), m_range(other.m_range)
{
    other.m_serialPortName.clear();
    other.m_errorOccurred = false;
    other.m_range.reset();
}

std::vector<float> Sensor::readValues()
{
    qDebug(ltr("Reading values of sensor on port %1").arg(m_serialPortName));

    const auto readCmd = QByteArray(READ_CMD.data(), static_cast<int>(READ_CMD.size()));
    QByteArray response;
    if(!getSensorCommandResponse(m_serialPortName, readCmd, response))
        return {};

    const auto values = parseReadResponse(response);

    if(!m_range) {
        qDebug(ltr("No sensor range known"));

        const auto sensorRange = readRange();
        if(m_errorOccurred) {
            return {};
        }

        m_range = sensorRange;
    }

    std::vector<float> normalizedValues;

    for(const auto& value: values) {
        normalizedValues.push_back(utils::map(value, m_range->first, m_range->second, 0.f, 1.f));
    }

    return normalizedValues;
}

Sensor::Sensor(const QString& serialPortName) : m_serialPortName(serialPortName)
{
    qDebug(ltr("Creating sensor on serial port %1").arg(m_serialPortName));
}

bool Sensor::isValidSensor()
{
    const auto versionCmd = QByteArray(VERSION_CMD.data(), static_cast<int>(VERSION_CMD.size()));

    QByteArray response;

    if(!getSensorCommandResponse(m_serialPortName, versionCmd, response)) {
        if(!response.isEmpty()) {
            qCritical(ltr("Only read \"%1\" from serial port %2").arg(QString(response)).arg(m_serialPortName));
        }
        return false;
    }

    const auto version = parseVersionResponse(response);
    return version == SUPPORTED_FIRMWARE;
}

bool Sensor::getSensorCommandResponse(const QString& serialPortName, QByteArray command, QByteArray& response)
{
    QSerialPort serialPort;
    serialPort.setPortName(serialPortName);
    serialPort.setBaudRate(115200); // Not required, STM32 VCP works with any baud rate

    m_errorOccurred = true; // Cleared on success

    if(!serialPort.open(QIODevice::ReadWrite)) {
        qCritical(ltr("Unable to open serial port %1, error code: %2").arg(serialPortName).arg(serialPort.errorString()));
        return false;
    }

    qDebug(ltr("Sending \"%1\" command to serial port %2").arg(QString(command)).arg(serialPortName));

    command.prepend("\r\n");
    command.append("\r\n");

    const auto bytesWritten = serialPort.write(command);

    if(bytesWritten == -1) {
        qCritical(ltr("Failed to write data to serial port %1, error code: %2").arg(serialPortName).arg(serialPort.errorString()));
        return false;
    }
    else if(bytesWritten != command.size()) {
        qCritical(ltr("Only %1/%2 bytes written to serial port %3, error: %4")
                      .arg(bytesWritten)
                      .arg(command.size())
                      .arg(serialPortName)
                      .arg(serialPort.errorString()));
        return false;
    }
    else if(!serialPort.waitForBytesWritten(100)) {
        qCritical(ltr("Writing operation timed out for serial port %1, error: %2").arg(serialPortName).arg(serialPort.errorString()));
        return false;
    }

    while(serialPort.waitForReadyRead(100)) {
        const auto newData = serialPort.readAll();
        qDebug(ltr("Read additional %1 bytes from serial port %2").arg(newData.size()).arg(serialPortName));
        response.append(newData);
    }

    if(serialPort.error() == QSerialPort::ReadError) {
        qCritical(ltr("Failed to read from serial port %1, error: %2").arg(serialPortName).arg(serialPort.errorString()));
        return false;
    }
    else if(serialPort.error() == QSerialPort::TimeoutError && response.isEmpty()) {
        qCritical(ltr("No data was read from serial port %1, error: %2").arg(serialPortName).arg(serialPort.errorString()));
        return false;
    }

    m_errorOccurred = false;
    return true;
}

std::string Sensor::parseVersionResponse(const QByteArray& response) const
{
    const auto versionRegex = std::regex("[a-zA-Z]+ v[0-9]\\.[0-9]");
    const auto strResponse = response.toStdString();
    std::smatch versionMatch;

    if(std::regex_search(strResponse, versionMatch, versionRegex)) {
        qDebug(ltr("Found version match: '%1'").arg(QString().fromStdString(versionMatch[0].str())));
        return versionMatch[0];
    }

    return {};
}

std::pair<int, int> Sensor::parseRangeResponse(const QByteArray& response) const
{
    const auto rangeRegex = std::regex("([0-9]+),([0-9]+)");
    const auto strResponse = response.toStdString();
    std::smatch rangeMatch;

    if(std::regex_search(strResponse, rangeMatch, rangeRegex)) {
        qDebug(ltr("Found range response: '%1'").arg(QString().fromStdString(rangeMatch[0].str())));
        return std::pair{std::stoi(rangeMatch[1]), std::stoi(rangeMatch[2])};
    }

    return {-1, -1};
}

std::vector<int> Sensor::parseReadResponse(const QByteArray& response) const
{
    const auto readRegex = std::regex("([0-9]+),([0-9]+),([0-9]+)");
    const auto strResponse = response.toStdString();
    std::smatch readMatch;

    if(std::regex_search(strResponse, readMatch, readRegex)) {
        qDebug(ltr("Found read response: '%1'").arg(QString().fromStdString(readMatch[0].str())));
        return {std::stoi(readMatch[1]), std::stoi(readMatch[2]), std::stoi(readMatch[3])};
    }

    return {};
}

std::pair<int, int> Sensor::readRange()
{
    qDebug(ltr("Reading range of sensor on port %1").arg(m_serialPortName));

    const auto rangeCmd = QByteArray(RANGE_CMD.data(), static_cast<int>(RANGE_CMD.size()));
    QByteArray response;
    if(!getSensorCommandResponse(m_serialPortName, rangeCmd, response))
        return {-1, -1};

    return parseRangeResponse(response);
}