#include <debug.h>
#include "payload_parser.h"

#define pp_check_capacity(pp, needed) \
    if ((pp)->current + (needed) > (pp)->end) { \
        if ((pp)->empty_handler == NULL || !(pp)->empty_handler(pp, needed)) {(pp)->ok = 0;} ; \
    }

void pp_skip(PayloadParser *pp, uint32_t num)
{
    pp->current += num;
}

uint8_t pp_u8(PayloadParser *pp)
{
    pp_check_capacity(pp, 1);
    if (!pp->ok) return 0;

    return *pp->current++;
}

uint16_t pp_u16(PayloadParser *pp)
{
    pp_check_capacity(pp, 2);
    if (!pp->ok) return 0;

    uint16_t x = 0;

    if (pp->bigendian) {
        x |= *pp->current++ << 8;
        x |= *pp->current++;
    } else {
        x |= *pp->current++;
        x |= *pp->current++ << 8;
    }
    return x;
}

uint32_t pp_u32(PayloadParser *pp)
{
    pp_check_capacity(pp, 4);
    if (!pp->ok) return 0;

    uint32_t x = 0;

    if (pp->bigendian) {
        x |= (uint32_t) (*pp->current++ << 24);
        x |= (uint32_t) (*pp->current++ << 16);
        x |= (uint32_t) (*pp->current++ << 8);
        x |= *pp->current++;
    } else {
        x |= *pp->current++;
        x |= (uint32_t) (*pp->current++ << 8);
        x |= (uint32_t) (*pp->current++ << 16);
        x |= (uint32_t) (*pp->current++ << 24);
    }
    return x;
}

uint64_t pp_u64(PayloadParser *pp)
{
    pp_check_capacity(pp, 8);
    if (!pp->ok) return 0;

    uint32_t x0, x1;
    uint64_t x;

    if (pp->bigendian) {
        x1 = pp_u32(pp);
        x0 = pp_u32(pp);
    } else {
        x0 = pp_u32(pp);
        x1 = pp_u32(pp);
    }
    x = ((uint64_t)x1)<<32 | x0;

    return x;
}

const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length)
{
    int32_t len = (int) (pp->end - pp->current);
    if (!pp->ok || len <= 0) {
        if (length != NULL) *length = 0;
        return NULL;
    }

    if (length != NULL) {
        *length = (uint32_t) len;
    }

    return pp->current;
}

/** Read int8_t from the payload. */
int8_t pp_i8(PayloadParser *pp)
{
    return ((union conv8) {.u8 = pp_u8(pp)}).i8;
}

/** Read int16_t from the payload. */
int16_t pp_i16(PayloadParser *pp)
{
    return ((union conv16) {.u16 = pp_u16(pp)}).i16;
}

/** Read int32_t from the payload. */
int32_t pp_i32(PayloadParser *pp)
{
    return ((union conv32) {.u32 = pp_u32(pp)}).i32;
}

/** Read int364_t from the payload. */
int64_t pp_i64(PayloadParser *pp)
{
    return ((union conv64) {.u64 = pp_u64(pp)}).i64;
}

/** Read 4-byte float from the payload. */
float pp_float(PayloadParser *pp)
{
    return ((union conv32) {.u32 = pp_u32(pp)}).f32;
}

/** Read 8-byte float from the payload. */
double pp_double(PayloadParser *pp)
{
    return ((union conv64) {.u64 = pp_u64(pp)}).f64;
}

/** Read a zstring */
uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen)
{
    pp_check_capacity(pp, 1);
    uint32_t len = 0;
    while (len < maxlen-1 && pp->current != pp->end) {
        char c = *buffer++ = *pp->current++;
        if (c == 0) break;
        len++;
    }
    *buffer = 0;
    return len;
}

/** Read a buffer */
uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen)
{
    uint32_t len = 0;
    while (len < maxlen && pp->current != pp->end) {
        *buffer++ = *pp->current++;
        len++;
    }
    return len;
}