#include <stdio.h>
#include <stdlib.h>
#include "raichu_fpu.h"

unsigned int fpu_atof(char *str){ /* not implemented */
	float f;
	f = (float)atof(str);
	return *((unsigned int *)(&f));
}

void print_float(unsigned int f){ /* not implemented */
	printf("%f", *((float *)(&f)));
}

unsigned int priority(unsigned int in1){
	unsigned int ret = -1;
	unsigned int in1p;
	
	if(!in1)
		return 0;
	
	in1p = in1 >> 16;
	if(in1p){
		ret += 16;
		in1 = in1p;
	}
	
	in1p = in1 >> 8;
	if(in1p){
		ret += 8;
		in1 = in1p;
	}
	
	in1p = in1 >> 4;
	if(in1p){
		ret += 4;
		in1 = in1p;
	}
	
	in1p = in1 >> 2;
	if(in1p){
		ret += 2;
		in1 = in1p;
	}
	
	if(in1 >> 1)
		return ret + 2;
	
	return ret + in1;
}

unsigned int fpu_faddbs(unsigned int abs1, unsigned int abs2, unsigned int s){
	unsigned int abs1_exp, abs2_exp;
	unsigned int expbak, mantbak;
	unsigned int ret_exp_b, expmant_b, mant_;
	unsigned int carry;
	
	abs1_exp = (abs1 >> 23);
	abs2_exp = (abs2 >> 23);
	
	if(abs1_exp < abs2_exp){
		unsigned int mantb, mants;
		
		expbak = abs2_exp;
		
		mantb = (0x01000000U | ((abs2 & 0x007fffffU) << 1));
		if((abs2_exp - abs1_exp) >= 32)
			mants = 0x00000000U;
		else
			mants = ((0x01000000U | ((abs1 & 0x007fffffU) << 1)) >> (abs2_exp - abs1_exp));
		
		mantbak = mantb + mants;
	}
	else{
		unsigned int mantb, mants;
		
		expbak = abs1_exp;
		
		mantb = (0x01000000U | ((abs1 & 0x007fffffU) << 1));
		if((abs1_exp - abs2_exp) >= 32)
			mants = 0x00000000U;
		else
			mants = ((0x01000000U | ((abs2 & 0x007fffffU) << 1)) >> (abs1_exp - abs2_exp));
		
		mantbak = mantb + mants;
	}
	
	carry = (mantbak >> 25);
	mant_ = (mantbak >> carry);
	
	ret_exp_b = (expbak + carry) & 0x000000ffU;
	
	expmant_b = ((ret_exp_b << 24) | (mant_ & 0x00ffffffU));
	
	return (s | (((expmant_b >> 1) + (expmant_b & 1)) & 0x7fffffffU));
}

unsigned int fpu_fsubbs(unsigned int abs1, unsigned int abs2, unsigned int s){
	unsigned int abs1_exp, abs2_exp;
	unsigned int abs1_mant, abs2_mant;
	unsigned int msbpos;
	unsigned int mantbak, ret_exp_b, ret_mant_b, expmant_b, expmant;
	
	abs1_exp = (abs1 >> 23);
	abs2_exp = (abs2 >> 23);
	abs1_mant = (0x04000000U | ((abs1 & 0x007fffffU) << 3));
	
	if((abs1_exp - abs2_exp) >= 32)
		abs2_mant = 0x00000000U;
	else
		abs2_mant = (0x04000000U | ((abs2 & 0x007fffffU) << 3)) >> (abs1_exp - abs2_exp);
	
	mantbak = (abs1_mant - abs2_mant);
	
	msbpos = priority(mantbak);
	if(msbpos <= 24)
		ret_mant_b = (mantbak << (24 - msbpos));
	else
		ret_mant_b = (mantbak >> (msbpos - 24));
	
	ret_exp_b = ((abs1_exp - (26 - msbpos)) & 0x000000ffU);
	
	expmant_b = (ret_exp_b << 24) | (ret_mant_b & 0x00ffffffU);
	expmant = ((expmant_b >> 1) + (expmant_b & 1)) & 0x7fffffffU;
	
	if(!(expmant & 0x7f800000U))
		return 0x00000000U;
	
	return (s | expmant);
}

unsigned int fpu_fadd(unsigned int in1, unsigned int in2){
	unsigned int abs1, abs2;
	
	if(!(in2 & 0x7f800000U)){
		if(in1 & 0x7f800000U)
			return in1;
		else
			return 0x00000000U;
	}
	
	if(!(in1 & 0x7f800000U)) /* when FADD (none when FSUB) */
		return in2;
	
	abs1 = in1 & 0x7fffffffU;
	abs2 = in2 & 0x7fffffffU;
	
	if((in1 ^ in2) & 0x80000000U){/* when FADD (reverse when FSUB) */
		if(abs1 == abs2)
			return 0x00000000U;
		
		/*if(!(in1 & 0x7f800000U)) when FSUB (none when FADD)
			return (in2 ^ 0x80000000U);*/
		
		if(abs1 < abs2)
			return fpu_fsubbs(abs2, abs1, ((in1 ^ 0x80000000U) & 0x80000000U));
		else
			return fpu_fsubbs(abs1, abs2, (in1 & 0x80000000U));
	}
	else{
		/*if(!(in1 & 0x7f800000U)) when FSUB(none when FADD)
			return (in2 ^ 0x80000000U);*/
		
		return fpu_faddbs(abs1, abs2, (in1 & 0x80000000U));
	}
}

unsigned int fpu_fsub(unsigned int in1, unsigned int in2){
	unsigned int abs1, abs2;
	
	if(!(in2 & 0x7f800000U)){
		if(in1 & 0x7f800000U)
			return in1;
		else
			return 0x00000000U;
	}
	
	/*if(!(in1 & 0x7f800000U))  when FADD (none when FSUB)
		return in2;*/
	
	abs1 = in1 & 0x7fffffffU;
	abs2 = in2 & 0x7fffffffU;
	
	if(!((in1 ^ in2) & 0x80000000U)){/* when FSUB (reverse when FADD) */
		if(abs1 == abs2)
			return 0x00000000U;
		
		if(!(in1 & 0x7f800000U))/* when FSUB (none when FADD) */
			return (in2 ^ 0x80000000U);
		
		if(abs1 < abs2)
			return fpu_fsubbs(abs2, abs1, ((in1 ^ 0x80000000U) & 0x80000000U));
		else
			return fpu_fsubbs(abs1, abs2, (in1 & 0x80000000U));
	}
	else{
		if(!(in1 & 0x7f800000U)) /* when FSUB(none when FADD)*/
			return (in2 ^ 0x80000000U);
		
		return fpu_faddbs(abs1, abs2, (in1 & 0x80000000U));
	}
}

unsigned int fpu_fmul(unsigned int in1, unsigned int in2){
	unsigned int in1_exp, in2_exp;
	unsigned int in1_mant_hi, in2_mant_hi;
	unsigned int in1_mant_lo, in2_mant_lo;
	unsigned int in1_mant_lohi, in2_mant_lohi;
	unsigned int ret_s;
	unsigned int ret_mant_hh, ret_mant_hl, ret_mant_lh;
	unsigned int ret_mant_llh;
	unsigned int ret_mantbak;
	unsigned int ret_exp_b, ret_mant_b, ret_expmant_b;
	unsigned int carry;
	
	in1_exp = (in1 & 0x7f800000U);
	in2_exp = (in2 & 0x7f800000U);
	
	if((!in1_exp) || (!in2_exp))
		return 0x00000000U;
	
	ret_s = (in1 ^ in2) & 0x80000000U;
	
	in1_mant_hi = (0x00000040U | ((in1 >> 17) & 0x0000003fU));
	in1_mant_lo = (in1 & 0x0001ffffU);
	in2_mant_hi = (0x00000040U | ((in2 >> 17) & 0x0000003fU));
	in2_mant_lo = (in2 & 0x0001ffffU);
	
	ret_mant_hh = in1_mant_hi * in2_mant_hi;
	ret_mant_hl = in1_mant_hi * in2_mant_lo;
	ret_mant_lh = in1_mant_lo * in2_mant_hi;
	
	in1_mant_lohi = (in1_mant_lo >> 1);
	in2_mant_lohi = (in2_mant_lo >> 1);
	
	ret_mant_llh = in1_mant_lohi * in2_mant_lohi + 
	               (((in1_mant_lohi & ((unsigned int)(-(int)(in2_mant_lo & 1)))) + 
	                 (((unsigned int)(-(int)(in1_mant_lo & 1))) & in2_mant_lohi)) >> 1);
	
	ret_mantbak = ((ret_mant_hh << 12) | (ret_mant_llh >> 20)) + (ret_mant_hl >> 5) + (ret_mant_lh >> 5);
	
	carry = (ret_mantbak >> 25);
	
	ret_mant_b = (ret_mantbak >> carry);
	ret_exp_b = (in1_exp + in2_exp + ((carry - 127) << 23)) & 0x7f800000U;
	
	ret_expmant_b = ((ret_exp_b << 1) | (ret_mant_b & 0x00ffffffU));
	
	return (ret_s | (((ret_expmant_b >> 1) + (ret_expmant_b & 1)) & 0x7fffffffU));
}


unsigned int mant_table[256] = {
	 52,  51,  50,  50,  49,  48,  48,  47,  46,  46,  45,  45,  44,  43,  43,  42, 
	 42,  41,  40,  40,  39,  39,  38,  38,  37,  37,  36,  35,  35,  34,  34,  33, 
	 33,  32,  32,  31,  31,  30,  30,  30,  29,  29,  28,  28,  27,  27,  26,  26, 
	 25,  25,  25,  24,  24,  23,  23,  22,  22,  22,  21,  21,  20,  20,  20,  19, 
	 19,  19,  18,  18,  17,  17,  17,  16,  16,  16,  15,  15,  15,  14,  14,  13, 
	 13,  13,  12,  12,  12,  11,  11,  11,  11,  10,  10,  10,   9,   9,   9,   8, 
	  8,   8,   7,   7,   7,   7,   6,   6,   6,   5,   5,   5,   5,   4,   4,   4, 
	  3,   3,   3,   3,   2,   2,   2,   2,   1,   1,   1,   1,   0,   0,   0,   0, 
	127, 127, 126, 125, 124, 123, 122, 121, 120, 119, 118, 117, 116, 115, 115, 114, 
	113, 112, 111, 110, 110, 109, 108, 107, 106, 106, 105, 104, 103, 103, 102, 101, 
	100, 100,  99,  98,  98,  97,  96,  96,  95,  94,  94,  93,  92,  92,  91,  90, 
	 90,  89,  89,  88,  87,  87,  86,  86,  85,  84,  84,  83,  83,  82,  82,  81, 
	 81,  80,  79,  79,  78,  78,  77,  77,  76,  76,  75,  75,  74,  74,  73,  73, 
	 72,  72,  71,  71,  70,  70,  69,  69,  69,  68,  68,  67,  67,  66,  66,  65, 
	 65,  65,  64,  64,  63,  63,  62,  62,  62,  61,  61,  60,  60,  60,  59,  59, 
	 58,  58,  58,  57,  57,  57,  56,  56,  55,  55,  55,  54,  54,  54,  53,  53};

unsigned int fpu_finvsqrt(unsigned int in1){
	unsigned int in1_exps, in1_mant;
	unsigned int ret_exp, ret_mant;
	unsigned int a, x;
	unsigned int ah, al, xh, xl;
	unsigned int axhh, axhl, axlh, axll;
	unsigned int ax;
	unsigned int axh, axl;
	unsigned int axxhh, axxhl, axxlh, axxll;
	unsigned int axx_buf, axx, threeminusaxx;
	unsigned int tmaxxh, tmaxxl;
	unsigned int nextxhh, nextxhl, nextxlh, nextxll;
	unsigned int nextx_buf, nextx;
	unsigned int miszero;
	
	in1_exps = (((in1 >> 23) - 127) & 0x000000ffU);
	in1_mant = (in1 & 0x007fffffU);
	
	if(!(in1 & 0x7f800000))
		return 0x00000000U;
	else{
		ret_exp = (~((in1_exps & 0x00000080U) | (in1_exps >> 1))) + 127;
		if(!in1_mant)
			ret_exp += ((in1 >> 23) & 1);
		ret_exp = (ret_exp & 0x000000ffU);
	}
	
	a = (0x01000000U | (in1_mant << 1));
	ah = (a >> 12);
	al = (a & 0x00000fffU);
	
	x = (0x01000000U | (mant_table[(in1 >> 16) & 0x000000ffU] << 17));
	xh = (x >> 12);
	xl = (x & 0x00000fffU);
	
	axhh = ah * xh;
	axhl = ah * xl;
	axlh = al * xh;
	axll = al * xl;
	
	ax = (axhh << 4) + (((axhl + axlh) + (axll >> 12)) >> 8);
	
	if((!in1_mant) && (in1 & 0x00800000U))
		miszero = 1;
	else
		miszero = 0;
	
	axh = (ax >> 12);
	axl = (ax & 0x00000fffU);
	
	axxhh = axh * xh;
	axxhl = axh * xl;
	axxlh = axl * xh;
	axxll = axl * xl;
	
	axx_buf = axxhh + (((axxhl + axxlh) + (axxll >> 12)) >> 12);
	
	if(!(axx_buf & 0x60000000U))
		axx = (axx_buf >> 3);
	else
		axx = (axx_buf >> 4);
	
	threeminusaxx = 0x0c000000U - axx;
	
	tmaxxh = (threeminusaxx >> 12);
	tmaxxl = (threeminusaxx & 0x00000fffU);
	
	nextxhh = tmaxxh * xh;
	nextxhl = tmaxxl * xh;
	nextxlh = tmaxxh * xl;
	nextxll = tmaxxl * xl;
	
	nextx_buf = nextxhh + (((nextxhl + nextxlh) + (nextxll >> 12)) >> 12);
	
	if(nextx_buf & 0x10000000U)
		nextx = (nextx_buf >> 4);
	else
		nextx = (nextx_buf >> 3);
	
	x = nextx;
	xh = (x >> 12);
	xl = (x & 0x00000fffU);
	
	axhh = ah * xh;
	axhl = ah * xl;
	axlh = al * xh;
	axll = al * xl;
	
	ax = (axhh << 4) + (((axhl + axlh) + (axll >> 12)) >> 8);
	
	axh = (ax >> 12);
	axl = (ax & 0x00000fffU);
	
	axxhh = axh * xh;
	axxhl = axh * xl;
	axxlh = axl * xh;
	axxll = axl * xl;
	
	axx_buf = axxhh + (((axxhl + axxlh) + (axxll >> 12)) >> 12);
	
	if(!(axx_buf & 0x60000000U))
		axx = (axx_buf >> 3);
	else
		axx = (axx_buf >> 4);
	
	threeminusaxx = 0x0c000000U - axx;
	
	tmaxxh = (threeminusaxx >> 12);
	tmaxxl = (threeminusaxx & 0x00000fffU);
	
	nextxhh = tmaxxh * xh;
	nextxhl = tmaxxl * xh;
	nextxlh = tmaxxh * xl;
	nextxll = tmaxxl * xl;
	
	nextx_buf = nextxhh + (((nextxhl + nextxlh) + (nextxll >> 12)) >> 12);
	
	if(nextx_buf & 0x10000000U)
		nextx = (nextx_buf >> 4);
	else
		nextx = (nextx_buf >> 3);
	
	if(miszero)
		return (ret_exp << 23);
	else
		ret_mant = (((nextx >> 1) + (nextx & 1)) & 0x007fffffU);
	
	if((nextx & 0x00ffffffU) == 0x00ffffffU)
		return ((((ret_exp + 1) & 0x000000ffU) << 23) | ret_mant);
	return ((ret_exp << 23) | ret_mant);
}

unsigned int fpu_itof(unsigned int in1){
	unsigned int ret_s, ret_exp_b, ret_expmant_b;
	unsigned int in1_abs, absmask;
	unsigned int msbpos;
	
	ret_s = (in1 & 0x80000000U);
	
	absmask = (unsigned int)(((int)ret_s) >> 31);
	in1_abs = (in1 ^ absmask) - absmask;
	
	msbpos = priority(in1_abs);
	
	if(!in1)
		ret_exp_b = 0;
	else
		ret_exp_b = msbpos + 127;
	
	if(msbpos <= 24)
		ret_expmant_b = ((ret_exp_b << 24) | ((in1_abs << (24 - msbpos)) & 0x00ffffffU));
	else
		ret_expmant_b = ((ret_exp_b << 24) | ((in1_abs >> (msbpos - 24)) & 0x00ffffffU));
	
	return (ret_s | (((ret_expmant_b >> 1) + (ret_expmant_b & 1)) & 0x7fffffffU));
}

unsigned int fpu_ftoi(unsigned int in1){
	unsigned int in1_exp, in1_mant;
	unsigned int ret_abs2, ret_abs;
	unsigned int nshift;
	
	in1_exp = ((in1 >> 23) & 0x000000ffU);
	
	if(in1_exp <= 125)
		return 0x00000000U;
	else if(in1_exp == 126)
		return (unsigned int)(1 - (int)(in1 >> 30));
	else if(in1_exp >= 158)
		return (0x7fffffffU + (in1 >> 31));
	
	nshift = in1_exp - 127;
	in1_mant = (0x00800000U | (in1 & 0x007fffffU));
	
	if(nshift < 22)
		ret_abs2 = (in1_mant >> (22 - nshift));
	else
		ret_abs2 = (in1_mant << (nshift - 22));
	
	ret_abs = (ret_abs2 >> 1) + (ret_abs2 & 1);
	
	if(in1 & 0x80000000U)
		return (unsigned int)(-(int)ret_abs);
	else
		return ret_abs;
}

unsigned int fpu_fless(unsigned int in1, unsigned int in2){
	unsigned int in1_abs, in2_abs;
	
	if(!(in1 & 0x7f800000U))
		in1 = 0x00000000U;
	
	if(!(in2 & 0x7f800000U))
		in2 = 0x00000000U;
	
	in1_abs = (in1 & 0x7fffffffU);
	in2_abs = (in2 & 0x7fffffffU);
	
	if(in1 & 0x80000000U){
		if((!(in2 & 0x80000000U)) || (in1_abs > in2_abs))
			return 0xffffffffU;
		return 0x00000000U;
	}
	else{
		if((!(in2 & 0x80000000U)) && (in1_abs < in2_abs))
			return 0xffffffffU;
		return 0x00000000U;
	}
}

unsigned int fpu_fneg(unsigned int op1){
	return fpu_fsub(0x00000000U, op1);
}

unsigned int fpu_fiszero(unsigned int op1){
	if(!(op1 & 0x7f800000U))
		return 0xffffffffU;
	return 0x00000000U;
}

unsigned int fpu_fispos(unsigned int op1){
	return fpu_fless(0x00000000U, op1);
}

unsigned int fpu_fisneg(unsigned int op1){
	return fpu_fless(op1, 0x00000000U);
}

unsigned int fpu_fabs(unsigned int op1){
	return (op1 & 0x7fffffffU);
}

unsigned int fpu_fhalf(unsigned int op1){
	return fpu_fmul(op1, 0x3f000000U);
}

unsigned int fpu_fsqr(unsigned int op1){
	return fpu_fmul(op1, op1);
}

unsigned int fpu_finv(unsigned int op1){
	return ((fpu_finvsqrt(fpu_fmul(op1, op1))) ^ (op1 & 0x80000000U));
}

unsigned int fpu_fsqrt(unsigned int op1){
	return fpu_finv(fpu_finvsqrt(op1));
}

unsigned int fpu_fdiv(unsigned int op1, unsigned int op2){
	return fpu_fmul(op1, fpu_finv(op2));
}