#include "msgpack.h"
#include <endian.h>

#define TO_I8(b)  (*(int8_t*)(b))
#define TO_I16(b) ((int16_t)be16toh(*(uint16_t *)(b)))
#define TO_I32(b) ((int32_t)be32toh(*(uint32_t *)(b)))
#define TO_I64(b) ((int64_t)be64toh(*(uint64_t *)(b)))

#define TO_U8(b)  (*(uint8_t*)(b))
#define TO_U16(b) ((uint16_t)be16toh(*(uint16_t *)(b)))
#define TO_U32(b) ((uint32_t)be32toh(*(uint32_t *)(b)))
#define TO_U64(b) ((uint64_t)be64toh(*(uint64_t *)(b)))

#define ENOUGH_BYTES(pack, n, on_fail) if((pack)->size < (n)) { on_fail; }

// range low, range high
#define RANGES(X) \
    X(0x00, 0x7F)        \
    X(0x80, 0x8F)        \
    X(0x90, 0x9F)        \
    X(0xA0, 0xBF)        \
    X(0xE0, 0xFF)

#define FORMAT_NIL(X)                              \
    X(0xC1, FMT_UNKNOWN, MSGPACK_UNKNOWN, ;, 1, 0) \
    X(0xC0, FMT_NIL,     MSGPACK_NIL,     ;, 1, 0)

// byte, fmt, type, _, offset, length, data
#define FORMAT_BOOL(X)                                    \
    X(0xC2, FMT_BOOL_TRUE,  MSGPACK_BOOL, ;, 1, 0, true)  \
    X(0xC3, FMT_BOOL_FALSE, MSGPACK_BOOL, ;, 1, 0, false)

// byte, fmt, type, subtype, offset, length, data
#define FORMAT_INT(X)                                                                   \
    X(0x00, FMT_FIX_UINT, MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 0, TO_U8(pack->bin))    \
    X(0XE0, FMT_FIX_INT,  MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 0, TO_I8(pack->bin))    \
    X(0xCC, FMT_U8,       MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 1, TO_U8(pack->bin+1))  \
    X(0xCD, FMT_U16,      MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 2, TO_U16(pack->bin+1)) \
    X(0xCE, FMT_U32,      MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 4, TO_U32(pack->bin+1)) \
    X(0xCF, FMT_U64,      MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 8, TO_U64(pack->bin+1)) \
    X(0xD0, FMT_I8,       MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 1, TO_I8(pack->bin+1))  \
    X(0xD1, FMT_I16,      MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 2, TO_I16(pack->bin+1)) \
    X(0xD2, FMT_I32,      MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 4, TO_I32(pack->bin+1)) \
    X(0xD3, FMT_I64,      MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 8, TO_I64(pack->bin+1))

// byte, fmt, type, subtype, offset, length, data
#define FORMAT_FLOAT(X)                                                                  \
    X(0xCA, FMT_FLOAT,  MSGPACK_FLOAT, MSGPACK_FLOAT_32, 1, 4, TO_U32(pack->bin+1)) \
    X(0xCB, FMT_DOUBLE, MSGPACK_FLOAT, MSGPACK_FLOAT_64, 1, 8, TO_U64(pack->bin+1))

// byte, fmt, type, subtype, offset, length, data
#define FORMAT_RAW(X)                                                              \
    X(0xA0, FMT_FIX_STR,  MSGPACK_RAW, MSGPACK_RAW_STRING, 1, pack->bin[0] & 0x1F) \
    X(0xC4, FMT_BIN8,     MSGPACK_RAW, MSGPACK_RAW_BIN,    1, pack->bin[1])        \
    X(0xC5, FMT_BIN16,    MSGPACK_RAW, MSGPACK_RAW_BIN,    3, TO_U16(pack->bin+1)) \
    X(0xC6, FMT_BIN32,    MSGPACK_RAW, MSGPACK_RAW_BIN,    5, TO_U32(pack->bin+1)) \
    X(0xD9, FMT_STR8,     MSGPACK_RAW, MSGPACK_RAW_STRING, 1, pack->bin[1])        \
    X(0xDA, FMT_STR16,    MSGPACK_RAW, MSGPACK_RAW_STRING, 3, TO_U16(pack->bin+1)) \
    X(0xDB, FMT_STR32,    MSGPACK_RAW, MSGPACK_RAW_STRING, 5, TO_U32(pack->bin+1))

// byte, fmt, type, elements, offset, lenght
#define FORMAT_ARRAY(X) \
    X(0x90, FMT_FIX_ARRAY, MSGPACK_ARRAY, pack->bin[0] & 0x0F, 1, elements_length(pack, fmt)) \
    X(0xDC, FMT_ARRAY16,   MSGPACK_ARRAY, TO_U16(pack->bin+1),    3, elements_length(pack, fmt)) \
    X(0xDD, FMT_ARRAY32,   MSGPACK_ARRAY, TO_U32(pack->bin+1),    5, elements_length(pack, fmt))
#define FORMAT_MAP(X) \
    X(0x80, FMT_FIX_MAP, MSGPACK_MAP, 2 * (pack->bin[0] & 0x0F), 1, elements_length(pack, fmt)) \
    X(0xDE, FMT_MAP16,   MSGPACK_MAP, 2 * (TO_U16(pack->bin+1)),    3, elements_length(pack, fmt)) \
    X(0xDF, FMT_MAP32,   MSGPACK_MAP, 2 * (TO_U32(pack->bin+1)),    5, elements_length(pack, fmt))

#define FORMAT_EXT(X)

#define FORMATS(X)   \
    FORMAT_NIL  (X)  \
    FORMAT_BOOL (X)  \
    FORMAT_INT  (X)  \
    FORMAT_FLOAT(X)  \
    FORMAT_RAW  (X)  \
    FORMAT_ARRAY(X)  \
    FORMAT_MAP  (X)  \
    FORMAT_EXT  (X)

#define EXPAND(X) X
#define X_FMT_ENUM(_byte, fmt, ...)       fmt,
#define X_FMT_TYPE(_byte, fmt, type, ...) [fmt] = type,
#define X_BYTE_FMT( byte, fmt, ...)       [byte] = fmt,

enum msgpack_fmt {
    FORMATS(X_FMT_ENUM)
};

static const enum msgpack_type fmt_to_type[] = {
    FORMATS(X_FMT_TYPE)
};
static const enum msgpack_fmt byte_to_fmt[] = {
    FORMATS(X_BYTE_FMT)
};

static enum msgpack_fmt pack_fmt(const msgpack_t *pack);
// static size_t elements_length(msgpack_t *pack, enum msgpack_fmt fmt);
// static size_t pack_size(msgpack_t *pack);

#define SUCCESS(type) SUCCESS2(type, 0)
#define ERROR(type, err) ERROR2(type, err, 0)
#define SUCCESS2(type, rest) \
    (((rest) << 6) | (type))
#define ERROR2(type, err, rest) \
    ((1 << (sizeof(int)*8-1))| ((rest) << 6) | ((err) << 3) | (type))

#define CHECK_PACK(type, pack)                       \
    if(!(pack) || !(pack)->bin || (pack)->size == 0) \
        return ERROR((type), MSGPACK_ERROR_INVALID_PACK)

#define CHECK_ARG(type, m)  \
    if(!(m))                \
        return ERROR((type), MSGPACK_ERROR_INVALID_ARGUMENT)

#define MOVE_PACK(pack, sz) \
    do {                    \
        pack->bin  += (sz); \
        pack->size -= (sz); \
    } while(0)

#define READ_FUNCTION(type)                                             \
    {                                                                   \
        CHECK_PACK(MSGPACK_##type, pack);                               \
        CHECK_ARG (MSGPACK_##type, m);                                  \
                                                                        \
        enum msgpack_fmt fmt = pack_fmt(pack);                          \
                                                                        \
        switch(fmt) {                                                   \
            FORMAT_##type(X_READ_##type)                                \
        default:                                                        \
            return ERROR2(MSGPACK_##type, MSGPACK_ERROR_WRONG_TYPE,     \
                          fmt_to_type[fmt]);                            \
        }                                                               \
    }

#define X_READ_BOOL(_byte, x_fmt, x_type, _, x_offset, x_length, x_data) \
    case x_fmt:                                                          \
        MOVE_PACK(pack, (x_offset));                                     \
        *m = x_data;                                                     \
        return SUCCESS(x_type);

#define X_READ_INT(_byte, x_fmt, x_type, x_subtype, x_offset, x_length, x_data) \
    case x_fmt:                                                                 \
        ENOUGH_BYTES(pack, (x_offset) + (x_length),                             \
            return ERROR2(x_type, MSGPACK_ERROR_UNEXPECTED_END, x_subtype));    \
        MOVE_PACK(pack, (x_offset) + (x_length));                               \
                                                                                \
        m->u = x_data;                                                          \
        return SUCCESS2(x_type, x_subtype);

#define X_READ_FLOAT(...) X_READ_INT(__VA_ARGS__)

#define X_READ_RAW(_byte, x_fmt, x_type, x_subtype, x_offset, x_length)      \
    case x_fmt:                                                              \
        ENOUGH_BYTES(pack, (x_offset),                                       \
            return ERROR2(x_type, MSGPACK_ERROR_UNEXPECTED_END, x_subtype)); \
        ENOUGH_BYTES(pack, (size_t)(x_offset) + (x_length),                  \
            return ERROR2(x_type, MSGPACK_ERROR_UNEXPECTED_END, x_subtype)); \
        MOVE_PACK(pack, (x_offset) + (x_length));                            \
                                                                             \
        m->bin = pack->bin + (x_offset);                                     \
        m->size = (x_length);                                                \
        return SUCCESS2(x_type, x_subtype);

#define X_READ_EXT(...) X_READ_RAW(__VA_ARGS__)

#define X_READ_ARRAY(_byte, x_fmt, x_type, x_elements, x_offset, _length)    \
    case x_fmt:                                                              \
        ENOUGH_BYTES(pack, (x_offset),                                       \
            return ERROR(x_type, MSGPACK_ERROR_UNEXPECTED_END));             \
        MOVE_PACK(pack, (x_offset));                                         \
                                                                             \
        *m = x_elements;                                                     \
        return SUCCESS(x_type);

#define X_READ_MAP(...) X_READ_ARRAY(__VA_ARGS__)

int msgpack_read_bool  (msgpack_t *pack,        bool     *m) READ_FUNCTION(BOOL)
int msgpack_read_int   (msgpack_t *pack, union  mp_int   *m) READ_FUNCTION(INT)
int msgpack_read_float (msgpack_t *pack, union  mp_float *m) READ_FUNCTION(FLOAT)

int msgpack_read_raw   (msgpack_t *pack, struct mp_bin  *m)  READ_FUNCTION(RAW)
// int msgpack_read_raw_cpy (msgpack_t *pack, struct mp_bin *m);
int msgpack_read_ext   (msgpack_t *pack, struct mp_bin  *m)  READ_FUNCTION(EXT)
// int msgpack_read_ext_cpy (msgpack_t *pack, struct mp_bin *m);

int msgpack_read_array (msgpack_t *pack, size_t *m)          READ_FUNCTION(ARRAY)
int msgpack_read_map   (msgpack_t *pack, size_t *m)          READ_FUNCTION(MAP)
// int msgpack_read_array2  (const msgpack_t *pack, struct mp_array *m);
// int msgpack_read_map2    (const msgpack_t *pack, struct mp_map   *m);

int msgpack_write_int     (msgpack_t *pack, const union mp_int   *m, enum msgpack_type subtype);
int msgpack_write_bool    (msgpack_t *pack, const       bool     *m);
int msgpack_write_float   (msgpack_t *pack, const union mp_float *m, enum msgpack_type subtype);

int msgpack_write_raw     (msgpack_t *pack, const struct mp_bin  *m, enum msgpack_type subtype);
int msgpack_write_ext     (msgpack_t *pack, const struct mp_bin  *m, enum msgpack_type subtype);

int msgpack_write_array   (msgpack_t *pack, const size_t *m);
int msgpack_write_map     (msgpack_t *pack, const size_t *m);


#define X_MASK_RANGE(range_low, range_high)         \
    if(byte >= range_low && byte <= range_high)     \
        byte &= ~(uint8_t)(range_high-range_low);   \
    else                                            \

static enum msgpack_fmt pack_fmt(const msgpack_t *pack)
{
    ENOUGH_BYTES(pack, 1, return 0);
    uint8_t byte = pack->bin[0];

    RANGES(X_MASK_RANGE){};
    return byte_to_fmt[byte];
}

// #define X_COMPLEX_LENGTH(_byte, x_fmt, _type, x_elements, x_offset, x_length) \
//     case x_fmt: {                                 \
//         ENOUGH_BYTES(pack, (x_offset), return 0); \
//         offset   = (x_offset);                    \
//         elements = (x_elements);                  \
//     } break;

// static size_t elements_length(msgpack_t *pack, enum msgpack_fmt fmt)
// {
//     size_t offset   = 0;
//     size_t elements = 0;

//     switch(fmt) {
//         FORMAT_ARRAY(X_COMPLEX_LENGTH)
//         FORMAT_MAP(X_COMPLEX_LENGTH)
//     default: return 0;
//     }

//     for(size_t i = 0; i < elements; i++) {
//         msgpack_t new_pack = {pack->bin + offset,
//                               pack->size - offset};//, pack->membuf};
//         size_t len = pack_size(&new_pack);
//         if(len == 0) return 0;

//         offset += len;
//         ENOUGH_BYTES(pack, offset, return 0);
//     }

//     return offset;
// }

// #define X_PACK_SIZE(_byte, x_fmt, _type, _, x_offset, x_length, ...) \
//     case x_fmt: {                                 \
//         ENOUGH_BYTES(pack, (x_offset), return 0); \
//         offset = (x_offset);                      \
//         length = (x_length);                      \
//     } break;

// static size_t pack_size(msgpack_t *pack)
// {
//     enum msgpack_fmt fmt = pack_fmt(pack);

//     size_t offset = 0;
//     size_t length = 0;

//     switch(fmt) {
//         FORMATS(X_PACK_SIZE)
//     default: return 0;
//     }

//     if(fmt_to_type[fmt] == MSGPACK_ARRAY || fmt_to_type[fmt] == MSGPACK_MAP)
//         if(length == 0) return 0;

//     return offset + length;
// }