#include <string.h>
#include "bcdfp.h"

extern void die(const char *msg);

unsigned char bcd2int(unsigned char bcd) {
	return (bcd >> 4) * 10 + (bcd & 0x0f);
}

unsigned char int2bcd(unsigned char i) {
	return ((i / 10) << 4) | (i % 10);
}

unsigned short fp2int(const unsigned char *fp) {
	unsigned short result = 0;

	/* examine the exponent/sign byte */
	if(fp[0] == 0) return 0; /* special case */
	if(fp[0] & 0x80) die("negative numbers not supported");

	switch(fp[0]) {
		case 0x40:
			result = bcd2int(fp[1]); break;
		case 0x41:
			result = bcd2int(fp[1]) * 100 + bcd2int(fp[2]); break;
		case 0x42:
			result = bcd2int(fp[1]) * 10000 + bcd2int(fp[2]) * 100 + bcd2int(fp[3]); break;
		default:
			die("number out of range"); break;
	}

	return result;
}

void int2fp(unsigned short num, unsigned char *fp) {
	memset(fp, 0, 6);

	if(num == 0) return;

	if(num >= 10000) {
		fp[0] = 0x42;
		fp[3] = int2bcd(num % 100);
		num /= 100;
		fp[2] = int2bcd(num % 100);
		num /= 100;
		fp[1] = int2bcd(num);
	} else if(num >= 100) {
		fp[0] = 0x41;
		fp[2] = int2bcd(num % 100);
		num /= 100;
		fp[1] = int2bcd(num);
	} else {
		fp[0] = 0x40;
		fp[1] = int2bcd(num);
	}
}