#include <assert.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

/*
 * Pack header data into a byte.
 * 3 bits are used for type (2^3 = 8 different types)
 * 5 bits are used for length (2^5 = 32 max length, unit is up to you)
 */

typedef enum {
    Ok = 0,
    OversizedType,
    OversizedLen,
} Result;

#define TYPE_LEN (4)
#define LEN      (4)

Result msg_create_header(uint8_t type, uint8_t len, uint8_t *out);
void   msg_parse_header(uint8_t header, uint8_t *type_out, uint8_t *len_out);

int main(void) {
    assert(TYPE_LEN + LEN <= 8);
    uint8_t maxlen = (1 << LEN) - 1;
    uint8_t maxtype = (1 << TYPE_LEN) - 1;

    {
        uint8_t hdr;
        Result  res = msg_create_header(3, 8, &hdr);

        if (res == OversizedType)
            printf("The type is out of range");
        if (res == OversizedLen)
            printf("The length is out of range");

        printf("Header: 0x%02X\n", hdr);
        printf("Error: %d\n", res);

        uint8_t plen, ptype;
        msg_parse_header(hdr, &ptype, &plen);
        printf("Type: %d\n", ptype);
        printf("Len: %d\n", plen);
    }
    {
        printf("Max len: %d\n", maxlen);
        printf("Max type: %d\n", maxtype);

        uint8_t hdr;
        Result  res = msg_create_header(maxtype, maxlen, &hdr);
        assert(res == Ok);

        uint8_t plen, ptype;
        msg_parse_header(hdr, &ptype, &plen);
        printf("plen: %d\n", plen);
        printf("ptype: %d\n", ptype);
        assert(ptype == maxtype);
        assert(plen == maxlen);
    }
    {
        for (int i = 0; i < 1000; i++) {
            uint8_t len = arc4random() % maxlen;
            uint8_t type = arc4random() % maxtype;
            uint8_t hdr;
            Result  res = msg_create_header(type, len, &hdr);
            assert(res == Ok);

            uint8_t plen, ptype;
            msg_parse_header(hdr, &ptype, &plen);
            assert(ptype == type);
            assert(plen == len);
        }
    }

    return 0;
}

Result msg_create_header(uint8_t type, uint8_t len, uint8_t *out) {
    if (type > (1 << TYPE_LEN) - 1)
        return OversizedType;

    if (len > (1 << LEN) - 1)
        return OversizedLen;

    *out = (type << LEN) | (len & (0xFF >> TYPE_LEN));

    return Ok;
}

void msg_parse_header(uint8_t header, uint8_t *type_out, uint8_t *len_out) {
    *type_out = (header >> LEN) & ((1 << LEN) - 1);
    *len_out = header & (0xFF >> TYPE_LEN);
}