MGrblSim.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-05-02T16:10:25+02:00
**     Author: Sylvain Fargier <sfargier> <sylvain.fargier@cern.ch>
**
*/
 
#include "MGrblSim.hpp"
 
#include <chrono>
#include <cstring>
#include <sstream>
 
#include <ccut/utils.hpp>
#include <logger/Logger.hpp>
#include <poll.h>
#include <stdlib.h>
#include <unistd.h>
 
#include "Exception.hpp"
#include "GrblParser.hpp"
 
using namespace logger;
using namespace std::chrono;
using namespace grbl;
using namespace smc;
 
namespace mgrbl {
 
static const std::string s_loggerCat{"smc:mgrbldevice:sim"};
 
constexpr size_t MGrblSim::defaultInstancesCount;
constexpr size_t MGrblSim::axisCount;
 
MGrblSim::MGrblSim(size_t instances) : trigArmed{false}
{
    m_instances.reserve(instances);
    m_layout.assign(axisCount, 0);
    while (instances--)
        m_instances.emplace_back(new grbl::GrblSim());
 
    const std::string msg("CMGrbl 1.0\r\n");
    ::strcpy(m_buffer.end(), msg.data());
    m_buffer.fwd(msg.length());
}
 
MGrblSim::~MGrblSim()
{
    m_instances.clear();
}
 
void MGrblSim::read()
{
    for (std::unique_ptr<grbl::GrblSim> &instance : m_instances)
        instance->read();
}
 
void MGrblSim::write(const std::string &raw)
{
    if (raw.empty())
        return;
    const char c = raw[0];
 
    if (c >= '0' && c <= '9')
    {
        const size_t instance = c - '0';
        if (instance > m_instances.size())
        {
            error(s_loggerCat) << "unknown instance: " << c;
            ctrlWrite("error:3");
            return;
        }
        m_instances[instance]->write(raw.substr(1));
    }
    else if (c == 'C')
        processCtrl(raw);
    else
    {
        error(s_loggerCat) << "unknown command: " << raw;
        ctrlWrite("error:3");
    }
}
 
void MGrblSim::wake()
{
    if (m_instances.empty())
        return;
    m_instances.at(0)->wake();
}
 
void MGrblSim::ctrlWrite(const std::string &line)
{
    m_buffer.add('C');
    for (char c : line)
        m_buffer.add(c);
    m_buffer.add('\r');
    m_buffer.add('\n');
    wake();
}
 
bool MGrblSim::takeLine(std::string &line)
{
    if (m_buffer.takeLine(line))
        return true;
 
    for (size_t i = 0; i < m_instances.size(); ++i)
    {
        if (m_instances[i]->takeLine(line))
        {
            char c[2] = {'0', 0};
            c[0] += i;
 
            line = c + line;
            return true;
        }
    }
    return false;
}
 
bool MGrblSim::wait(const std::chrono::milliseconds &timeout)
{
    std::vector<struct pollfd> pfds;
    pfds.reserve(m_instances.size() + 1);
    bool ret = false;
 
    for (size_t i = 0; i < m_instances.size(); ++i)
    {
        pfds.emplace_back(pollfd{m_instances[i]->m_in, POLLIN | POLLERR, 0});
    }
    pfds.emplace_back(pollfd{m_instances[0]->m_wakeFd[0], POLLIN | POLLERR, 0});
 
    if (::poll(pfds.data(), pfds.size(), timeout.count() + 1) >= 0)
    {
        if (pfds.back().revents & POLLIN)
        {
            std::vector<char> buffer(10);
            ::read(m_instances[0]->m_wakeFd[0], buffer.data(), buffer.size());
        }
 
        for (size_t i = 0; i < pfds.size(); ++i)
        {
            if (pfds[i].revents & POLLERR)
                throw smc::make_errno_exception(smc::ErrorCode::IO);
            ret = ret || (pfds[i].revents & POLLIN);
        }
    }
    else
        throw smc::make_errno_exception(smc::ErrorCode::IO);
    return ret;
}
 
void MGrblSim::processCtrl(const std::string &line)
{
    const std::string str = ccut::trim(line).substr(1);
    try
    {
        if (ccut::startsWith(str, "$layout="))
        {
            Layout layout;
            if ((!MGrblParser::parseLayout(str, layout)) ||
                (layout.size() != m_layout.size()))
                throw Exception(smc::ErrorCode::InvalidArguments,
                                "invalid layout (wrong instances count)");
            for (size_t idx : layout)
            {
                if (idx >= m_instances.size())
                    throw Exception(
                        smc::ErrorCode::InvalidArguments,
                        "invalid layout (instance index out_of_range)");
            }
            m_layout.swap(layout);
            ctrlWrite("ok");
        }
        else if (str == "$layout")
        {
            std::string layout(m_layout.size(), '0');
            for (size_t i = 0; i < m_layout.size(); ++i)
                layout[i] = static_cast<char>(m_layout[i] + '0');
 
            ctrlWrite("$layout=" + layout);
            ctrlWrite("ok");
        }
        else if (str == "$grblCount")
        {
            ctrlWrite("$grblCount=" + std::to_string(m_instances.size()));
            ctrlWrite("ok");
        }
        else if (str == "$I")
        {
            ctrlWrite("[VER:1.0]");
            ctrlWrite("[INSTANCES:" + std::to_string(m_instances.size()) + "]");
            ctrlWrite("ok");
        }
        else if (ccut::startsWith(str, "$trigArm="))
        {
            if (str[9] == '1')
            {
                for (std::unique_ptr<grbl::GrblSim> const &instance :
                     m_instances)
                    instance->send("M64P0");
                trigArmed = 1;
            }
            else
            {
                for (std::unique_ptr<grbl::GrblSim> const &instance :
                     m_instances)
                    instance->send("M65P0");
                trigArmed = 0;
            }
            ctrlWrite("ok");
        }
        else if (str == "$trigArm")
        {
            std::string res{"$trigArm="};
            res += (trigArmed ? "1" : "0");
            ctrlWrite(res);
            ctrlWrite("ok");
        }
        else if (str == "$trigSoft")
        {
            // Same as disarming
            for (std::unique_ptr<grbl::GrblSim> const &instance : m_instances)
                instance->send("M65P0");
            trigArmed = 0;
            ctrlWrite("ok");
        }
        else
            throw Exception(smc::ErrorCode::InvalidArguments, "unknown command");
    }
    catch (Exception &ex)
    {
        error(s_loggerCat) << ex.what() << ": " << str;
        ctrlWrite("error:52");
    }
}
 
} // namespace mgrbl