/*-
 * Copyright (c) 2008 Ivan Voras <ivoras@freebsd.org>
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHORS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHORS OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 * $Id$
 */


#ifndef _FIXPT_H_
#define _FIXPT_H_

/* Fixed point header library */

#ifndef FIXPT_Q_BITS
/* Number of bits to the right of the fixed decimal point */
#define FIXPT_Q_BITS 16
#endif

#define FIXPT_Q_HALF (1 << (FIXPT_Q_BITS - 1))

#ifndef FIXPT_Q_TYPE

/* Data type that will hold our fixed point numbers, and its appropriate
 * unsigned version, if applicable.*/
#define FIXPT_Q_TYPE int64_t
#define FIXPT_Q_UTYPE uint64_t

/* How many bits are used for the sign (0 or 1) */
#define FIXPT_Q_SIGN_BITS 1

/* This should be the next larger type from FIXPT_Q_TYPE; in this case
 * the fixed point dot can be put in the middle of the data type
 * (e.g. Q32.32); if there isn't a larger data type, the decimal number
 * precision (right of the decimal point) needs to be capped at half
 * the optimal precision (e.g. 16). */
#define FIXPT_Q_EXTYPE int64_t

#define FIXPT_MAX_STRLEN (1+9+1+5)

#define FIXPT_Q_HI(x) ((FIXPT_Q_UTYPE)x >> FIXPT_Q_BITS)
#define FIXPT_Q_LO(x) (x & 0xffff)

#endif


/** Addition */
static inline FIXPT_Q_TYPE 
fixpt_add(const FIXPT_Q_TYPE a, const FIXPT_Q_TYPE b)
{
	return (a + b);
}


/** Subtraction */
static inline FIXPT_Q_TYPE 
fixpt_sub(const FIXPT_Q_TYPE a, const FIXPT_Q_TYPE b)
{
	return (a - b);
}


/** Multiplication, with good rounding */
static inline FIXPT_Q_TYPE 
fixpt_mul(const FIXPT_Q_TYPE a, const FIXPT_Q_TYPE b)
{
	return (((FIXPT_Q_EXTYPE)a * b + FIXPT_Q_HALF) >> FIXPT_Q_BITS);
}


/** Division, with good rounding */
static inline FIXPT_Q_TYPE 
fixpt_div(const FIXPT_Q_TYPE a, const FIXPT_Q_TYPE b)
{
	FIXPT_Q_EXTYPE roundedATimes2Q = ((FIXPT_Q_EXTYPE)a << FIXPT_Q_BITS) + b/2;
	return (FIXPT_Q_TYPE)(roundedATimes2Q / b);
}


/** Faster division, without rounding */
static inline FIXPT_Q_TYPE 
fixpt_fastdiv(const FIXPT_Q_TYPE a, const FIXPT_Q_TYPE b)
{
	return (((FIXPT_Q_EXTYPE)a << FIXPT_Q_BITS) / b);
}


/** Comparison */
static inline FIXPT_Q_TYPE 
fixpt_cmp(const FIXPT_Q_TYPE a, const FIXPT_Q_TYPE b)
{
	/* Could do it as return (a-b) except for overflows */
	if (a > b)
		return (1);
	if (a < b)
		return (-1);
	return (0);
}


/** Returns the integer part (floor) of a number */
static inline int 
fixpt_to_int(const FIXPT_Q_TYPE a)
{
	return (a >> FIXPT_Q_BITS);
}


/** Creates a fixed point number from the two parts, explicitely given */
static inline FIXPT_Q_TYPE 
make_fixpt(int i, unsigned int d)
{
	return ((FIXPT_Q_TYPE)i << FIXPT_Q_BITS) | d;
}


/** Creates a fixed point number from an existing integer */
static inline FIXPT_Q_TYPE 
int_to_fixpt(int a)
{
	return (a << FIXPT_Q_BITS);
}


/** Square root of a fixed point number, a few (6) rounds of N-R. */
static inline FIXPT_Q_TYPE 
fixpt_sqrt(const FIXPT_Q_TYPE a)
{
	FIXPT_Q_TYPE s;
	int i;

	s = (a + (1 << FIXPT_Q_BITS)) >> 1;
	for (i = 0; i < 8; i++)
		s = (s + fixpt_div(a, s)) >> 1;
	return (s);
}


/** Integer power of a number */
static inline FIXPT_Q_TYPE 
fixpt_intpow(const FIXPT_Q_TYPE a, unsigned int p)
{
	/* TODO: use a smarter algorithm. */
	FIXPT_Q_TYPE r = a;
	int i;

	for (i = 1; i < p; i++)
		r = fixpt_mul(r, a);

	return (r);
}


#ifndef FIXPT_NO_FP_OPS
/** Convert a double to fixpt number */
static inline FIXPT_Q_TYPE
double_to_fixpt(const double a)
{
	return (a * (1 << FIXPT_Q_BITS));
}

/** Convert a fixpt number to double */
static inline double 
fixpt_to_double(const FIXPT_Q_TYPE a)
{
	return (a / (1 << FIXPT_Q_BITS));
}
#endif


#ifndef FIXPT_NO_STR_OPS
/** Creates a human readable string representation of a number */
static void 
fixpt_2str(char *buf, const int buf_len, FIXPT_Q_TYPE val, const int decimals)
{
	char temp[FIXPT_MAX_STRLEN+1];
	char *ipt = temp;
	FIXPT_Q_TYPE whole, frac;
	
	memset(ipt, '\0', FIXPT_MAX_STRLEN+1);

	if (val < 0) {
		*ipt++ = '-';
		val = -val;
	}

	whole = FIXPT_Q_HI(val);
	frac = FIXPT_Q_LO(val);

	ipt += sprintf(ipt, "%u", (unsigned int)whole);
	if (frac != 0) {
		int idx = 0;
		*ipt++ = '.';
		while (idx < FIXPT_Q_BITS / 3 && frac != 0 && idx < decimals) {
			int q;
			frac *= 10;
			q = frac >> FIXPT_Q_BITS;
			*ipt++ = q + '0';
			frac -= q << FIXPT_Q_BITS;
			idx++;
		}
	}

	strncpy(buf, temp, buf_len);
}
#endif


#endif