fwt_software/src/serial/SerialMotorController.cpp

#include "fgc/SerialMotorController.h"

#include "fgc/Logger.h"
#include "fgc/TelemetryParser.h"

#include <atomic>
#include <istream>
#include <mutex>
#include <thread>

#include <boost/asio.hpp>

namespace fgc {

struct SerialMotorController::Impl {
    Impl(std::string dev, unsigned int b) : device(std::move(dev)), baud(b), serial(io) {}

    std::string                  device;
    unsigned int                 baud;
    boost::asio::io_context      io;
    boost::asio::serial_port     serial;
    boost::asio::streambuf       buffer;
    std::thread                  io_thread;
    std::mutex                   mutex;
    MotorTelemetry               latest;
    std::atomic<bool>            connected{false};

    // DUMP capture: the firmware emits a "DUMP BEGIN" ... "DUMP END" block in
    // response to a DUMP command. We accumulate it line-by-line and publish the
    // completed block (latest_dump) for the UI / `dump` command.
    bool                         dumping = false;
    std::string                  dump_buf;
    std::string                  latest_dump;

    void doRead() {
        boost::asio::async_read_until(
            serial, buffer, '\n',
            [this](const boost::system::error_code& ec, std::size_t) {
                if (ec) {
                    LOG_WARN << "Serial read failed: " << ec.message();
                    return;
                }
                std::istream is(&buffer);
                std::string line;
                std::getline(is, line);
                // Strip a trailing CR (firmware may send CRLF).
                if (!line.empty() && line.back() == '\r') line.pop_back();
                dispatchLine(line);
                doRead();
            });
    }

    // Route an inbound line: ST -> telemetry; OK/ERR -> command ack; everything
    // else (DG/DUMP/BOOT/IOIN/GSTAT...) is async output we just trace.
    void dispatchLine(const std::string& line) {
        if (line.rfind("ST ", 0) == 0 || line == "ST") {
            if (auto t = parseTelemetryLine(line)) {
                LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
                std::lock_guard<std::mutex> lock(mutex);
                latest = *t;
            } else {
                LOG_TRACE_CAT(LogCat::Serial) << "RX(unparsed ST) " << line;
            }
        } else if (line.rfind("ERR", 0) == 0) {
            LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
            LOG_WARN << "Motor controller: " << line;
        } else if (!line.empty()) {
            // OK acks and other async output (DG/DUMP/BOOT/...).
            LOG_TRACE_CAT(LogCat::Serial) << "RX " << line;
            captureDump(line);
        }
    }

    // Assemble the multi-line DUMP block. On completion, store it and emit it
    // once at INFO so it's visible regardless of the SERIAL trace toggle.
    void captureDump(const std::string& line) {
        if (line.rfind("DUMP BEGIN", 0) == 0) {
            dumping  = true;
            dump_buf = line + "\n";
            return;
        }
        if (!dumping) return;
        dump_buf += line + "\n";
        if (line.rfind("DUMP END", 0) == 0) {
            dumping = false;
            {
                std::lock_guard<std::mutex> lock(mutex);
                latest_dump = dump_buf;
            }
            LOG_INFO << "firmware dump:\n" << dump_buf;
            dump_buf.clear();
        }
    }
};

SerialMotorController::SerialMotorController(std::string device, unsigned int baud)
    : impl_(std::make_unique<Impl>(std::move(device), baud)) {}

SerialMotorController::~SerialMotorController() { stop(); }

void SerialMotorController::start() {
    namespace asio = boost::asio;
    boost::system::error_code ec;
    impl_->serial.open(impl_->device, ec);
    if (ec) {
        LOG_ERROR << "Failed to open serial port " << impl_->device << ": " << ec.message();
        impl_->connected = false;
        return;
    }
    impl_->serial.set_option(asio::serial_port_base::baud_rate(impl_->baud));
    impl_->serial.set_option(asio::serial_port_base::character_size(8));
    impl_->serial.set_option(
        asio::serial_port_base::parity(asio::serial_port_base::parity::none));
    impl_->serial.set_option(
        asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one));
    impl_->serial.set_option(
        asio::serial_port_base::flow_control(asio::serial_port_base::flow_control::none));

    impl_->connected = true;
    impl_->doRead();
    impl_->io_thread = std::thread([this] { impl_->io.run(); });
    LOG_INFO << "Serial controller started on " << impl_->device << " @ " << impl_->baud;
}

void SerialMotorController::stop() {
    if (!impl_) return;
    impl_->io.stop();
    if (impl_->io_thread.joinable()) impl_->io_thread.join();
    boost::system::error_code ec;
    if (impl_->serial.is_open()) impl_->serial.close(ec);
    impl_->connected = false;
}

void SerialMotorController::sendCommand(const std::string& cmd) {
    if (!impl_->connected) return;
    LOG_TRACE_CAT(LogCat::Serial) << "TX " << cmd;
    // The firmware requires a newline terminator on every command.
    auto data = std::make_shared<std::string>(cmd + "\n");
    boost::asio::async_write(impl_->serial, boost::asio::buffer(*data),
                             [data](const boost::system::error_code& ec, std::size_t) {
                                 if (ec) LOG_WARN << "Serial write failed: " << ec.message();
                             });
}

MotorTelemetry SerialMotorController::telemetry() {
    std::lock_guard<std::mutex> lock(impl_->mutex);
    return impl_->latest;
}

std::string SerialMotorController::lastDump() {
    std::lock_guard<std::mutex> lock(impl_->mutex);
    return impl_->latest_dump;
}

bool SerialMotorController::connected() const { return impl_->connected; }

}  // namespace fgc