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

#define I8(b)  (*(int8_t*)(b))
#define I16(b) ((int16_t)be16toh(*(uint16_t *)(b)))
#define I32(b) ((int32_t)be32toh(*(uint32_t *)(b)))
#define I64(b) ((int64_t)be64toh(*(uint64_t *)(b)))

#define U8(b)  (*(uint8_t*)(b))
#define U16(b) ((uint16_t)be16toh(*(uint16_t *)(b)))
#define U32(b) ((uint32_t)be32toh(*(uint32_t *)(b)))
#define 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, 1) \
    X(0xC3, FMT_BOOL_FALSE, MSGPACK_BOOL, ;, 1, 0, 0)

// byte, fmt, type, subtype, offset, length, data
#define FORMAT_INT(X)                                                                       \
    X(0x00, FMT_FIX_UINT, MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 0, m->u = U8(pack->bin))    \
    X(0XE0, FMT_FIX_INT,  MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 0, m->i = I8(pack->bin))    \
    X(0xCC, FMT_U8,       MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 1, m->u = U8(pack->bin+1))  \
    X(0xCD, FMT_U16,      MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 2, m->u = U16(pack->bin+1)) \
    X(0xCE, FMT_U32,      MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 4, m->u = U32(pack->bin+1)) \
    X(0xCF, FMT_U64,      MSGPACK_INT, MSGPACK_INT_UNSIGNED, 1, 8, m->u = U64(pack->bin+1)) \
    X(0xD0, FMT_I8,       MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 1, m->i = I8(pack->bin+1))  \
    X(0xD1, FMT_I16,      MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 2, m->i = I16(pack->bin+1)) \
    X(0xD2, FMT_I32,      MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 4, m->i = I32(pack->bin+1)) \
    X(0xD3, FMT_I64,      MSGPACK_INT, MSGPACK_INT_SIGNED,   1, 8, m->i = 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, m->_u = U32(pack->bin+1)) \
    X(0xCB, FMT_DOUBLE, MSGPACK_FLOAT, MSGPACK_FLOAT_64, 1, 8, m->_u = U64(pack->bin+1)) \

// byte, fmt, type, subtype, offset, length
#define FORMAT_RAW(X)                                                           \
    X(0xA0, FMT_FIX_STR,  MSGPACK_RAW, MSGPACK_RAW_STRING, 1, pack->bin[0] & 1) \
    X(0xC4, FMT_BIN8,     MSGPACK_RAW, MSGPACK_RAW_BIN,    1, pack->bin[1])     \
    X(0xC5, FMT_BIN16,    MSGPACK_RAW, MSGPACK_RAW_BIN,    3, U16(pack->bin+1)) \
    X(0xC6, FMT_BIN32,    MSGPACK_RAW, MSGPACK_RAW_BIN,    5, 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, U16(pack->bin+1)) \
    X(0xDB, FMT_STR32,    MSGPACK_RAW, MSGPACK_RAW_STRING, 5, 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, U16(pack->bin+1),    3, elements_length(pack, fmt)) \
    X(0xDD, FMT_ARRAY32,   MSGPACK_ARRAY, 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 * (U16(pack->bin+1)),    3, elements_length(pack, fmt)) \
    X(0xDF, FMT_MAP32,   MSGPACK_MAP, 2 * (U16(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_READ_ARGS(type, pack, m)                                    \
    do {                                                                  \
        if(!(pack) || !(pack)->bin || (pack)->size == 0)                  \
            return ERROR((type), MSGPACK_ERROR_INVALID_PACK);             \
        if((pack)->bin[0] == 0xc)                                         \
            return ERROR2((type), MSGPACK_ERROR_WRONG_TYPE, MSGPACK_NIL); \
        if(!(m))                                                          \
            return ERROR((type), MSGPACK_ERROR_INVALID_ARGUMENT);         \
    } while(0)



#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)); \
        x_data;                                                         \
        return SUCCESS2(x_type, x_subtype);                             \
    }

int msgpack_read_int(const msgpack_t *pack, union mp_int *m)
{
    CHECK_READ_ARGS(MSGPACK_INT, pack, m);

    enum msgpack_fmt fmt = pack_fmt(pack);

    switch(fmt) {
        FORMAT_INT(X_READ_INT)
    default:
        return ERROR2(MSGPACK_INT, MSGPACK_ERROR_WRONG_TYPE, fmt_to_type[fmt]);
    }
}

#define X_READ_BOOL(_byte, x_fmt, x_type, _, _offset, _length, x_data) \
    case x_fmt: {               \
        *m = x_data;            \
        return SUCCESS(x_type); \
    }
int msgpack_read_bool(const msgpack_t *pack, int *m)
{
    CHECK_READ_ARGS(MSGPACK_BOOL, pack, m);

    enum msgpack_fmt fmt = pack_fmt(pack);

    switch(fmt) {
        FORMAT_BOOL(X_READ_BOOL)
    default:
        return ERROR2(MSGPACK_BOOL, MSGPACK_ERROR_WRONG_TYPE, fmt_to_type[fmt]);
    }
}

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

int msgpack_read_float(const msgpack_t *pack, union mp_float *m)
{
    CHECK_READ_ARGS(MSGPACK_FLOAT, pack, m);

    enum msgpack_fmt fmt = pack_fmt(pack);

    switch(fmt) {
        FORMAT_FLOAT(X_READ_FLOAT)
    default:
        return ERROR2(MSGPACK_FLOAT, MSGPACK_ERROR_WRONG_TYPE, fmt_to_type[fmt]);
    }   
}

int msgpack_read_raw     (const msgpack_t *pack, struct mp_bin *m);
int msgpack_read_raw_cpy (const msgpack_t *pack, struct mp_bin *m);
int msgpack_read_ext     (const msgpack_t *pack, struct mp_bin *m);
int msgpack_read_ext_cpy (const msgpack_t *pack, struct mp_bin *m);

int msgpack_read_array   (msgpack_t *pack, size_t *length);
int msgpack_read_map     (msgpack_t *pack, size_t *length);
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, int subtype);
int msgpack_write_bool    (msgpack_t *pack, const       int      *m);
int msgpack_write_float   (msgpack_t *pack, const union mp_float *m, int subtype);

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

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


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

#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);
    unsigned char byte = pack->bin[0];

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