simplesync/include/simplesync.hpp

#ifndef SIMPLESYNC_HPP
#define SIMPLESYNC_HPP

#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <functional>
#include <iostream>
#include <vector>

#ifdef ARDUINO
#include <Arduino.h>
#ifdef __AVR__
#include "include/nonstd_functional.hpp"
using nonstd::function;
#else /* ndef __AVR__ */
using std::function;
#endif
#else /* ndef ARDUINO */
using std::function;
#endif

namespace simplesync {

enum error_t {
    BUFFER_OVERFLOW = (-1),
    INVALID_COMMAND = (-2),
    ID_INVALID      = (-3),
    OUT_OF_MEMORY   = (-4),
    CRC_MISSMATCH   = (-5),
};

inline int cobs_encode(uint8_t *buf_in, unsigned int buf_in_size, uint8_t *buf_out,
                       unsigned int buf_out_size) {
    unsigned int i_out = 1;
    unsigned int last0 = 0;
    for (unsigned int i_in = 0; i_in < buf_in_size; i_in += 1, i_out += 1) {
        if (i_out >= buf_out_size) return OUT_OF_MEMORY;
        if ((buf_in[i_in] != 0x00) && ((i_out - last0) < 0xFF)) {
            buf_out[i_out] = buf_in[i_in];
        } else {
            buf_out[last0] = i_out - last0;
            last0          = i_out;
        }
    }
    buf_out[last0] = i_out - last0;
    buf_out[i_out] = 0x00;
    return i_out + 1;
}

inline int cobs_decode(uint8_t *buf_in, unsigned int &buf_in_size_used, uint8_t *buf_out,
                       unsigned int &buf_out_size_used) {
    unsigned int i_out = 0;
    unsigned int next0 = 0;
    bool skip_next0    = true;
    unsigned int i_in;
    for (i_in = 0; i_in < buf_in_size_used || buf_in[i_in] == 0x00; i_in += 1) {
        if (i_out >= buf_out_size_used) {
            buf_out_size_used = i_out;
            buf_in_size_used  = i_in;
            return OUT_OF_MEMORY;
        }
        if (next0 == i_in) {
            if (buf_in[i_in] == 0x00) break;
            if (!skip_next0) buf_out[i_out++] = 0x00;
            next0      = i_in + buf_in[i_in];
            skip_next0 = (next0 - i_in) == 0xFF;
        } else {
            buf_out[i_out++] = buf_in[i_in];
        }
    }
    buf_out_size_used = i_out;
    buf_in_size_used  = i_in;
    return 0;
}

inline unsigned int encode_zigzag(int input) {
    return (input << 1) ^ (input >> (sizeof(int) * 8 - 1));
}

inline unsigned int decode_zigzag(int input) { return (input >> 1) ^ (-(input & 1)); }

inline unsigned int encode_unsigned_varint(unsigned int input, uint8_t *out) {
    int offset = 0;
    while (input > 0x7F) {
        out[offset++] = input & 0x7F;
        input >>= 7;
    }
    out[offset++] = input | 0x80;
    return offset;
}

inline unsigned int encode_varint(int input, uint8_t *out) {
    return encode_unsigned_varint(encode_zigzag(input), out);
}

inline unsigned int decode_unsigned_varint(uint8_t *input, unsigned int &out) {
    int offset = 0;
    out        = 0;
    for (;; offset += 1) {
        out |= ((unsigned int)(input[offset] & 0x7F)) << (7 * offset);
        if (input[offset] & 0x80) break;
    }
    return offset + 1;
}

inline unsigned int decode_varint(uint8_t *input, int &out) {
    unsigned int out_uint;
    int offset = decode_unsigned_varint(input, out_uint);
    out        = decode_zigzag(out_uint);
    return offset;
}

inline uint16_t calc_fletcher_checksum(uint8_t *buf, unsigned int buf_size) {
    uint32_t c0, c1;
    for (c0 = c1 = 0; buf_size > 0;) {
        size_t blocklen = buf_size;
        if (blocklen > 5802) blocklen = 5802;
        buf_size -= blocklen;
        do {
            c0 = c0 + *buf++;
            c1 = c1 + c0;
        } while (--blocklen);
        c0 = c0 % 255;
        c1 = c1 % 255;
    }
    return (c1 << 8 | c0);
}

template <typename time_t, typename timedelta_t, unsigned int BUF_SIZE = 256,
          unsigned int str_len_max = 32>
class SimpleSync {
  public:
    class NumberInterface {
      public:
        time_t last_send;
        char *key;
        timedelta_t update_interval;
        bool send_requested = true;
        int value;
        NumberInterface(SimpleSync &s, char *key, timedelta_t update_interval)
            : key(key), update_interval(update_interval) {
            s.number_interfaces.push_back(this);
        }
    };

    class StringInterface {
      public:
        time_t last_send;
        char *key;
        timedelta_t update_interval;
        bool send_requested = true;
        char value[str_len_max + 1];
        StringInterface(SimpleSync &s, char *key, timedelta_t update_interval)
            : key(key), update_interval(update_interval) {
            value[str_len_max] = 0x00;
            s.string_interfaces.push_back(this);
        }
    };

    std::vector<NumberInterface *> number_interfaces;
    std::vector<StringInterface *> string_interfaces;

    function<int(uint8_t *, unsigned int)> write_pkg;
    function<void(NumberInterface *&, char *, int)> number_update;
    function<void(StringInterface *&, char *, char *)> string_update;
    SimpleSync(function<int(uint8_t *, unsigned int)> write_pkg,
               function<void(NumberInterface *&, char *, int)> number_update,
               function<void(StringInterface *&, char *, char *)> string_update)
        : write_pkg(write_pkg), number_update(number_update), string_update(string_update) {}

    int parse_pkg(uint8_t *buf, unsigned int buf_size) {
        uint16_t crc = calc_fletcher_checksum(buf, buf_size - 2);
        if(buf_size < 4) return CRC_MISSMATCH;
        if (buf[--buf_size] != (crc & 0xFF) || (buf[--buf_size] != (crc >> 8))) return CRC_MISSMATCH;
        for (unsigned int parsed = 0; parsed < buf_size;) {
            switch (buf[parsed]) {
            case 0x00: {
                for (auto ni : number_interfaces) ni->send_requested = true;
                for (auto ni : string_interfaces) ni->send_requested = true;
            } break;
            case 0x01: {
                char *key = (char *)&buf[parsed + 1];
                parsed += 1 + strlen(key) + 1;
                int value;
                parsed += decode_varint(&buf[parsed], value);
                NumberInterface *ni_ptr = nullptr;
                for (auto ni : number_interfaces) {
                    if (strcmp(ni->key, key) == 0) {
                        ni_ptr = ni;
                        break;
                    }
                }
                if (number_update) {
                    number_update(ni_ptr, key, value);
                }
                if (ni_ptr) {
                    ni_ptr->value = value;
                }
            } break;
            case 0x02: {
                char *key = (char *)&buf[parsed + 1];
                parsed += 1 + strlen(key) + 1;
                char *value       = (char *)&buf[parsed];
                unsigned int strl = strlen(value) + 1;
                parsed += strl;
                StringInterface *stri_ptr = nullptr;
                for (StringInterface *si : string_interfaces) {
                    if (strcmp(si->key, key) == 0) {
                        stri_ptr = si;
                        break;
                    }
                }
                if (string_update) {
                    string_update(stri_ptr, key, value);
                }
                if (stri_ptr) {
                    memcpy(stri_ptr->value, value, str_len_max > strl ? strl : str_len_max);
                }
            } break;
            default:
                // Error: Unknown datatype, ignore rest of the packet
                return -1;
            }
        }
        return 0;
    }

    void request_all_interfaces() const {
        uint8_t buf[]             = {0x00, 0x00};
        unsigned int buf_len_used = sizeof(buf);
        uint8_t cobs_buf[10];
        unsigned int cobs_buf_needed = cobs_encode(buf, buf_len_used, cobs_buf, BUF_SIZE);
        write_pkg(cobs_buf, cobs_buf_needed);
    }

    void update(time_t time) {
        uint8_t buf[BUF_SIZE];
        unsigned int buf_len_used = 0;
        for (auto ni : number_interfaces) {
            if (ni->send_requested || ni->last_send > time ||
                ni->last_send + ni->update_interval <= time) {
                unsigned int strl           = strlen(ni->key) + 1;
                unsigned int buf_len_needed = 1 + strl + 8 * sizeof(int) / 7;
                buf_len_needed += buf_len_needed / 0xFF + 3 + 2; // COBS overhead
                if (BUF_SIZE < buf_len_needed) continue; // Error: update to large, skip interface
                if (BUF_SIZE < buf_len_used + buf_len_needed) {
                    uint8_t cobs_buf[BUF_SIZE];
                    uint16_t crc        = calc_fletcher_checksum(buf, buf_len_used);
                    buf[buf_len_used++] = crc >> 8;
                    buf[buf_len_used++] = crc & 0xFF;
                    unsigned int cobs_buf_needed =
                        cobs_encode(buf, buf_len_used, cobs_buf, BUF_SIZE);
                    write_pkg(cobs_buf, cobs_buf_needed);
                    buf_len_used = 0;
                }
                buf[buf_len_used++] = 0x01;
                memcpy((char *)&buf[buf_len_used], ni->key, strl);
                buf_len_used += strl;
                buf_len_used += encode_varint(ni->value, &buf[buf_len_used]);
                ni->last_send      = time;
                ni->send_requested = false;
            }
        }
        for (auto ni : string_interfaces) {
            if (ni->send_requested || ni->last_send > time ||
                ni->last_send + ni->update_interval <= time) {
                unsigned int strl           = strlen(ni->key) + 1;
                unsigned int strl_value     = strlen(ni->value) + 1;
                unsigned int buf_len_needed = 1 + strl + strl_value;
                buf_len_needed += buf_len_needed / 0xFF + 3 + 2; // COBS overhead
                if (BUF_SIZE < buf_len_needed) continue; // Error: update to large, skip interface
                if (BUF_SIZE < buf_len_used + buf_len_needed + 2) {
                    uint8_t cobs_buf[BUF_SIZE];
                    uint16_t crc        = calc_fletcher_checksum(buf, buf_len_used);
                    buf[buf_len_used++] = crc >> 8;
                    buf[buf_len_used++] = crc & 0xFF;
                    unsigned int cobs_buf_needed =
                        cobs_encode(buf, buf_len_used, cobs_buf, BUF_SIZE);
                    write_pkg(cobs_buf, cobs_buf_needed);
                    buf_len_used = 0;
                }
                buf[buf_len_used++] = 0x02;
                memcpy((char *)&buf[buf_len_used], ni->key, strl);
                buf_len_used += strl;
                memcpy((char *)&buf[buf_len_used], ni->value, strl_value);
                buf_len_used += strl_value;
                ni->last_send      = time;
                ni->send_requested = false;
            }
        }
        uint8_t cobs_buf[BUF_SIZE];
        uint16_t crc                 = calc_fletcher_checksum(buf, buf_len_used);
        buf[buf_len_used++]          = crc >> 8;
        buf[buf_len_used++]          = crc & 0xFF;
        unsigned int cobs_buf_needed = cobs_encode(buf, buf_len_used, cobs_buf, BUF_SIZE);
        write_pkg(cobs_buf, cobs_buf_needed);
    }

    int parse_stream_buf(uint8_t *buf, unsigned int buf_size) {
        int parsed = 0;
        for (unsigned int i = 0; i < buf_size; i += 1) {
            if (buf[i] == 0x00) {
                unsigned int buf_used = buf_size - parsed;
                uint8_t packet_buf[BUF_SIZE];
                unsigned int packed_buf_used = sizeof(packet_buf);
                if (cobs_decode(buf + parsed, buf_used, packet_buf, packed_buf_used) == 0) {
                    parse_pkg(packet_buf, packed_buf_used);
                }
                parsed = i + 1;
            }
        }
        return parsed;
    }

  protected:
    uint8_t stream_buf[BUF_SIZE];
    unsigned int stream_buf_used = 0;
    void handle_stream_buf(unsigned int buf_new_size) {
        for (unsigned int i = stream_buf_used - buf_new_size; i < stream_buf_used; i += 1) {
            if (stream_buf[i] == 0x00) {
                unsigned int stream_buf_pkg_delimiter = i;
                uint8_t packet_buf[BUF_SIZE];
                unsigned int packed_buf_used = sizeof(packet_buf);
                if (cobs_decode(stream_buf, stream_buf_pkg_delimiter, packet_buf,
                                packed_buf_used) == 0) {
                    parse_pkg(packet_buf, packed_buf_used);
                }
                i += 1;
                memmove(stream_buf, stream_buf + i, i);
                stream_buf_used -= i;
                i = 0;
            }
        }
    }

  public:
    int handle_stream(const uint8_t *buf_new, unsigned int buf_new_size) {
        if (buf_new_size + stream_buf_used >= BUF_SIZE) {
            /* ERROR: buffer overflow -> discared data */
            stream_buf_used = 0;
            return BUFFER_OVERFLOW;
        }
        memcpy(stream_buf + stream_buf_used, buf_new, buf_new_size);
        stream_buf_used += buf_new_size;
        handle_stream_buf(buf_new_size);
        return 0;
    }

#ifdef ARDUINO
    SimpleSync(Print *ostream, function<void(NumberInterface *&, char *, int)> number_update,
               function<void(StringInterface *&, char *, char *)> string_update)
        : write_pkg([ostream](uint8_t *buf, unsigned int buf_size) {
              if (ostream) {
                  ostream->write(buf, buf_size);
                  return 0;
              }
              return -1;
          }),
          number_update(number_update), string_update(string_update) {}
    int handle_stream(Stream &in_stream) {
        unsigned int buf_new_size = in_stream.available();
        if (buf_new_size + stream_buf_used > BUF_SIZE) {
            /* ERROR: buffer overflow -> discared data */
            stream_buf_used = 0;
            return BUFFER_OVERFLOW;
        }
        in_stream.readBytes(stream_buf + stream_buf_used, buf_new_size);
        stream_buf_used += buf_new_size;
        handle_stream_buf(buf_new_size);
        return 0;
    }
#endif
};

template <typename time_t, typename timedelta_t, unsigned int buf_len = 256,
          unsigned int str_len_max = 32>
class SimpleSyncStatic : public SimpleSync<time_t, timedelta_t, buf_len, str_len_max> {
  public:
    SimpleSyncStatic(function<int(uint8_t *, unsigned int)> write_pkg)
        : SimpleSync<time_t, timedelta_t, buf_len, str_len_max>(write_pkg, nullptr, nullptr){};
#ifdef ARDUINO
    SimpleSyncStatic(Print *ostream)
        : SimpleSync<time_t, timedelta_t, buf_len, str_len_max>(ostream, nullptr, nullptr) {}
#endif
};
} // namespace simplesync
#endif // SIMPLESYNC_HPP