#include <stdlib.h>
#include <stdint.h>
#include <stdarg.h>
#include <string.h>
#include <errno.h>

#include <skipstone/link.h>
#include <skipstone/map.h>
#include <skipstone/queue.h>
#include <skipstone/message.h>

#define case_type(t, type_t) \
    case t: { \
        type_t v = va_arg(args, type_t); \
        if (len < offset + sizeof(v)) { \
            goto done; \
        } else { \
            memcpy((uint8_t *)message + offset, &v, sizeof(v)); \
            offset += sizeof(v); \
        } \
        break; \
    }

#define case_type_int(t, type_t) \
    case t: { \
        type_t v = (type_t)va_arg(args, int); \
        if (len < offset + sizeof(v)) { \
            goto done; \
        } else { \
            memcpy((uint8_t *)message + offset, &v, sizeof(v)); \
            offset += sizeof(v); \
        } \
        break; \
    }

ssize_t skipstone_message_pack(void *message,
                               size_t len,
                               const char *template, ...) {
    va_list args;

    size_t offset = 0,
           i;

    va_start(args, template);

    for (i=0; template[i]; i++) {
        char c = template[i];

        switch (c) {
            case_type_int('c', int8_t);
            case_type_int('C', uint8_t);
            case_type_int('s', int16_t);
            case_type_int('S', uint16_t);
            case_type('l', int32_t);
            case_type('L', uint32_t);
            case_type('q', int64_t);
            case_type('Q', uint64_t);

            case 'z': {
                char *value = va_arg(args, char *);
                size_t sz = strlen(value);

                if (len < offset + sz) {
                    goto done;
                }

                memcpy((uint8_t *)message + offset, value, sz);

                break;
            }

            default:
                errno = EINVAL;

                goto error_invalid_template;
        }
    }

    va_end(args);

done:
    return offset;

error_invalid_template:
    return -1;
}