#include <asn_application.h>
#include <asn_internal.h>
#include <per_encoder.h>

static int _uper_encode_flush_outp(asn_per_outp_t *po);

static int
ignore_output(const void *data, size_t size, void *app_key) {
    (void)data;
    (void)size;
    (void)app_key;
    return 0;
}

asn_enc_rval_t
uper_encode(const asn_TYPE_descriptor_t *td,
            const asn_per_constraints_t *constraints, const void *sptr,
            asn_app_consume_bytes_f *cb, void *app_key) {
    asn_per_outp_t po;
    asn_enc_rval_t er = {0,0,0};

    /*
     * Invoke type-specific encoder.
     */
    if(!td || !td->op->uper_encoder)
        ASN__ENCODE_FAILED;	/* PER is not compiled in */

    po.buffer = po.tmpspace;
    po.nboff = 0;
    po.nbits = 8 * sizeof(po.tmpspace);
    po.output = cb ? cb : ignore_output;
    po.op_key = app_key;
    po.flushed_bytes = 0;

    er = td->op->uper_encoder(td, constraints, sptr, &po);
    if(er.encoded != -1) {
        size_t bits_to_flush;

        bits_to_flush = ((po.buffer - po.tmpspace) << 3) + po.nboff;

        /* Set number of bits encoded to a firm value */
        er.encoded = (po.flushed_bytes << 3) + bits_to_flush;

        if(_uper_encode_flush_outp(&po)) ASN__ENCODE_FAILED;
    }

    return er;
}

/*
 * Argument type and callback necessary for uper_encode_to_buffer().
 */
typedef struct enc_to_buf_arg {
	void *buffer;
	size_t left;
} enc_to_buf_arg;
static int encode_to_buffer_cb(const void *buffer, size_t size, void *key) {
	enc_to_buf_arg *arg = (enc_to_buf_arg *)key;

	if(arg->left < size)
		return -1;	/* Data exceeds the available buffer size */

	memcpy(arg->buffer, buffer, size);
	arg->buffer = ((char *)arg->buffer) + size;
	arg->left -= size;

	return 0;
}

asn_enc_rval_t
uper_encode_to_buffer(const asn_TYPE_descriptor_t *td,
                      const asn_per_constraints_t *constraints,
                      const void *sptr, void *buffer, size_t buffer_size) {
    enc_to_buf_arg key;

    key.buffer = buffer;
    key.left = buffer_size;

    if(td) ASN_DEBUG("Encoding \"%s\" using UNALIGNED PER", td->name);

    return uper_encode(td, constraints, sptr, encode_to_buffer_cb, &key);
}

typedef struct enc_dyn_arg {
	void *buffer;
	size_t length;
	size_t allocated;
} enc_dyn_arg;
static int
encode_dyn_cb(const void *buffer, size_t size, void *key) {
    enc_dyn_arg *arg = key;
    if(arg->length + size >= arg->allocated) {
        size_t new_size = arg->allocated ? arg->allocated : 8;
        void *p;

        do {
            new_size <<= 2;
        } while(arg->length + size >= new_size);

        p = REALLOC(arg->buffer, new_size);
        if(!p) {
            FREEMEM(arg->buffer);
            memset(arg, 0, sizeof(*arg));
            return -1;
        }
        arg->buffer = p;
        arg->allocated = new_size;
    }
    memcpy(((char *)arg->buffer) + arg->length, buffer, size);
    arg->length += size;
    return 0;
}
ssize_t
uper_encode_to_new_buffer(const asn_TYPE_descriptor_t *td,
                          const asn_per_constraints_t *constraints,
                          const void *sptr, void **buffer_r) {
    asn_enc_rval_t er = {0,0,0};
	enc_dyn_arg key;

	memset(&key, 0, sizeof(key));

	er = uper_encode(td, constraints, sptr, encode_dyn_cb, &key);
	switch(er.encoded) {
	case -1:
		FREEMEM(key.buffer);
		return -1;
	case 0:
		FREEMEM(key.buffer);
		key.buffer = MALLOC(1);
		if(key.buffer) {
			*(char *)key.buffer = '\0';
			*buffer_r = key.buffer;
			return 1;
		} else {
			return -1;
		}
	default:
		*buffer_r = key.buffer;
		ASN_DEBUG("Complete encoded in %ld bits", (long)er.encoded);
		return ((er.encoded + 7) >> 3);
	}
}

/*
 * Internally useful functions.
 */

/* Flush partially filled buffer */
static int
_uper_encode_flush_outp(asn_per_outp_t *po) {
	uint8_t *buf;

	if(po->nboff == 0 && po->buffer == po->tmpspace)
		return 0;

	buf = po->buffer + (po->nboff >> 3);
	/* Make sure we account for the last, partially filled */
	if(po->nboff & 0x07) {
		buf[0] &= 0xff << (8 - (po->nboff & 0x07));
		buf++;
	}

	return po->output(po->tmpspace, buf - po->tmpspace, po->op_key);
}

asn_enc_rval_t
aper_encode_to_buffer(const asn_TYPE_descriptor_t *td,
                      const asn_per_constraints_t *constraints,
                      const void *sptr, void *buffer, size_t buffer_size) {
    enc_to_buf_arg key;

    key.buffer = buffer;
    key.left = buffer_size;

    if(td) ASN_DEBUG("Encoding \"%s\" using ALIGNED PER", td->name);

    return aper_encode(td, constraints, sptr, encode_to_buffer_cb, &key);
}

ssize_t
aper_encode_to_new_buffer(const asn_TYPE_descriptor_t *td,
                          const asn_per_constraints_t *constraints,
                          const void *sptr, void **buffer_r) {
    asn_enc_rval_t er = {0,0,0};
	enc_dyn_arg key;

	memset(&key, 0, sizeof(key));

	er = aper_encode(td, constraints, sptr, encode_dyn_cb, &key);
	switch(er.encoded) {
	case -1:
		FREEMEM(key.buffer);
		return -1;
	case 0:
		FREEMEM(key.buffer);
		key.buffer = MALLOC(1);
		if(key.buffer) {
			*(char *)key.buffer = '\0';
			*buffer_r = key.buffer;
			return 1;
		} else {
			return -1;
		}
	default:
		*buffer_r = key.buffer;
		ASN_DEBUG("Complete encoded in %ld bits", (long)er.encoded);
		return ((er.encoded + 7) >> 3);
	}
}

static int
_aper_encode_flush_outp(asn_per_outp_t *po) {
	uint8_t *buf;

	if(po->nboff == 0 && po->buffer == po->tmpspace)
		return 0;

	buf = po->buffer + (po->nboff >> 3);
	/* Make sure we account for the last, partially filled */
	if(po->nboff & 0x07) {
		buf[0] &= 0xff << (8 - (po->nboff & 0x07));
		buf++;
	}

	if (po->output) {
		return po->output(po->tmpspace, buf - po->tmpspace, po->op_key);
	}
	return 0;
}

asn_enc_rval_t
aper_encode(const asn_TYPE_descriptor_t *td,
        const asn_per_constraints_t *constraints,
        const void *sptr, asn_app_consume_bytes_f *cb, void *app_key) {
	asn_per_outp_t po;
	asn_enc_rval_t er = {0,0,0};

	/*
	 * Invoke type-specific encoder.
	 */
	if(!td || !td->op->aper_encoder)
		ASN__ENCODE_FAILED;	 /* PER is not compiled in */

	po.buffer = po.tmpspace;
	po.nboff = 0;
	po.nbits = 8 * sizeof(po.tmpspace);
	po.output = cb;
	po.op_key = app_key;
	po.flushed_bytes = 0;

	er = td->op->aper_encoder(td, constraints, sptr, &po);
	if(er.encoded != -1) {
		size_t bits_to_flush;

		bits_to_flush = ((po.buffer - po.tmpspace) << 3) + po.nboff;

		/* Set number of bits encoded to a firm value */
		er.encoded = (po.flushed_bytes << 3) + bits_to_flush;

		if(_aper_encode_flush_outp(&po))
			ASN__ENCODE_FAILED;
	}

	return er;
}