//converts bin32/16 to 4/3/2 byte packed bcd and revers 4/3/2 byte packed bcd to bin32/16
//based on double babble algorithm https://en.wikipedia.org/wiki/Double_dabble

#define BIN2BCD_ENROLLED
//#define BIN2BCD_DIGITS_6 //default 8 digits
#define BIN2BCD_DIGITS_4

#ifdef BIN2BCD_DIGITS_6
 #define BIN2BCD_DIGITS 6
 #define BIN2BCD_TYPE uint32_t
 #define BIN2BCD_BITS	32
#elif defined BIN2BCD_DIGITS_4
 #define BIN2BCD_DIGITS 4
 #define BIN2BCD_TYPE uint16_t
 #define BIN2BCD_BITS	16
#else
 #define BIN2BCD_DIGITS 8
 #define BIN2BCD_TYPE	uint32_t
 #define BIN2BCD_BITS	32
#endif

//converts bin32/16 to 4/3/2 byte packed bcd 
BIN2BCD_TYPE bin2bcd(BIN2BCD_TYPE x) { //double dabble algorithm
	#ifdef BIN2BCD_DIGITS_4
	union {uint16_t w42; struct{uint8_t l8,h8;};} bcd={.w42=0}, bin={.w42=x};
	#else
	union {uint32_t w42; struct{uint8_t l8,ml8,mh8,h8;};} bcd={.w42=0}, bin={.w42=x};
	#endif

	//to speed up search/count for first 1-bit position to skip leading 0-bits
	uint8_t n;
	for (uint8_t i=BIN2BCD_BITS; i; i--) {	
	    if (bin.h8 & 0x80) {n=i; break;}
	    bin.w42 <<= 1;
	}
	
	for (uint8_t i=n; i; i--) {	
		//digit value >4 needs 3 shifts therefore skip first 2 shifts as well as leading 0-bits
		if (i < n-2) {
			//check all digits for > 4 and add 3
#			ifndef BIN2BCD_ENROLLED
         uint8_t *pbcd = &bcd.w42;
			for (uint8_t i=BIN2BCD_DIGITS/2; i; pbcd++,i--) {
				if ((*pbcd & 0xF0) > 0x40) *pbcd += 0x30;
				if ((*pbcd & 0x0F) > 0x04) *pbcd += 0x03;
			}
#			endif
	
#			ifdef BIN2BCD_ENROLLED
			#if BIN2BCD_DIGITS > 6
			if ((bcd.h8 & 0xF0) > 0x40) bcd.h8 += 0x30;
			if ((bcd.h8 & 0x0F) > 0x04) bcd.h8 += 0x03;
			#endif
			#if BIN2BCD_DIGITS > 4
			if ((bcd.mh8 & 0xF0) > 0x40) bcd.mh8 += 0x30;
			if ((bcd.mh8 & 0x0F) > 0x04) bcd.mh8 += 0x03;
			#endif
			#if BIN2BCD_DIGITS < 6
			if ((bcd.h8 & 0xF0) > 0x40) bcd.h8 += 0x30;
			if ((bcd.h8 & 0x0F) > 0x04) bcd.h8 += 0x03;
			#else
			if ((bcd.ml8 & 0xF0) > 0x40) bcd.ml8 += 0x30;
			if ((bcd.ml8 & 0x0F) > 0x04) bcd.ml8 += 0x03;
			#endif			
			if ((bcd.l8 & 0xF0) > 0x40) bcd.l8 += 0x30;
			if ((bcd.l8 & 0x0F) > 0x04) bcd.l8 += 0x03;
#			endif
		}		
		bcd.w42 <<= 1; 
		if (bin.h8 & 0x80) bcd.l8 |= 0x01;		
		bin.w42 <<= 1;
	}
	return bcd.w42;
}

//converts 4/3/2 byte packed bcd to bin32/16
BIN2BCD_TYPE bcd2bin(BIN2BCD_TYPE x) { //double dabble algorithm
	#ifdef BIN2BCD_DIGITS_4
	union {uint16_t w42; struct{uint8_t l8,h8;};} bcd={.w42=x}, bin={.w42=0};
	#else
	union {uint32_t w42; struct{uint8_t l8,ml8,mh8,h8;};} bcd={.w42=x}, bin={.w42=0};
	#endif

	//to speed up search&count for leading 0-digits to skip correction of related digits 
	uint8_t n;	
	for (uint8_t i=0; i<BIN2BCD_DIGITS; i++) {
		n = i*4;
		if (bcd.w42 & (0xF0>>n)) break;
	}
	
	for (uint8_t i=BIN2BCD_BITS; i; i--) {
		bin.w42 >>= 1;
		if (bcd.l8 & 0x01) bin.h8 |= 0x80;
		bcd.w42 >>= 1;
		
		//0-digits don't need correction therefore skip leading 0-digits 
		if (i > n) {
			//check all digits for value > 7 and sub 3
#			ifndef BIN2BCD_ENROLLED
         uint8_t *pbcd = &bcd.w42;
			for (uint8_t i=BIN2BCD_DIGITS/2; i; pbcd++,i--) {
				if ((*pbcd & 0xF0) > 0x70) *pbcd -= 0x30;
				if ((*pbcd & 0x0F) > 0x07) *pbcd -= 0x03;
			}
#			endif

#			ifdef BIN2BCD_ENROLLED
			#if BIN2BCD_DIGITS > 6	
			if ((bcd.h8 & 0xF0) > 0x70) bcd.h8 -= 0x30;
			if ((bcd.h8 & 0x0F) > 0x07) bcd.h8 -= 0x03;
			#endif
			#if BIN2BCD_DIGITS > 4
			if ((bcd.mh8 & 0xF0) > 0x70) bcd.mh8 -= 0x30;
			if ((bcd.mh8 & 0x0F) > 0x07) bcd.mh8 -= 0x03;
			#endif
			#ifdef BIN2BCD_DIGITS < 6
			if ((bcd.h8 & 0xF0) > 0x70) bcd.h8 -= 0x30;
			if ((bcd.h8 & 0x0F) > 0x07) bcd.h8 -= 0x03;
			#else
			if ((bcd.ml8 & 0xF0) > 0x70) bcd.ml8 -= 0x30;
			if ((bcd.ml8 & 0x0F) > 0x07) bcd.ml8 -= 0x03;
			#endif	
			if ((bcd.l8 & 0xF0) > 0x70) bcd.l8 -= 0x30;
			if ((bcd.l8 & 0x0F) > 0x07) bcd.l8 -= 0x03;
#			endif
		}
	}
	return bin.w42;
}