/* Fixed point math code V1.67 17/2/08
   See fixmath.h for docs
   Copyright 2008 Jeffrey Lee
   This file is part of WOUM.
   WOUM is free software: you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.
   WOUM is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with WOUM.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _FIXMATH_C
#define _FIXMATH_C

#include "fixmath.h"

/* 16.16 code */

const f1616 f1616_FromInt(int i)
{
	return (f1616) i << 16; /* ignore overflows */
}

const f1616 f1616_FromFloat(float f)
{
	if (f <= -32769)
		return (f1616) 0x80000000;
	else if (f >= 32768)
		return (f1616) 0x7FFFFFFF;
	else
		return (f1616) (f*65536);
}

const int f1616_ToInt(f1616 f)
{
	return (int) f >> 16;
}

const float f1616_ToFloat(f1616 f)
{
	return ((float) f)/65536;
}

/* 24.8 code */

const f248 f248_FromInt(int i)
{
	return (f248) i << 8; /* ignore overflows */
}

const f248 f248_FromFloat(float f)
{
	if (f <= -8388609)
		return (f248) 0x80000000;
	else if (f >= 8388608)
		return (f248) 0x7FFFFFFF;
	else
		return (f248) (f*256);
}

const int f248_ToInt(f248 f)
{
	return (int) f >> 8;
}

const float f248_ToFloat(f248 f)
{
	return ((float) f)/256;
}

/* 20.12 code */

const f2012 f2012_FromInt(int i)
{
	return (f2012) i << 12; /* ignore overflows */
}

const f2012 f2012_FromFloat(float f)
{
	if (f <= -524289)
		return (f2012) 0x80000000;
	else if (f >= 524288)
		return (f2012) 0x7FFFFFFF;
	else
		return (f2012) (f*4096);
}

const int f2012_ToInt(f2012 f)
{
	return (int) f >> 12;
}

const float f2012_ToFloat(f2012 f)
{
	return ((float) f)/4096;
}

/* 48.16 code */

const f4816 f4816_FromInt(int i)
{
	return (((f4816) i) << 16);
}

extern long long float_to_long_long(float f);

const f4816 f4816_FromFloat(float f)
{
	if (f <= -0x800000000000LL)
		return (f4816) 0x8000000000000000LL;
	else if (f >= 0x7FFFFFFFFFFFLL)
		return (f4816) 0x7FFFFFFFFFFFFFFFLL;
	else
		return (f4816) /*float_to_long_long*/ (f*65536);
}

const int f4816_ToInt(f4816 f)
{
	return (int) (f >> 16);
}

const float f4816_ToFloat(f4816 f)
{
	return ((float) f)/65536;
}

#define V(a) ((long long *) a)

void genll_movetox(const void *a,const void *b,long long dest,void *c)
{
	long long d[3]; /* delta to adjust values by */
	long long e[3]; /* answer (since c may be a or b) */
	d[0] = (V(b)[0]-V(a)[0])/2; d[1] = (V(b)[1]-V(a)[1])/2; d[2] = (V(b)[2]-V(a)[2])/2;
	e[0] = V(a)[0]+d[0]; e[1] = V(a)[1]+d[1]; e[2] = V(a)[2]+d[2];
	if (d[0] < 0)
	{ d[0] = -d[0]; d[1] = -d[1]; d[2] = -d[2]; }
	while (((d[0] = d[0] >> 1) != 0) && (e[0] != dest))
	{
		d[1] = d[1] >> 1; d[2] = d[2] >> 1;
		if (e[0] < dest)
		{ e[0] += d[0]; e[1] += d[1]; e[2] += d[2]; }
		else
		{ e[0] -= d[0]; e[1] -= d[1]; e[2] -= d[2]; }
	}
	V(c)[0] = dest; V(c)[1] = e[1]; V(c)[2] = e[2];
}

void genll_movetoy(const void *a,const void *b,long long dest,void *c)
{
	long long d[3]; /* delta to adjust values by */
	long long e[3]; /* answer (since c may be a or b) */
	d[0] = (V(b)[0]-V(a)[0])/2; d[1] = (V(b)[1]-V(a)[1])/2; d[2] = (V(b)[2]-V(a)[2])/2;
	e[0] = V(a)[0]+d[0]; e[1] = V(a)[1]+d[1]; e[2] = V(a)[2]+d[2];
	if (d[1] < 0)
	{ d[0] = -d[0]; d[1] = -d[1]; d[2] = -d[2]; }
	while (((d[1] = d[1] >> 1) != 0) && (e[1] != dest))
	{
		d[0] = d[0] >> 1; d[2] = d[2] >> 1;
		if (e[1] < dest)
		{ e[0] += d[0]; e[1] += d[1]; e[2] += d[2]; }
		else
		{ e[0] -= d[0]; e[1] -= d[1]; e[2] -= d[2]; }
	}
	V(c)[0] = e[0]; V(c)[1] = dest; V(c)[2] = e[2];
}

void genll_movetoz(const void *a,const void *b,long long dest,void *c)
{
	long long d[3]; /* delta to adjust values by */
	long long e[3]; /* answer (since c may be a or b) */
	d[0] = (V(b)[0]-V(a)[0])/2; d[1] = (V(b)[1]-V(a)[1])/2; d[2] = (V(b)[2]-V(a)[2])/2;
	e[0] = V(a)[0]+d[0]; e[1] = V(a)[1]+d[1]; e[2] = V(a)[2]+d[2];
	if (d[2] < 0)
	{ d[0] = -d[0]; d[1] = -d[1]; d[2] = -d[2]; }
	while (((d[2] = d[2] >> 1) != 0) && (e[2] != dest))
	{
		d[0] = d[0] >> 1; d[1] = d[1] >> 1;
		if (e[2] < dest)
		{ e[0] += d[0]; e[1] += d[1]; e[2] += d[2]; }
		else
		{ e[0] -= d[0]; e[1] -= d[1]; e[2] -= d[2]; }
	}
	V(c)[0] = e[0]; V(c)[1] = e[1]; V(c)[2] = dest;
}

void genll_pmovetox(const void *a,const void *b,int neg,void *c)
{
	int steps; /* Number of steps to make (since values won't become 0) */
	long long d[3]; /* Delta */
	long long e[3]; /* answer */
	long long ax,bx; /* x from a and b, since we may need to negate them */
	steps = 64;
	if (neg)
	{ ax = -V(a)[0]; bx = -V(b)[0]; }
	else
	{ ax = V(a)[0]; bx = V(b)[0]; }
	d[0] = (bx-ax)/2; d[1] = (V(b)[1]-V(a)[1])/2; d[2] = (V(b)[2]-V(a)[2])/2;
	e[0] = ax+d[0]; e[1] = V(a)[1]+d[1]; e[2] = V(a)[2]+d[2];
	if (d[2] < d[0])
	{ d[0] = -d[0]; d[1] = -d[1]; d[2] = -d[2]; }
	while ((steps--) && (e[0] != e[2]))
	{
		d[0] = d[0] >> 1; d[1] = d[1] >> 1; d[2] = d[2] >> 1;
		if (e[0] < e[2])
		{ e[0] -= d[0]; e[1] -= d[1]; e[2] -= d[2]; }
		else
		{ e[0] += d[0]; e[1] += d[1]; e[2] += d[2]; }
	}
	if (neg)
		V(c)[0] = -e[2]; /* Negate answer, and clamp value */
	else
		V(c)[0] = e[2]; /* clamp value */
	V(c)[1] = e[1]; V(c)[2] = e[2];
}

void genll_pmovetoy(const void *a,const void *b,int neg,void *c)
{
	int steps; /* Number of steps to make (since values won't become 0) */
	long long d[3]; /* Delta */
	long long e[3]; /* answer */
	long long ay,by; /* y from a and b, since we may need to negate them */
	steps = 64;
	if (neg)
	{ ay = -V(a)[1]; by = -V(b)[1]; }
	else
	{ ay = V(a)[1]; by = V(b)[1]; }
	d[0] = (V(b)[0]-V(a)[0])/2; d[1] = (by-ay)/2; d[2] = (V(b)[2]-V(a)[2])/2;
	e[0] = V(a)[0]+d[0]; e[1] = ay+d[1]; e[2] = V(a)[2]+d[2];
	if (d[2] < d[1])
	{ d[0] = -d[0]; d[1] = -d[1]; d[2] = -d[2]; }
	while ((steps--) && (e[1] != e[2]))
	{
		d[0] = d[0] >> 1; d[1] = d[1] >> 1; d[2] = d[2] >> 1;
		if (e[1] < e[2])
		{ e[0] -= d[0]; e[1] -= d[1]; e[2] -= d[2]; }
		else
		{ e[0] += d[0]; e[1] += d[1]; e[2] += d[2]; }
	}
	if (neg)
		V(c)[1] = -e[2]; /* Negate answer, and clamp value */
	else
		V(c)[1] = e[2]; /* clamp value */
	V(c)[0] = e[0]; V(c)[2] = e[2];
}

const long long genll_move2d(long long x1,long long y1,long long x2,long long y2,long long targx)
{
	long long dx,dy;
	dx = (x2-x1)/2;
	dy = (y2-y1)/2;
	x1+=dx;
	y1+=dy;
	if (dx < 0)
	{
		dx = -dx;
		dy = -dy;
	}
	while ((dx) && (x1 != targx))
	{
		dx = dx >> 1;
		dy = dy >> 1;
		if (x1 > targx)
		{
			x1 -= dx;
			y1 -= dy;
		}
		else
		{
			x1 += dx;
			y1 += dy;
		}
	}
	return y1;
}

#endif
