/* record.c - printf/scanf style formatting for binary data
 * PUBLIC DOMAIN - September 18, 2008 - J.Mayo
 */
#include <assert.h>
#include <ctype.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>
#include <stdarg.h>

#ifndef NDEBUG
#include <math.h>
#endif

/*=* Byte-order functions *=*/
/* WRite Big-Endian 16-bit value */
#define WR_BE16(dest, offset, value) do { \
		unsigned _tmp=value; \
		(dest)[offset]=(_tmp/256)%256; \
		(dest)[(offset)+1]=_tmp%256; \
	} while(0)

/* WRite Big-Endian 32-bit value */
#define WR_BE32(dest, offset, value) do { \
		unsigned _tmp=value; \
		(dest)[offset]=(_tmp/16777216L)%256; \
		(dest)[(offset)+1]=(_tmp/65536L)%256; \
		(dest)[(offset)+2]=(_tmp/256)%256; \
		(dest)[(offset)+3]=_tmp%256; \
	} while(0)

/* WRite Big-Endian 64-bit value */
#define WR_BE64(dest, offset, value) do { \
		unsigned long long _tmp=value; \
		(dest)[offset]=((_tmp)>>56)&255; \
		(dest)[(offset)+1]=((_tmp)>>48)&255; \
		(dest)[(offset)+2]=((_tmp)>>40)&255; \
		(dest)[(offset)+3]=((_tmp)>>32)&255; \
		(dest)[(offset)+4]=((_tmp)>>24)&255; \
		(dest)[(offset)+5]=((_tmp)>>16)&255; \
		(dest)[(offset)+6]=((_tmp)>>8)&255; \
		(dest)[(offset)+7]=(_tmp)&255; \
	} while(0)

/* ReaD Big-Endian 16-bit value */
#define RD_BE16(src, offset) (((src)[offset]*256)|(src)[(offset)+1])

/* ReaD Big-Endian 32-bit value */
#define RD_BE32(src, offset) (((src)[offset]*16777216L)|((src)[(offset)+1]*65536L)|((src)[(offset)+2]*256)|(src)[(offset)+3])

/* ReaD Big-Endian 64-bit value */
#define RD_BE64(src, offset) ( \
	((src)[(offset)]*0x100000000000000ULL)| \
	((src)[(offset)+1]*0x1000000000000ULL)| \
	((src)[(offset)+2]*0x10000000000ULL)| \
	((src)[(offset)+3]*0x100000000ULL)| \
	((src)[(offset)+4]*0x1000000U)| \
	((src)[(offset)+5]*0x10000U)| \
	((src)[(offset)+6]*0x100U)| \
	((src)[(offset)+7]))

/******************************************************************************
 * ieee754 doubles
 ******************************************************************************/

void ieee754_encode64be(void *dest, double f) {
	const unsigned
		bits=64,
		expbits=11,
		sigbits=bits-expbits-1;
	const int bias=(1<<(expbits-1))-1;
	double norm;
	int shift;
	unsigned sign, exponent;
	unsigned long long sig;

	sign=f<0;
	norm=fabs(f);
	for(shift=0;norm>=2.;norm/=2.,shift++) ;
	for(;norm<1.;norm*=2.,shift--) ;
	norm-=1.;
	sig=norm*((1ll<<sigbits)+.5);
	exponent=shift+bias;

	sig|=((unsigned long long)sign<<(bits-1))|((unsigned long long)exponent<<sigbits);
	WR_BE64((unsigned char*)dest, 0, sig);
}

double ieee754_decode64be(void *src) {
	const unsigned
		bits=64,
		expbits=11,
		sigbits=bits-expbits-1;
	const int bias=(1<<(expbits-1))-1;
	unsigned long long i;
	int shift;
	double res;
	i=RD_BE64((unsigned char*)src, 0);
	if(i==0) return 0.;
	/* printf("hex=0x%llX i=0x%llX\n", *(long long*)src, i); */
	res=i&((1LL<<sigbits)-1); /* mask off significant */	
	/* printf("res=%f hex=0x%llX\n", res, *(long long*)&res); */
	res/=(1LL<<sigbits);
	res+=1.;
	shift=((i>>sigbits)&((1LL<<expbits)-1))-bias;
	/* printf("shift=%d res=%f hex=0x%llX\n", shift, res, *(long long*)&res); */
	while(shift>0) { res*=2.0; shift--; }
	while(shift<0) { res/=2.0; shift++; }
	if(i>>(bits-1)) {
		res=-res;
	}
	return res;
}

#ifndef NDEBUG
void ieee754_test(void) {
	char buf[8];
	double tmp, pi;

	ieee754_encode64be(buf, pi=M_PI);
	tmp=ieee754_decode64be(buf);
	printf("in=%f hex=0x%llX out=%f hex=0x%llX buf=0x%llX ok? %d\n", pi, *(long long*)&pi, tmp, *(long long*)&tmp, *(long long*)buf, tmp==pi);
}
#endif

/******************************************************************************
 * record - printf/scanf style formatting for binary data
 ******************************************************************************/

/* template for the format parser
 * variables:
 * width - 0 means unset
 * precision - -1 means unspecified
 * format:
 * u	- 32-bit unsigned integer
 * hu	- 16-bit unsigned integer
 * llu	- 64-bit unsigned
 * f	- 64-bit IEEE double
 * c    - 8-bit unsigned integer
 * s	- null terminated string
 * return:
 * -1 on failure
 *  new offset on success
 */
#define RECORD_FORMAT_PARSER(c_action, hu_action, u_action, llu_action, f_action, lf_action, s_action) do { \
		size_t offset=0; \
		va_list ap; \
		va_start(ap, fmt); \
	out: \
		while(*fmt) { \
			if(*fmt=='%') { \
				char *endptr; \
				int width=0, precision=-1; \
				int longness=0; \
				fmt++; \
				if(*fmt=='*') { \
					width=va_arg(ap, int); \
					fmt++; \
				} else if(isdigit(*fmt) || (*fmt=='-' && isdigit(fmt[1]))) { \
					width=strtol(fmt, &endptr, 10); \
					assert(endptr!=fmt); \
					fmt=endptr; \
				} \
				if(*fmt=='.') { \
					fmt++; \
					if(*fmt=='*') { \
						precision=va_arg(ap, int); \
						fmt++; \
					} else if(isdigit(fmt[1])) { \
						precision=strtoul(fmt+1, &endptr, 10); \
						assert(endptr!=fmt); \
						fmt=endptr; \
					} \
				} \
				while(*fmt) switch(*fmt) { \
					case 0: abort(); \
					case 'h': \
						fmt++; \
						longness--; \
						break; \
					case 'L': \
						fmt++; \
						longness+=2; \
						break; \
					case 'l': \
						fmt++; \
						longness++; \
						break; \
					case 'c': \
						if(offset+1>len) goto out_of_data; \
						c_action; \
						offset++; \
						fmt++; \
						goto out; \
					case 'u': \
						/* printf("width=%d precision=%d\n", width, precision); */ \
						if(longness==0) { \
							if(offset+4>len) goto out_of_data; \
							u_action; \
							offset+=4; \
						} else if(longness==2) { \
							if(offset+8>len) goto out_of_data; \
							llu_action; \
							offset+=8; \
						} else if(longness==-1) { \
							if(offset+2>len) goto out_of_data; \
							hu_action; \
							offset+=2; \
						} else { \
							fprintf(stderr, "invalid type specifier for format tag '%c'\n", *fmt); \
							goto bad_format; \
						} \
						fmt++; \
						goto out; \
					case 'a': case 'A': case 'e': case 'E': \
					case 'g': case 'G': case 'f': case 'F': /* process double and floats */ \
						switch(longness) { \
							case 0: \
								f_action; break; \
							case 1: \
								lf_action; break; \
							case 2: \
								fprintf(stderr, "long double not supported\n"); \
							default: \
								goto bad_format; \
						} \
						goto out; \
					case 's': \
						s_action; goto out; \
					default: \
						fprintf(stderr, "Unknown format code '%c'\n", *fmt); \
						goto bad_format; \
				} \
			} else { \
				/* printf("*fmt='%c'\n", *fmt); */ \
				fmt++; \
			} \
		} \
		va_end(ap); \
		return offset; /* success */ \
	out_of_data: \
		fprintf(stderr, "format exceeds data available\n"); \
		va_end(ap); \
		return -1; /* failure */ \
	bad_format: \
		fprintf(stderr, "invalid format code\n"); \
		va_end(ap); \
		return -1; /* failure */ \
	} while(0)

int record_read(const void *data, size_t len, const const char *fmt, ...) __attribute__ ((format (scanf, 3, 4)));
int record_read(const void *data, size_t len, const const char *fmt, ...) {
	RECORD_FORMAT_PARSER(
		*va_arg(ap, uint8_t*)=((char*)data)[offset],
		*va_arg(ap, uint16_t*)=RD_BE16((char*)data, offset),
		*va_arg(ap, uint32_t*)=RD_BE32((char*)data, offset),
		*va_arg(ap, uint64_t*)=RD_BE64((char*)data, offset),
		{
		fprintf(stderr, "float format unsupported\n");
		goto bad_format;
		},
		{ /*  doubles */
		if(offset+8>len) goto out_of_data;
		*va_arg(ap, double*)=ieee754_decode64be((char*)data+offset);
		offset+=8;
		},
		{ /* string - the width paramter controls the amount of consumed data */
			const char *tmp;
			char *dest;
			size_t max=len-offset;
			dest=va_arg(ap, char *);
			if(width>0 && (unsigned)width-1<max) {
				max=width-1;
			}
			tmp=memchr((char*)data+offset, 0, max);
			if(!tmp) {
				fprintf(stderr, "could not find null terminated string in buffer.\n");
				goto out_of_data;
			}
			max=tmp-((char*)data+offset);
			memcpy(dest, (char*)data+offset, max+1); /* include null terminator */
			offset+=max+1;
		}
	);
}

int record_write(const void *data, size_t len, const const char *fmt, ...) __attribute__ ((format (printf, 3, 4)));
int record_write(const void *data, size_t len, const const char *fmt, ...) {
	RECORD_FORMAT_PARSER(
		((uint8_t*)data)[offset]=va_arg(ap, unsigned),
		WR_BE16((char*)data, offset, va_arg(ap, unsigned)),
		WR_BE32((char*)data, offset, va_arg(ap, uint32_t)),
		WR_BE64((char*)data, offset, va_arg(ap, uint64_t)),
		{ /*  doubles */
		if(offset+8>len) goto out_of_data;
		ieee754_encode64be((char*)data+offset, va_arg(ap, double));
		offset+=8;
		},
		goto bad_format,
		{ /* string - precision is max number of characters, not including null terminator */
			/* TODO: adjust precision to match the way it is used in record_read */
			const char *s;
			const char *tmp;
			s=va_arg(ap, const char *);
			if(precision==-1) {
				tmp=s+strlen(s);
			} else {
				tmp=memchr(s, 0, (size_t)precision);
				if(!tmp) {
					tmp=s+precision;
				}
			}
			if(offset+(tmp-s)+1>len) goto out_of_data;
			memcpy((char*)data+offset, s, (size_t)(tmp-s));
			((char*)data)[offset+(tmp-s)]=0;
			offset+=(tmp-s)+1;
		}
	);
}

int main(void) {
	char data[31];
	uint32_t a;
	uint64_t b;
	uint16_t c, d;
	uint8_t e;
	char buf[8];
	double f;
	int res;

	res=record_write(data, sizeof data, "%u%llu%hu%hu%c%f%s", 0x1020304, 0x5060708090a0b0cll, 0xd0e, 0xf10, 42, 1.3e9, "Hello");
	if(res==-1) {
		printf("error\n");
		return 1;
	}
	printf("ofs=%d\n", res);

	res=record_read(data, sizeof data, "%u%llu%hu%hu%c%lf%8s", &a, &b, &c, &d, &e, &f, buf);
	if(res==-1) {
		printf("error\n");
		return 1;
	}
	printf("ofs=%d %#x %#llx %#x %#x %hhd %g '%s'\n", res, a, b, c, d, e, f, buf);
	return 0;
}