#include "rd.h"

#include <string.h>

void tobi_rd_init(tobi_rd *rd, const uint8_t *data, size_t len, size_t base) {
    rd->data = data;
    rd->len = len;
    rd->pos = 0;
    rd->base = base;
    rd->failed = 0;
}

size_t tobi_rd_off(const tobi_rd *rd) {
    return rd->base + rd->pos;
}

size_t tobi_rd_left(const tobi_rd *rd) {
    return rd->pos <= rd->len ? rd->len - rd->pos : 0;
}

int tobi_rd_u8(tobi_rd *rd, uint8_t *out) {
    if (rd->pos >= rd->len) {
        rd->failed = 1;
        return 0;
    }
    *out = rd->data[rd->pos++];
    return 1;
}

int tobi_rd_peek(const tobi_rd *rd, uint8_t *out) {
    if (rd->pos >= rd->len) {
        return 0;
    }
    *out = rd->data[rd->pos];
    return 1;
}

int tobi_rd_u16(tobi_rd *rd, uint16_t *out) {
    uint8_t b[2];
    if (!tobi_rd_bytes(rd, b, sizeof(b))) {
        return 0;
    }
    *out = (uint16_t)b[0] | ((uint16_t)b[1] << 8);
    return 1;
}

int tobi_rd_u32(tobi_rd *rd, uint32_t *out) {
    uint8_t b[4];
    if (!tobi_rd_bytes(rd, b, sizeof(b))) {
        return 0;
    }
    *out = (uint32_t)b[0] | ((uint32_t)b[1] << 8) | ((uint32_t)b[2] << 16) | ((uint32_t)b[3] << 24);
    return 1;
}

int tobi_rd_u64(tobi_rd *rd, uint64_t *out) {
    uint8_t b[8];
    if (!tobi_rd_bytes(rd, b, sizeof(b))) {
        return 0;
    }
    uint64_t v = 0;
    for (size_t i = 0; i < 8; i++) {
        v |= (uint64_t)b[i] << (i * 8);
    }
    *out = v;
    return 1;
}

int tobi_rd_bytes(tobi_rd *rd, uint8_t *out, size_t n) {
    if (n > tobi_rd_left(rd)) {
        rd->failed = 1;
        return 0;
    }
    if (n != 0) {
        memcpy(out, rd->data + rd->pos, n);
    }
    rd->pos += n;
    return 1;
}

int tobi_rd_skip(tobi_rd *rd, size_t n) {
    if (n > tobi_rd_left(rd)) {
        rd->failed = 1;
        return 0;
    }
    rd->pos += n;
    return 1;
}

int tobi_rd_sub(tobi_rd *rd, size_t n, tobi_rd *sub) {
    if (n > tobi_rd_left(rd)) {
        rd->failed = 1;
        return 0;
    }
    tobi_rd_init(sub, rd->data + rd->pos, n, rd->base + rd->pos);
    rd->pos += n;
    return 1;
}

int tobi_rd_pkg_len(tobi_rd *rd, size_t *pkg_len, size_t *encoded_len) {
    size_t start = rd->pos;
    uint8_t lead = 0;
    if (!tobi_rd_u8(rd, &lead)) {
        return 0;
    }
    unsigned follow = lead >> 6;
    size_t len = 0;
    if (follow == 0) {
        len = (size_t)(lead & 0x3f);
    } else {
        len = (size_t)(lead & 0x0f);
        for (unsigned i = 0; i < follow; i++) {
            uint8_t b = 0;
            if (!tobi_rd_u8(rd, &b)) {
                rd->pos = start;
                rd->failed = 1;
                return 0;
            }
            len |= (size_t)b << (4 + 8 * i);
        }
    }
    *pkg_len = len;
    *encoded_len = rd->pos - start;
    return 1;
}