AdaptiveBrightness/AdaptiveBrightness/sensor_driver.cpp

#include "sensor_driver.hpp"

#include <regex>

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

#include "log_tr.hpp"

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

std::pair<int, int> SensorDriver::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);
}

std::vector<int> SensorDriver::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 {};

    return parseReadResponse(response);
}

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

bool SensorDriver::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 == "AdaptiveBrightness v1.2";
}

bool SensorDriver::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 SensorDriver::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> SensorDriver::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> SensorDriver::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 {};
}

//////////////////////////////////////////////////////////////////////////

SensorDriver::SensorDriver()
{
    qDebug("Initializing sensor driver");
}

SensorDriver::~SensorDriver()
{
    qDebug("Destroying sensor driver");
}

std::vector<SensorDriver::Sensor> SensorDriver::enumerateSensors() const
{
    qDebug(ltr("Enumerating sensors"));

    std::vector<Sensor> sensors;

    const auto portsInfo = QSerialPortInfo::availablePorts();

    qDebug(ltr("Found %1 serial ports").arg(portsInfo.count()));

    for(const auto& portInfo: portsInfo) {
        qDebug(ltr("Checking port %1").arg(portInfo.portName()));
        auto sensor = Sensor(portInfo.portName());
        if(sensor.isValidSensor()) {
            qInfo(ltr("Successfully enumerated sensor on port %1").arg(portInfo.portName()));
            sensors.push_back(sensor);
        }
    }

    return sensors;
}