#include <rb2/regbus.h>
#include <fixpoint/fixpoint.h>

#include <hwo/bits.h>

void	rb2_convert		(uint8_t srctype,void *src,uint8_t dsttype, void *dst)
{
	uint8_t rawstype,
			rawdtype;

	BITS32	b32;
			
	rawstype = srctype & 0x0F;
	rawdtype = dsttype & 0x0F;

	b32.i32 = *(int32_t*)src;
	
	if (srctype & RDT_8BIT)
		b32.i32 &= 0x000000FF;
	else if (srctype & RDT_16BIT)
		b32.i32 &= 0x0000FFFF;

	if (rawstype != rawdtype) {
		if (rawstype == RDT_INT32) {			
			switch (rawdtype)
			{
				case RDT_FLOAT:
					b32.f32 = fp4816_to_float( fp4816_from_int32(b32.i32) );
					break;
				case RDT_FP4816:
					(*(fp4816_t*)dst) = fp4816_from_int32(b32.i32);
					return;
				case RDT_FP1616:
					(*(fp1616_t*)dst).value = fp4816_from_int32(b32.i32);
					return;
			};
		} else if (rawstype == RDT_FLOAT) {
			switch (rawdtype)
			{
				case RDT_INT32:
					b32.i32 = b32.f32;
					break;
				case RDT_FP4816:
					(*(fp4816_t*)dst) = fp4816_from_float(b32.f32);
					return;
				case RDT_FP1616:
					(*(fp1616_t*)dst).value = fp4816_from_float(b32.f32);
					return;
			};
		} else if (rawstype == RDT_FP4816) {
			switch (rawdtype)
			{
				case RDT_INT32:
					b32.i32 = fp4816_to_int32(*(fp4816_t*)src);
					break;
				case RDT_FLOAT:
					b32.f32 = fp4816_to_float( *(fp4816_t*)src );
					break;
			};			
		} else if (rawstype == RDT_FP1616) {
			switch (rawdtype){
				case RDT_INT32:
					b32.i32 = (*(fp1616_t*)src).value;
					break;
				case RDT_FLOAT:
					b32.f32 = fp4816_to_float( fp4816_from_1616( (*(fp1616_t*)src) ) );
					break;
			};
		};
	};
	
	if (dsttype & RDT_8BIT)
		*(int8_t*)dst = b32.i8[0];
	else if (dsttype & RDT_16BIT)
		*(int16_t*)dst = b32.i16[0];
	else
		*(int32_t*)dst = b32.i32;
};