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

/* very dumb and limited BCD floating point conversions.
   they're written this way because they're only required to
   support line numbers, and I don't want to have to link with
   the math library (-lm). */

extern void die(const char *msg);

void die_range(void) {
	die("Line number out of range (>65535)");
}

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 int result = 0;

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

	switch(fp[0]) {
		case 0x40:
			result = bcd2int(fp[1]);
			if(fp[2] >= 0x50) result++;
			break;
		case 0x41:
			result = bcd2int(fp[1]) * 100 + bcd2int(fp[2]);
			if(fp[3] >= 0x50) result++;
			break;
		case 0x42:
			result = bcd2int(fp[1]) * 10000 + bcd2int(fp[2]) * 100 + bcd2int(fp[3]);
			if(fp[4] >= 0x50) result++;
			break;
		default:
			die_range(); break;
	}

	if(result > 0xffff) die_range();

	return (unsigned short)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);
	}
}
