MGrblPlatform.cpp

/*
** Copyright (C) 2021 CERN
**
** This software is provided 'as-is', without any express or implied
** warranty.  In no event will the authors be held liable for any damages
** arising from the use of this software.
**
** Permission is granted to anyone to use this software for any purpose,
** including commercial applications, and to alter it and redistribute it
** freely, subject to the following restrictions:
**
** 1. The origin of this software must not be misrepresented; you must not
**    claim that you wrote the original software. If you use this software
**    in a product, an acknowledgment in the product documentation would be
**    appreciated but is not required.
** 2. Altered source versions must be plainly marked as such, and must not be
**    misrepresented as being the original software.
** 3. This notice may not be removed or altered from any source distribution.
**
** Created on: 2022-04-29T11:16:27+02:00
**     Author: Sylvain Fargier <sfargier> <sylvain.fargier@cern.ch>
**
*/
 
#include "MGrblPlatform.hpp"
 
#include <ccut/Signal.hpp>
#include <ccut/async.hpp>
#include <ccut/utils.hpp>
#include <ccut/yml.hpp>
#include <logger/Logger.hpp>
 
#include "MotionLibConfig.hpp"
#include "platform/specialized/GrblPlatform/GrblPlatform.hpp"
#include "util/grbl/MGrblDeviceWrapper.hpp"
#include "util/grbl/MGrblSim.hpp"
 
#ifdef PLATFORM_SAMBUCA
#include "util/edge/MEdgeGrblDevice.hpp"
#endif
 
using namespace logger;
using namespace std::chrono;
namespace yml = ccut::yml;
 
namespace smc {
namespace internal {
 
static const std::string s_mgrblPrefix{"grbl://"};
static const std::string s_loggerCat{"smc:platform:mgrbl"};
 
MGrblPlatform::MGrblPlatform(const ccut::yml::NodeRef &ref, Context *data) :
    Thread("MGrblPlatform"),
    m_ctx(data ? data : new Context{s_mgrblPrefix, ref}),
    m_mainTriggerUrl(s_mgrblPrefix + m_ctx->id + "/trigger")
{
    std::string type;
    yml::NodeRef deviceNode = yml::get(ref, "device");
 
    if (deviceNode.is_map())
        yml::get(deviceNode, "type") >> yml::default_to(type, std::string());
    else
        deviceNode >> yml::default_to(type, std::string());
 
    if (type == "GrblSim")
    {
        m_ctx->device.reset(new mgrbl::MGrblSim());
    }
#ifdef PLATFORM_SAMBUCA
    else if (type == "EdgeSim")
    {
        int lun;
        std::string path;
        yml::get(deviceNode, "lun") >> yml::default_to(lun, int());
        yml::get(deviceNode, "path") >> yml::default_to(path, std::string());
        m_ctx->device.reset(new edge::MEdgeGrblDevice(lun, path));
    }
    else if (type == "Edge")
    {
        int lun = 0;
        yml::get(deviceNode, "lun") >> yml::default_to(lun, int());
        m_ctx->device.reset(new edge::MEdgeGrblDevice(lun));
    }
#endif /* PLATFORM_SAMBUCA */
    else
        throw Exception(ErrorCode::InvalidArguments,
                        "unknown MGrblPlatform device type");
 
    m_ctx->device->dataSignal.connect(
        m_dataConn, [this](const std::string &line) { processMessage(line); });
 
    createPlatforms(*m_ctx);
    getBuildInfo(*m_ctx);
    loadLayout(*m_ctx);
 
    /* not running their thread, but still running */
    for (const GrblPlatform::Shared &p : m_platforms)
        p->m_started.store(true);
    start();
}
 
MGrblPlatform::~MGrblPlatform()
{
    MGrblPlatform::stop();
    m_dataConn.disconnect();
    m_ctx->device.reset();
}
 
void MGrblPlatform::stop()
{
    Thread::stop();
}
 
GrblPlatform::Shared MGrblPlatform::getFirstInstance() const
{
    std::lock_guard<std::mutex> guard(m_lock);
    if (m_platforms.empty()) /* unlikely */
        throw Exception(ErrorCode::InternalError,
                        "no sub-platforms in MGrblPlatform");
 
    return m_platforms[0];
}
 
void MGrblPlatform::updateInstance(MGrblAxis &axis) const
{
    GrblPlatform::Shared platform{getFirstInstance()};
 
    size_t grblAxis = static_cast<size_t>(platform->getAxis(axis.uid()));
 
    std::lock_guard<std::mutex> guard(m_lock);
    if (m_layout.size() < grblAxis)
        throw Exception(ErrorCode::InternalError, "axis not in layout");
    const mgrbl::Instance instance = m_layout[grblAxis];
 
    if (instance >= m_platforms.size())
        throw Exception(ErrorCode::InternalError,
                        "axis not bound to an instance");
 
    axis.m_grbl = m_platforms[instance];
}
 
void MGrblPlatform::updateInstance(MGrblTrigger &trigger) const
{
    std::lock_guard<std::mutex> guard(m_lock);
    if (m_platforms.empty()) /* unlikely */
        throw Exception(ErrorCode::InternalError,
                        "no sub-platforms in MGrblPlatform");
 
    const decltype(m_triggers)::const_iterator instanceId = m_triggers.find(
        trigger.uid());
    if (instanceId == m_triggers.end())
        throw Exception(ErrorCode::InternalError, "wrong address of trigger");
 
    const mgrbl::Instance instance = instanceId->second;
    if (instance >= m_platforms.size())
        throw Exception(ErrorCode::InternalError,
                        "trigger not bound to an instance");
 
    trigger.m_grbl = m_platforms[instance];
}
 
void MGrblPlatform::updateInstance(MGrblGpio &gpio) const
{
    std::lock_guard<std::mutex> guard(m_lock);
 
    if (m_platforms.empty()) /* unlikely */
        throw Exception(ErrorCode::InternalError,
                        "no sub-platforms in MGrblPlatform");
 
    const decltype(m_gpios)::const_iterator instanceId = m_gpios.find(
        gpio.uid());
    if (instanceId == m_gpios.end())
        throw Exception(ErrorCode::InternalError, "wrong address of gpio");
 
    const mgrbl::Instance instance = instanceId->second;
    if (instance >= m_platforms.size())
        throw Exception(ErrorCode::InternalError,
                        "gpio not bound to an instance");
 
    gpio.m_grbl = m_platforms[instance];
}
 
void MGrblPlatform::runOnInstance(
    Context &,
    std::function<void(GrblPlatform::Shared &platform,
                       GrblPlatform::Context &ctx)> fun,
    bool onlyFirst) const
{
    if (onlyFirst)
    {
        GrblPlatform::Shared platform{getFirstInstance()};
        fun(platform, *platform->m_ctx);
    }
    else
    {
        std::vector<GrblPlatform::Shared> platforms{getPlatforms()};
        for (GrblPlatform::Shared &platform : platforms)
        {
            fun(platform, *platform->m_ctx);
        }
    }
}
 
void MGrblPlatform::loadLayout(Context &ctx)
{
    const std::string cmd = mgrbl::ControlSymbol + mgrbl::gen::Control::Layout;
    ctx.device->send(cmd, [this, &ctx, &cmd](const std::string &line) {
        mgrbl::Layout l;
        if (mgrbl::MGrblParser::parseLayout(line.substr(1), l))
        {
            ctx.used.clear();
 
            {
                std::lock_guard<std::mutex> guard(m_lock);
                m_layout = l;
            }
            for (mgrbl::Instance i : l)
                ctx.used.insert(i);
 
            info(s_loggerCat) << "loaded layout: " << l;
            return true;
        }
        return false;
    });
}
 
void MGrblPlatform::getBuildInfo(Context &ctx)
{
    const std::string cmd = mgrbl::ControlSymbol +
        mgrbl::gen::Control::BuildInfo;
    ctx.device->send(cmd, [this, &ctx, &cmd](const std::string &line) {
        info(s_loggerCat) << "sambuca-vitis build-info: " << line;
        return true;
    });
}
 
void MGrblPlatform::createPlatforms(Context &ctx)
{
    const std::string cmd = mgrbl::ControlSymbol +
        mgrbl::gen::Control::GrblCount;
 
    ctx.device->send(cmd, [this, &cmd](const std::string &line) {
        if (ccut::startsWith(line, cmd))
        {
            std::lock_guard<std::mutex> guard(m_lock);
            m_platforms.reserve(std::stoi(line.substr(cmd.size() + 1)));
            return true;
        }
        return false;
    });
 
    size_t capacity;
    {
        std::lock_guard<std::mutex> guard(m_lock);
        capacity = m_platforms.capacity();
    }
 
    if (capacity == 0)
        throw Exception(ErrorCode::InvalidArguments,
                        "failed to retrieve MGrblPlatform instances count");
 
    for (size_t i = 0; i < capacity; ++i)
    {
        GrblPlatform::Shared grbl{
            new GrblPlatform(ctx.prefix, ctx.id,
                             std::unique_ptr<grbl::GrblDeviceBase>(
                                 new grbl::MGrblDeviceWrapper{i, ctx.device}))};
 
        /* reset grbl before running the platform */
        grbl->m_ctx->sendReset();
 
        std::lock_guard<std::mutex> guard(m_lock);
        m_platforms.emplace_back(grbl);
    }
}
 
void MGrblPlatform::thread_func()
{
    debug(s_loggerCat) << "started";
    m_ctx->nextPoll = steady_clock::now();
    const milliseconds interval = GrblPlatform::defaultInterval;
 
    std::vector<GrblPlatform::Shared> platforms = getPlatforms();
 
    try
    {
        if (platforms.empty()) /* unlikely */
            throw Exception(ErrorCode::InternalError,
                            "no sub-platforms in MGrblPlatform");
 
        { /* load error and setting descriptions */
            GrblPlatform::Shared first = platforms.front();
            first->loadErrors();
            first->loadAlarms();
            first->loadSettingsDesc();
            for (const GrblPlatform::Shared &p : platforms)
            {
                if (p != first)
                {
                    p->m_ctx->errors = first->m_ctx->errors;
                    p->m_ctx->alarms = first->m_ctx->alarms;
                    p->m_ctx->setting = first->m_ctx->setting;
                    p->m_ctx->settingGroup = first->m_ctx->settingGroup;
                }
                p->setup();
            }
        }
 
        while (m_started.load())
        {
            steady_clock::time_point now = steady_clock::now();
            const milliseconds timeout = (now < m_ctx->nextPoll) ?
                duration_cast<milliseconds>(m_ctx->nextPoll - now) :
                milliseconds::zero();
 
            m_ctx->device->fetch(timeout);
 
            while (processImmediate(*m_ctx))
                ;
 
            for (const GrblPlatform::Shared &p : platforms)
            {
                p->handleAlarm();
                while (p->processImmediate())
                    ;
 
                p->processMotion();
            }
 
            now = steady_clock::now();
            if (m_ctx->nextPoll <= now)
            {
                processUpdate(*m_ctx);
                m_ctx->nextPoll = now + interval;
            }
        }
    }
    catch (Exception &ex)
    {
        crit(s_loggerCat) << ex.what();
        crit(s_loggerCat) << "stopping thread";
        m_started.store(false);
    }
 
    for (const GrblPlatform::Shared &p : platforms)
    {
        p->m_started.store(false);
        p->clearQueues("platform stopped");
    }
    clearQueue("platform stopped");
}
 
void MGrblPlatform::clearQueue(const std::string &msg)
{
    info(s_loggerCat) << "clearing run-queues";
    std::unique_lock<std::mutex> lock(m_lock);
    while (!m_immediate.empty())
    {
        ImmediateCmd cmd = std::move(m_immediate.front());
        m_immediate.pop();
        cmd.prom->set_exception(Exception(ErrorCode::Canceled, msg));
    }
}
 
void MGrblPlatform::processUpdate(Context &ctx)
{
    std::string msg("0?");
    for (mgrbl::Instance i : ctx.used)
    {
        msg[0] = static_cast<char>(i + '0');
        ctx.device->sendRealtime(msg);
    }
}
 
bool MGrblPlatform::processImmediate(Context &ctx)
{
    std::unique_lock<std::mutex> lock(m_lock);
    if (m_immediate.empty())
        return false;
    ImmediateCmd cmd = std::move(m_immediate.front());
    m_immediate.pop();
    lock.unlock();
 
    try
    {
        cmd.run(cmd, ctx);
    }
    catch (Exception &ex)
    {
        cmd.prom->set_exception(ex);
    }
    catch (std::exception &ex)
    {
        cmd.prom->set_exception(Exception(ErrorCode::Runtime, ex.what()));
    }
    return true;
}
 
void MGrblPlatform::wake()
{
    std::lock_guard<std::mutex> lock(m_mutex);
    m_waked = true;
    if (m_ctx->device)
        m_ctx->device->wake();
}
 
void MGrblPlatform::processMessage(const std::string &message)
{
    try
    {
        if (mgrbl::MGrblParser::isControl(message))
        {
            notice(s_loggerCat) << "<~~ " << message;
            return;
        }
        mgrbl::Instance instance = mgrbl::MGrblParser::getInstance(message);
        const std::string instanceMessage{message.substr(1)};
 
        if (GrblPlatform::Context::isStepperMessage(instanceMessage))
            m_ctx->nextPoll = steady_clock::now();
 
        GrblPlatform::Shared p = getInstance(instance);
        p->m_ctx->device->dataSignal(instanceMessage);
    }
    catch (Exception &ex)
    {
        error(s_loggerCat) << "failed to processMessage: " << ex.what();
    }
}
 
MGrblAxis::Shared MGrblPlatform::createAxis(
    const std::string &uid,
    const std::shared_ptr<GrblPlatform> &grbl)
{
    return std::make_shared<MGrblAxis>(uid, shared_from_this(), grbl);
}
 
} // namespace internal
} // namespace smc