ecos: implemented reply parsing and handing of stop/go reply

server/src/hardware/protocol/ecos/kernel.cpp

@@ -165,14 +165,65 @@ void Kernel::receive(std::string_view message)
     EventLoop::call([this, msg](){ Log::log(m_logId, LogMessage::D2002_RX_X, msg); });
   }
 
-  if(startsWith(message, "<REPLY "))
-  {
+  Reply reply;
+  Event event;
 
-  }
-  else if(startsWith(message, "<EVENT "))
+  if(parseReply(message, reply))
   {
-
+    receiveReply(reply);
   }
+  else if(parseEvent(message, event))
+  {
+    receiveEvent(event);
+  }
+  else
+  {}//  EventLoop::call([this]() { Log::log(m_logId, LogMessage::E2018_ParseError); });
+}
+
+void Kernel::receiveReply(const Reply& reply)
+{
+  if(reply.objectId == ObjectId::ecos)
+  {
+    if(reply.command == Command::set)
+    {
+      if(reply.options.size() == 1)
+      {
+        if(reply.options[0] == Option::stop)
+        {
+          if(m_emergencyStop != TriState::True)
+          {
+            m_emergencyStop = TriState::True;
+            if(m_onEmergencyStop)
+              EventLoop::call([this]() { m_onEmergencyStop(); });
+          }
+        }
+        else if(reply.options[0] == Option::go)
+        {
+          if(m_emergencyStop != TriState::False)
+          {
+            m_emergencyStop = TriState::False;
+            if(m_onGo)
+              EventLoop::call([this]() { m_onGo(); });
+          }
+        }
+      }
+    }
+    else if(reply.command == Command::get)
+    {
+      if(reply.options.size() == 1)
+      {
+        if(reply.options[0] == Option::info)
+        {
+          //! @todo
+        }
+      }
+    }
+  }
+}
+
+void Kernel::receiveEvent(const Event& event)
+{
+  (void)event;
 }
 
 void Kernel::emergencyStop()

server/src/hardware/protocol/ecos/kernel.hpp

@@ -37,6 +37,9 @@ class OutputController;
 
 namespace ECoS {
 
+struct Reply;
+struct Event;
+
 class Kernel
 {
   private:
@@ -63,6 +66,9 @@ class Kernel
 
     void setIOHandler(std::unique_ptr<IOHandler> handler);
 
+    void receiveReply(const Reply& reply);
+    void receiveEvent(const Event& event);
+
   public:// REMOVE!! just for testing
     void postSend(const std::string& message)
     {

server/src/hardware/protocol/ecos/messages.cpp

@@ -0,0 +1,105 @@
+#include "messages.hpp"
+#include <cassert>
+#include <charconv>
+#include "../../../utils/startswith.hpp"
+
+namespace ECoS {
+
+static const std::string_view endDelimiter = "<END ";
+
+bool parseReply(std::string_view message, Reply& reply)
+{
+  static const std::string_view startDelimiter = "<REPLY ";
+
+  if(!startsWith(message, startDelimiter))
+    return false;
+
+  size_t n = startDelimiter.size();
+  size_t pos;
+  if((pos = message.find('(', n)) == std::string_view::npos)
+    return false;
+
+  // read command:
+  reply.command = message.substr(n, pos - n);
+  n = pos + 1;
+
+  // read objectId
+  auto r = std::from_chars(&message[n], &message[message.size() - 1], reply.objectId);
+  if(r.ec != std::errc())
+    return false;
+
+  // read arguments
+  n = r.ptr - message.data();
+  while((pos = std::min(message.find(',', n), message.find(')', n))) != std::string_view::npos)
+  {
+    while(message[n] == ' ' && n < pos)
+      n++;
+    if(pos > n)
+      reply.options.emplace_back(&message[n], pos - n);
+    if(message[pos] == ')')
+      break;
+    n = pos + 1;
+  }
+  if(pos == std::string_view::npos || pos + 1 >= message.size() || message[pos + 1] != '>')
+    return false;
+
+  // advance to next line
+  n = pos + 2;
+  while((message[n] == '\n' || message[n] == '\r') && n < message.size())
+    n++;
+  if(n >= message.size())
+    return false;
+
+  // find end
+  size_t end;
+  if((end = message.find(endDelimiter, n)) == std::string_view::npos)
+    return false;
+
+  // read lines
+  if(end > n)
+  {
+    while((pos = message.find('\n', n)) < end)
+    {
+      reply.lines.emplace_back(&message[n], pos - n);
+      n = pos + 1;
+    }
+  }
+
+  // read status code
+  std::underlying_type_t<Status> status;
+  r = std::from_chars(&message[end + endDelimiter.size()], &message[message.size() - 1], status);
+  if(r.ec != std::errc())
+    return false;
+  reply.status = static_cast<Status>(status);
+  n = r.ptr - message.data();
+
+  // read status message
+  while(message[n] != '(' && message[n] != '\n' && message[n] != '\r' && n < message.size())
+    n++;
+  if(n >= message.size() || message[n] != '(')
+    return false;
+
+  pos = ++n;
+  while(message[pos] != ')' && message[pos] != '\n' && message[pos] != '\r' && n < message.size())
+    pos++;
+  if(pos >= message.size() || message[pos] != ')')
+    return false;
+
+  reply.statusMessage = message.substr(n, pos - n);
+
+  return true;
+}
+
+bool parseEvent(std::string_view message, Event& event)
+{
+  static const std::string_view startDelimiter = "<EVENT ";
+
+  if(!startsWith(message, startDelimiter))
+    return false;
+
+  (void)event;
+
+  return false;
+}
+
+}

server/src/hardware/protocol/ecos/messages.hpp

@@ -24,9 +24,21 @@
 #define TRAINTASTIC_SERVER_HARDWARE_PROTOCOL_ECOS_MESSAGES_HPP
 
 #include <string>
+#include <vector>
 
 namespace ECoS {
 
+struct Command
+{
+  static constexpr std::string_view queryObjects = "queryObjects";
+  static constexpr std::string_view set = "set";
+  static constexpr std::string_view get = "get";
+  static constexpr std::string_view create = "create";
+  static constexpr std::string_view delete_ = "delete";
+  static constexpr std::string_view request = "request";
+  static constexpr std::string_view release = "release";
+};
+
 struct ObjectId
 {
   static constexpr uint16_t ecos = 1;
@@ -53,6 +65,26 @@ struct Option
   static constexpr std::string_view view = "view";
 };
 
+enum class Status : uint32_t
+{
+  Ok = 0,
+};
+
+struct Reply
+{
+  std::string_view command;
+  uint32_t objectId;
+  std::vector<std::string_view> options;
+  std::vector<std::string_view> lines;
+  Status status;
+  std::string_view statusMessage;
+};
+
+struct Event
+{
+  uint32_t objectId;
+};
+
 inline std::string buildCommand(std::string_view command, uint16_t objectId, std::initializer_list<std::string_view> options)
 {
   std::string s(command);
@@ -65,39 +97,42 @@ inline std::string buildCommand(std::string_view command, uint16_t objectId, std
 
 inline std::string queryObjects(uint16_t objectId, std::initializer_list<std::string_view> options = {})
 {
-  return buildCommand("queryObjects", objectId, options);
+  return buildCommand(Command::queryObjects, objectId, options);
 }
 
 inline std::string set(uint16_t objectId, std::initializer_list<std::string_view> options)
 {
-  return buildCommand("set", objectId, options);
+  return buildCommand(Command::set, objectId, options);
 }
 
 inline std::string get(uint16_t objectId, std::initializer_list<std::string_view> options)
 {
-  return buildCommand("get", objectId, options);
+  return buildCommand(Command::get, objectId, options);
 }
 
 inline std::string create(uint16_t objectId, std::initializer_list<std::string_view> options)
 {
-  return buildCommand("create", objectId, options);
+  return buildCommand(Command::create, objectId, options);
 }
 
 inline std::string delete_(uint16_t objectId, std::initializer_list<std::string_view> options)
 {
-  return buildCommand("delete", objectId, options);
+  return buildCommand(Command::delete_, objectId, options);
 }
 
 inline std::string request(uint16_t objectId, std::initializer_list<std::string_view> options)
 {
-  return buildCommand("request", objectId, options);
+  return buildCommand(Command::request, objectId, options);
 }
 
 inline std::string release(uint16_t objectId, std::initializer_list<std::string_view> options)
 {
-  return buildCommand("release", objectId, options);
+  return buildCommand(Command::release, objectId, options);
 }
 
+bool parseReply(std::string_view message, Reply& reply);
+bool parseEvent(std::string_view message, Event& event);
+
 }
 
 #endif