Using Changes from EMAM2CPP

basic_interface.cpp

@@ -8,7 +8,7 @@ using namespace std;
     INTERFACE STRING
 */
 
-const char *BASIC_INTERFACE = R"(
+string BASIC_INTERFACE = string(R"(
 {
     "name":"basic_interface",
     "version":"1.0",
@@ -27,4 +27,4 @@ const char *BASIC_INTERFACE = R"(
         {"name":"set_braking","type":{"type":"basic","base_type":"Q"},"direction":"OUTPUT","allows_multiple_inputs":false,"optional":false}
     ]
 }
-)";
\ No newline at end of file
+)");
\ No newline at end of file

basic_interface.h

@@ -8,7 +8,7 @@
 #define BASIC_INTERFACE_OUTPUT_COUNT 3
 #define BASIC_INTERFACE_PORT_COUNT (BASIC_INTERFACE_INPUT_COUNT+BASIC_INTERFACE_OUTPUT_COUNT)
 
-extern const char *BASIC_INTERFACE;
+extern std::string BASIC_INTERFACE;
 
 #define PORT_TRUE_VELOCITY 0
 #define PORT_TRUE_POSITION 1

buffer.cpp

+#include "buffer.h"
+
+
+void DynamicBuffer::check_size(int size) {
+    if (size >= buffer_size) {
+        int32_t new_size = ((size / START_BUFFER_SIZE) + 1) * START_BUFFER_SIZE;
+        char *new_buff = new char[new_size];
+        for (int32_t i = 0; i < buffer_size; ++i){
+            new_buff[i] = buffer[i];
+        }
+        delete[] buffer;
+        buffer = new_buff;
+        buffer_size = new_size;
+    }
+}
+
+
+uint64_t BinaryReader::read_u64() {
+    check_range(8, "u64");
+    uint64_t t = 0;
+    for (int i = 0; i < 8; ++i) {
+        t <<= 8;
+        t |= ((uint8_t*)pos)[i];
+    }
+    pos += 8;
+    return t;
+}
+uint32_t BinaryReader::read_u32() {
+    check_range(4, "u32");
+    auto buff = (uint8_t*)pos;
+    pos += 4;
+    return (((uint32_t)buff[0]) << 24) | (((uint32_t)buff[1]) << 16) | (((uint32_t)buff[2]) << 8) | (uint32_t)buff[3];
+}
+uint16_t BinaryReader::read_u16() {
+    check_range(2, "u16");
+    auto buff = (uint8_t*)pos;
+    pos += 2;
+    return (((uint16_t)buff[0]) << 8) | (uint16_t)buff[1];
+}
+uint8_t BinaryReader::read_u8() {
+    check_range(1, "u8");
+    auto buff = (uint8_t*)pos;
+    pos += 1;
+    return *buff;
+}
+double BinaryReader::read_f64() {
+    check_range(8, "f64");
+    auto t = read_u64();
+    return *((double*)&t);
+}
+std::string BinaryReader::read_str() {
+    if (pos >= end) throw BufferException("str read past buffer content.");
+    auto start = pos;
+    while (pos < end && *pos != '\0') ++pos;
+    ++pos; // Go after '\0'
+    return std::string(start, pos-start-1);
+}
+
+void BinaryReader::check_range(int bytes, const char* type) {
+    if (pos + bytes > end) throw BufferException(std::string(type) + " read past buffer content.");
+}
+
+
+
+
+void BinaryWriter::write_u64(uint64_t value) {
+    auto pos = buffer.push_slot(8);
+    for (int i = 0; i < 8; ++i) {
+        ((uint8_t*)pos)[7-i] = value & 0xFF;
+        value >>= 8;
+    }
+}
+void BinaryWriter::write_u32(uint32_t value) {
+    auto pos = buffer.push_slot(4);
+    ((uint8_t*)pos)[0] = value >> 24;
+    ((uint8_t*)pos)[1] = value >> 16;
+    ((uint8_t*)pos)[2] = value >> 8;
+    ((uint8_t*)pos)[3] = value;
+}
+void BinaryWriter::write_u16(uint16_t value) {
+    auto pos = buffer.push_slot(2);
+    ((uint8_t*)pos)[0] = (uint8_t) (value >> 8);
+    ((uint8_t*)pos)[1] = (uint8_t) value;
+}
+void BinaryWriter::write_u8(uint8_t value) {
+    auto pos = buffer.push_slot(2);
+    *((uint8_t*)pos) = value;
+}
+void BinaryWriter::write_f64(double value) {
+    uint64_t t = *((uint64_t*)&value);
+    write_u64(t);
+}
+void BinaryWriter::write_str(const std::string &str) {
+    int size = str.size() + 1; // Include terminating char
+    auto pos = buffer.push_slot(size);
+    memcpy(pos, str.c_str(), size);
+}
\ No newline at end of file

buffer.h

+#pragma once
+#include <stdint.h>
+#include <string>
+#include <exception>
+
+struct DynamicBuffer {
+    static constexpr int32_t START_BUFFER_SIZE = 1024;
+    char *buffer = nullptr;
+    char* pos = nullptr;
+    int32_t buffer_size = 0;
+
+    DynamicBuffer() {
+        buffer = new char[START_BUFFER_SIZE];
+        buffer_size = START_BUFFER_SIZE;
+        reset();
+    }
+
+    void reset() {
+        pos = buffer;
+    }
+
+    
+    void append(uint8_t c) {
+        append((char) c);
+    }
+    void append(const char c) {
+        *pos = c;
+        ++pos;
+        check_size(position());
+    }
+    void append(const char* str) {
+        while (*str){
+            append(*str); ++str;
+        }
+    }
+
+    // Must be a valid index
+    void go_to(int index) {
+        this->pos = buffer + index;
+    }
+
+    const char *as_terminated_string() {
+        *pos = '\0';
+        return buffer;
+    }
+
+    // Returns the current position in the buffer and increases its content (pos) by "bytes"
+    char *push_slot(int bytes) {
+        auto t = pos;
+        pos += bytes;
+        check_size(position());
+        return t;
+    }
+
+    int position() {
+        return pos-buffer;
+    }
+
+    void reserve(int size) {
+        check_size(size);
+    }
+
+    ~DynamicBuffer(){
+        if (buffer != nullptr) delete[] buffer;
+    }
+
+private:
+    void check_size(int size);
+
+};
+
+struct BinaryReader {
+    const char* data;
+    const char* end;
+    const char* pos;
+
+    BinaryReader(const char* data, int size) : data(data), end(data+size), pos(data) {}
+
+    uint64_t read_u64();
+    uint32_t read_u32();
+    uint16_t read_u16();
+    uint8_t read_u8();
+    double read_f64();
+    std::string read_str();
+private:
+    void check_range(int bytes, const char* type);
+};
+
+
+struct BinaryWriter {
+    DynamicBuffer &buffer;
+
+    BinaryWriter(DynamicBuffer &buffer) : buffer(buffer) {}
+
+    void write_u64(uint64_t value);
+    void write_u32(uint32_t value);
+    void write_u16(uint16_t value);
+    void write_u8(uint8_t value);
+    void write_f64(double value);
+    void write_str(const std::string &str);
+};
+
+
+class BufferException : public std::exception {
+    std::string description;
+public:
+    BufferException(const std::string& description) : description("BufferException:\n\t" + description) {}
+    
+    virtual const char* what() const throw()
+    {
+        return description.c_str();
+    }
+};
+

dynamic_interface.h

+/** (c) https://github.com/MontiCore/monticore */
+#pragma once
+#if defined(_MSC_VER)
+    //  Microsoft
+    #define EXPORT __declspec(dllexport)
+    #define IMPORT __declspec(dllimport)
+#elif defined(__GNUC__)
+    //  GCC
+    #define EXPORT __attribute__((visibility("default")))
+    #define IMPORT
+#else
+    //  do nothing and hope for the best?
+    #define EXPORT
+    #define IMPORT
+    #pragma warning Unknown dynamic link import/export semantics.
+#endif
+
+extern "C" {
+
+// These are the functions that any autopilot must implement in order to be loaded and executed by the hardware_emulator
+
+// These are wrappers around the "autopilot interface functions" (program.h)
+// These are simpler entry points so that the hardware_emulator can more easily communicate with the autopilot
+// (Simple pure C types & simple function calling standards) 
+
+// Returns The JSON string of the DynamicInterface
+EXPORT const char* DI__get_interface();
+
+// Port data-exchange functions
+// If is_json != 0, the data is a JSON string
+// Else the data is binary: First 4 bytes of data -> size of the rest
+// Same for the returned data from DI__get_port()
+EXPORT void DI__set_port(int i, const char* data, int is_json);
+EXPORT const char* DI__get_port(int i, int is_json);
+
+// Methods
+EXPORT void DI__init();
+EXPORT void DI__execute(double delta_sec);
+
+}

json.cpp

@@ -14,63 +14,24 @@
 
 char JsonWriter::LOCAL_BUFFER[JsonWriter::LOCAL_BUFFER_SIZE];
 
-void JsonWriter::init(){
-    if (buffer == nullptr) {
-        alloc();
-    }
+JsonWriter::JsonWriter(DynamicBuffer &buffer) : buffer(buffer) {
     offset = 0;
-    pos = 0;
     has_elem = false;
     has_key = true;
+    buffer.reset();
 }
 
-void JsonWriter::alloc(){
-    buffer = new char[START_BUFFER_SIZE];
-    buffer_size = START_BUFFER_SIZE;
-    pos = 0;        
-}
-
-void JsonWriter::append(const char c) {
-    buffer[pos] = c;
-    ++pos;
-    if (pos >= buffer_size) {
-        int32_t new_size = buffer_size + START_BUFFER_SIZE;
-        char *new_buff = new char[new_size];
-        for (int32_t i = 0; i < buffer_size; ++i){
-            new_buff[i] = buffer[i];
-        }
-        delete[] buffer;
-        buffer = new_buff;
-        buffer_size = new_size;
-    }
-}
-
-void JsonWriter::append(const char* str){
-    int32_t i = 0;
-    while (str[i]){
-        append(str[i]); ++i;
-    }
-}
-
-
-const char* JsonWriter::get_string() {
-    if (buffer == nullptr) {
-        alloc();
-    }
-    buffer[pos] = '\0';
-    return buffer;
-}
 
 void JsonWriter::start_object() {
     separate();
     if (format) offset += TAB;
-    append('{');
+    buffer.append('{');
     has_elem = false;
 }
 
 void JsonWriter::add_offset(){
     for (int32_t i = 0; i < offset; ++i){
-        append(' ');
+        buffer.append(' ');
     }
 }
 
@@ -78,18 +39,18 @@ void JsonWriter::end_object() {
     if (format) {
         offset -= TAB;
         if (has_elem){
-            append('\n');
+            buffer.append('\n');
             add_offset();
         }
     }
-    append('}');
+    buffer.append('}');
     has_elem = true;
 }
 
 void JsonWriter::start_array() {
     separate();
     if (format) offset += TAB;
-    append('[');
+    buffer.append('[');
     has_elem = false;
 }
 
@@ -97,21 +58,21 @@ void JsonWriter::end_array() {
     if (format) {
         offset -= TAB;
         if (has_elem){
-            append('\n');
+            buffer.append('\n');
             add_offset();
         }
     }
-    append(']');
+    buffer.append(']');
     has_elem = true;
 }
 
 void JsonWriter::separate() {
-    if (has_elem && !has_key) append(',');
+    if (has_elem && !has_key) buffer.append(',');
     else has_elem = true;
     
     if (!has_key) {
         if (format) {
-            append('\n');
+            buffer.append('\n');
             add_offset();
         }
     }
@@ -121,72 +82,72 @@ void JsonWriter::separate() {
 void JsonWriter::write_key(const char* key) {
     write_value(key);
     has_key = true;
-    if (format) append(": ");
-    else append(':');
+    if (format) buffer.append(": ");
+    else buffer.append(':');
 }
 
 void JsonWriter::write_value(const char* str) {
     separate();
-    append('"');
+    buffer.append('"');
     int32_t i = 0;
     while (str[i]){
         char c = str[i];
         switch(c) {
-            case '\n': append("\\n"); break;
-            case '\t': append("\\t"); break;
-            case '\r': append("\\r"); break;
-            case '\f': append("\\f"); break;
-            case '\b': append("\\b"); break;
-            case '"': append("\\\""); break;
-            case '/': append("\\/"); break;
-            case '\\': append("\\\\"); break;
-            default: append(c); break;
+            case '\n': buffer.append("\\n"); break;
+            case '\t': buffer.append("\\t"); break;
+            case '\r': buffer.append("\\r"); break;
+            case '\f': buffer.append("\\f"); break;
+            case '\b': buffer.append("\\b"); break;
+            case '"': buffer.append("\\\""); break;
+            case '/': buffer.append("\\/"); break;
+            case '\\': buffer.append("\\\\"); break;
+            default: buffer.append(c); break;
         }
         ++i;
     }
-    append('"');
+    buffer.append('"');
 }
 
 void JsonWriter::write_value(int32_t i){
     separate();
     snprintf(LOCAL_BUFFER, LOCAL_BUFFER_SIZE, "%d", i);
-    append(LOCAL_BUFFER);
+    buffer.append(LOCAL_BUFFER);
 }
 void JsonWriter::write_value(int64_t l){
     separate();
     snprintf(LOCAL_BUFFER, LOCAL_BUFFER_SIZE, "%lld", l);
-    append(LOCAL_BUFFER);
+    buffer.append(LOCAL_BUFFER);
 }
 void JsonWriter::write_value(double d){
     separate();
-    if (isnan(d)) append("\"NaN\"");
+    if (isnan(d)) buffer.append("\"NaN\"");
     else if (isinf(d)) {
-        if (d < 0) append("\"-Infinity\"");
-        else append("\"Infinity\"");
+        if (d < 0) buffer.append("\"-Infinity\"");
+        else buffer.append("\"Infinity\"");
     }
     else {
         snprintf(LOCAL_BUFFER, LOCAL_BUFFER_SIZE, "%.*e", 17, d);
-        append(LOCAL_BUFFER);
+        buffer.append(LOCAL_BUFFER);
     }
 }
 void JsonWriter::write_value(float d){
     separate();
-    if (isnan(d)) append("\"NaN\"");
+    if (isnan(d)) buffer.append("\"NaN\"");
     else if (isinf(d)) {
-        if (d < 0) append("\"-Infinity\"");
-        else append("\"Infinity\"");
+        if (d < 0) buffer.append("\"-Infinity\"");
+        else buffer.append("\"Infinity\"");
     }
     else {
         snprintf(LOCAL_BUFFER, LOCAL_BUFFER_SIZE, "%.*e", 9, (double)d); //#define FLT_DECIMAL_DIG  9
-        append(LOCAL_BUFFER);
+        buffer.append(LOCAL_BUFFER);
     }
 }
 void JsonWriter::write_value(bool b){
     separate();
     if(b) {
-        append("true");
+        buffer.append("true");
     } else {
-        append("false");
+        buffer.append("false");
     }
 }
 
@@ -292,7 +253,7 @@ bool ArrayIterator::operator!=( const ArrayIterator& other ) {
 
 
 
-void JsonTraverser::init(const char *data) {
+JsonTraverser::JsonTraverser(const char *data) {
     pos = data;
     c = *pos;
     depth = 0;

json.h

@@ -6,27 +6,29 @@
 #include <string>
 #include <exception>
 #include <stdint.h>
+#include "buffer.h"
 
 
 struct JsonWriter {
-
-    static constexpr int32_t START_BUFFER_SIZE = 1024;
-    char *buffer = nullptr;
-    int32_t pos = 0;
-    int32_t buffer_size = 0;
+    // Used for "snprintf" target
+    static constexpr int32_t LOCAL_BUFFER_SIZE = 1024;
+    static char LOCAL_BUFFER[LOCAL_BUFFER_SIZE];
 
     static constexpr int32_t TAB = 2;
-    bool format = true;
+
+    DynamicBuffer &buffer;
+    
     int32_t offset = 0;
     bool has_elem = false;
     bool has_key = true;
+    bool format = true;
     
-    static constexpr int32_t LOCAL_BUFFER_SIZE = 1024;
-    static char LOCAL_BUFFER[LOCAL_BUFFER_SIZE];
 
-    void init();
+    JsonWriter(DynamicBuffer &buffer) : buffer(buffer) {}
 
-    const char* get_string();
+    const char* get_string() {
+        return buffer.as_terminated_string();
+    }
 
     void start_object();
     void end_object();
@@ -70,18 +72,10 @@ struct JsonWriter {
         write_value(b);
     }
 
-    ~JsonWriter(){
-        if (buffer != nullptr) delete[] buffer;
-    }
-
     
 private:
     void separate();
     void add_offset();
-    void alloc();
-
-    void append(const char c);
-    void append(const char* str);
 };
 
 
@@ -203,9 +197,7 @@ struct JsonTraverser {
     friend struct ObjectIterator;
     friend struct ArrayIterator;
     
-
-
-    void init(const char *data);
+    JsonTraverser(const char *data);
 
     ValueType get_type() {
         return current_type;
@@ -224,10 +216,10 @@ struct JsonTraverser {
 private:
 
     //const char *data;
-    char c;
     const char* pos;
     int32_t depth;
     ValueType current_type;
+    char c;
     bool last_bool;
 
 

printf.h

-/**
- * (c) https://github.com/MontiCore/monticore
- */
 ///////////////////////////////////////////////////////////////////////////////
 // \author (c) Marco Paland (info@paland.com)
 //             2014-2019, PALANDesign Hannover, Germany

tcp_protocol.h

@@ -2,6 +2,7 @@
 #include <string>
 #include <exception>
 
+// SEE https://git.rwth-aachen.de/monticore/EmbeddedMontiArc/simulators/simulation/-/wikis/dev-docs/concepts/TCP-protocol
 
 /*
     PACKET IDS
@@ -22,5 +23,5 @@ constexpr int PACKET_PING = 9; // No Payload
 constexpr int PACKET_EMU_ID = 10; // Payload: byte with ID for the new remote emulator. The emulator expects PACKET_CONFIG as response. This is sent before the PACKET_INTERFACE
 constexpr int PACKET_CONFIG = 11; // Payload: JSON string, response to PACKET_REQUEST_CONFIG
 constexpr int PACKET_RECONNECT = 12; // Payload: Emulator ID (byte), sent to remote hardware_emulator. Expects remote emulator to respond with PACKET_INTERFACE.
-constexpr int PACKET_INPUT_JSON = 14;
-constexpr int PACKET_OUTPUT_JSON = 15;
\ No newline at end of file
+constexpr int PACKET_INPUT_JSON = 13;
+constexpr int PACKET_OUTPUT_JSON = 14;
\ No newline at end of file