#ifndef SIMPLESYNC_HPP
#define SIMPLESYNC_HPP

#ifdef ARDUINO
#include <Arduino.h>
#ifdef __AVR__
#include "include/nonstd_functional.hpp"
using nonstd::function;
#else /* ndef __AVR__ */
#define STDCLIB
#endif
#else /* ndef ARDUINO */
#define STDCLIB
#endif
#ifdef STDCLIB
#include <cstdint>
#include <cstdlib>
#include <cstring>
#include <functional>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
using std::function;
using std::string;
#endif

namespace simplesync {

enum error_t {
    BUFFER_OVERFLOW   = (-1),
    INVALID_COMMAND   = (-2),
    UNKNOWN_INTERFACE = (-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 timepoint_t, typename timepointdelta_t, unsigned int BUF_SIZE = 256,
          bool DYNAMIC_INTERFACES = false>
class SimpleSync {
  public:
    class Interface {
      public:
        uint8_t type;
        string key;

        std::function<int(uint8_t *buf, unsigned int buf_size)> pack;
        std::function<int(uint8_t *buf, unsigned int buf_size)> unpack;

        bool send_requested = true;
        timepoint_t last_send;

        timepointdelta_t update_interval;
        bool update_periodically;

        void *data;
        bool data_owned;

        Interface(uint8_t type, string key, void *data,
                  std::function<int(uint8_t *buf, unsigned int buf_size)> pack,
                  std::function<int(uint8_t *buf, unsigned int buf_size)> unpack)
            : type(type), key(key), pack(pack), unpack(unpack), update_periodically(false),
              data(data), data_owned(false) {}
        ~Interface() {
            if (data != nullptr && data_owned) switch (type) {
                case 0x01:
                    delete (int *)data;
                    break;
                case 0x02:
                    delete (string *)data;
                    break;
                default:
                    free(data);
                    break;
                }
        }
    };

    static Interface *NumberPtr(SimpleSync &sync, string key, int *value_ptr) {
        sync.interfaces.push_back(std::unique_ptr<Interface>(new Interface(
            0x01, key, (void *)value_ptr,
            [=](uint8_t *buf, unsigned int buf_size) -> int { /* pack */
                                                              if (buf_size < sizeof(int) * 8 / 7)
                                                                  return BUFFER_OVERFLOW;
                                                              return encode_varint(*value_ptr, buf);
            },
            [=](uint8_t *buf, unsigned int buf_size) -> int { /* unpack */
                                                              buf_size = 1 * buf_size;
                                                              return decode_varint(buf, *value_ptr);
            })));
        return sync.interfaces.back().get();
    }

    static Interface *NumberPtrPeriodic(SimpleSync &sync, string key, int *value_ptr,
                                        timepointdelta_t update_interval) {
        auto i = NumberPtr(sync, key, value_ptr);
        if (i) {
            i->update_interval     = update_interval;
            i->update_periodically = true;
        }
        return i;
    }

    void add_number(string key, int *value_ptr, timepointdelta_t update_interval) {
        NumberPtrPeriodic(*this, key, value_ptr, update_interval);
    }

    static Interface *Number(SimpleSync &sync, string key) {
        auto v_ptr   = new int;
        Interface *i = NumberPtr(sync, key, v_ptr);
        if (i)
            i->data_owned = true;
        else
            delete v_ptr;
        return i;
    }

    static Interface *TextPtr(SimpleSync &sync, string key, string *value_ptr) {
        sync.interfaces.push_back(std::unique_ptr<Interface>(new Interface(
            0x02, key, (void *)value_ptr,
            [=](uint8_t *buf, unsigned int buf_size) -> int { /* pack */
                                                              auto value_len =
                                                                  1 + strlen(value_ptr->c_str());
                                                              if (buf_size < value_len)
                                                                  return BUFFER_OVERFLOW;
                                                              memcpy(buf, value_ptr->c_str(),
                                                                     value_len);
                                                              return value_len;
            },
            [=](uint8_t *buf, unsigned int buf_size) -> int { /* unpack */
                                                              auto strl = strlen((char *)buf) + 1;
                                                              if (strl > buf_size)
                                                                  return BUFFER_OVERFLOW;
                                                              value_ptr->assign((char *)buf);
                                                              return strl;
            })));
        return sync.interfaces.back().get();
    }

    static Interface *TextPtrPeriodic(SimpleSync &sync, string key, string *value_ptr,
                                      timepointdelta_t update_interval) {
        auto i = TextPtr(sync, key, value_ptr);
        if (i) {
            i->update_interval     = update_interval;
            i->update_periodically = true;
        }
        return i;
    }

    static Interface *Text(SimpleSync &sync, string key) {
        auto v_ptr   = new string;
        Interface *i = TextPtr(sync, key, v_ptr);
        if (i)
            i->data_owned = true;
        else
            delete v_ptr;
        return i;
    }

    // int parse_number(uint8_t *buf, unsigned int buf_size, int* value){
    //     return decode_varint(&buf[parsed], *value);
    // }

    std::vector<std::unique_ptr<Interface>> interfaces;

    function<int(uint8_t *, unsigned int)> write_pkg;
    function<timepoint_t()> get_time;
    SimpleSync(function<int(uint8_t *, unsigned int)> write_pkg, function<timepoint_t()> get_time)
        : write_pkg(write_pkg), get_time(get_time) {
        last_pkg_recived = get_time();}

    Interface *get_interface(const uint8_t type, const char *const key) const {
        for (auto &i : interfaces) {
            if (i->type == type && strcmp(i->key.c_str(), key) == 0) {
                return i.get();
            }
        }
        return nullptr;
    }
    Interface *get_or_create_interface(uint8_t type, const char *const key) {
        Interface *i_exists = get_interface(type, key);
        if (i_exists) {
            return i_exists;
        }
        if (DYNAMIC_INTERFACES) {
            switch (type) {
            case 0x01:
                return Number(*this, key);
            case 0x02:
                return Text(*this, key);
            }
        }
        return nullptr;
    }

    bool watchdog_enabled = false;
    function<void()> watchdog_func;
    timepointdelta_t watchdog_timeout;
    timepoint_t last_pkg_recived;
    void set_watchdog(const function<void()> &f, timepointdelta_t timeout) {
        watchdog_enabled = bool(f);
        if (!watchdog_enabled) return;
        watchdog_timeout = timeout;
        watchdog_func    = f;
    }

    void check_watchdog(const timepoint_t &time) {
        if (watchdog_enabled && time > last_pkg_recived + watchdog_timeout) watchdog_func();
    }

    int parse_pkg(uint8_t *buf, unsigned int buf_size) {
        if (buf_size < 4) return CRC_MISSMATCH;
        uint16_t crc = calc_fletcher_checksum(buf, buf_size - 2);
        if (buf[--buf_size] != (crc & 0xFF) || (buf[--buf_size] != (crc >> 8)))
            return CRC_MISSMATCH;
        timepoint_t time = get_time();
        check_watchdog(time);
        for (unsigned int parsed = 0; parsed < buf_size;) {
            uint8_t type = buf[parsed];
            switch (type) {
            case 0x00: {
                char *key   = (char *)&buf[parsed + 1];
                auto keylen = strlen(key);
                parsed += 1 + keylen + 1;
                if (keylen == 0)
                    for (auto &ni : interfaces) ni->send_requested = true;
            } break;
            case 0x01:
            case 0x02: {
                char *key = (char *)&buf[parsed + 1];
                parsed += 1 + strlen(key) + 1;
                Interface *ni = get_or_create_interface(type, key);
                if (ni == nullptr) return UNKNOWN_INTERFACE;
                ni->send_requested = false;
                int unpacked_bytes = ni->unpack(buf + parsed, buf_size - parsed);
                if (unpacked_bytes >= 0) {
                    parsed += unpacked_bytes;
                } else { // ERROR -> return
                    return unpacked_bytes;
                }
            } break;
            default:
                // Error: Unknown datatype, ignore rest of the packet
                return -1;
            }
        }
        last_pkg_recived = time;
        return 0;
    }

    void request_all_interfaces() const {
        uint8_t buf[] = {0x00, 0x00, 0xac, 0xcd, 0xef};
        encode_and_write_pkg(buf, 2);
    }

  private:
    void encode_and_write_pkg(uint8_t *buf, unsigned int buf_size) const {
        if (buf_size <= 1) {

            return;
        }
        uint8_t cobs_buf[BUF_SIZE];
        uint16_t crc                 = calc_fletcher_checksum(buf, buf_size);
        buf[buf_size++]              = crc >> 8;
        buf[buf_size++]              = crc & 0xFF;
        unsigned int cobs_buf_needed = cobs_encode(buf, buf_size, cobs_buf, BUF_SIZE);
        write_pkg(cobs_buf, cobs_buf_needed);
    }

  public:
    void update() {
        timepoint_t time = get_time();
        check_watchdog(time);
        uint8_t buf[BUF_SIZE];
        unsigned int buf_len_used = 0;
        for (auto &ni : interfaces) {
            if (ni->send_requested ||
                (ni->update_periodically &&
                 (ni->last_send > time || ni->last_send <= time - ni->update_interval))) {
                unsigned int strl = strlen(ni->key.c_str()) + 1;
                unsigned int buf_len_needed =
                    1 + strl + 32; // TODO get real pack size (not always 32)
                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) {
                    encode_and_write_pkg(buf, buf_len_used);
                    buf_len_used = 0;
                }
                buf[buf_len_used++] = ni->type;
                memcpy((char *)&buf[buf_len_used], ni->key.c_str(), strl);
                buf_len_used += strl;
                int pack_len = ni->pack(&buf[buf_len_used], BUF_SIZE - buf_len_used);
                if (pack_len < 0) {
                    // ERROR -> return
                    buf_len_used = 0;
                    return;
                }
                buf_len_used += pack_len;
                ni->last_send      = time;
                ni->send_requested = false;
            }
        }
        encode_and_write_pkg(buf, buf_len_used);
        buf_len_used = 0;
    }

    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 (int i = stream_buf_used - buf_new_size; i < (int)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, stream_buf_used - i);
                stream_buf_used -= i;
                i = -1;
            }
        }
    }

  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;
    }

    int *get_number(const char *const key) {
        Interface *i = get_interface(0x01, key);
        if (i) return (int *)i->data;
        return nullptr;
    }

    string *get_string(const char *const key) {
        Interface *i = get_interface(0x02, key);
        if (i) return (string *)i->data;
        return nullptr;
    }

#ifdef ARDUINO
    SimpleSync(Print *ostream, function<timepoint_t()> get_time)
        : SimpleSync([ostream](uint8_t *buf, unsigned int buf_size) {
              if (ostream) {
                  ostream->write(buf, buf_size);
                  return 0;
              }
              return -1;
          }, get_time) {}
    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
};

} // namespace simplesync
#endif // SIMPLESYNC_HPP