/*
 * Copyright (C) 2008 Tom Phelan
 *
 * This file is part of dccp-tp.
 *
 * Dccp-tp is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 2.1 of the License, or
 * (at your option) any later version.
 *
 * Dccp-tp 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 Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with dccp-tp.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Documentation and source code for dccp-tp is available at
 * http://www.phelan-4.com/dccp-tp/.
 */

/*
 * fixedPointMath.c -- This file implements fixed-point math operations.  The
 * 'fixedp' type has nine decimal digits and most of ten integer digits.  The
 * maximum number that can be represented is 9,223,372,036.854775807 and the
 * minimum is -9,223,372,036.854775807.
 *
 * Fixed-point variables are of type fixedp.  fixedp variables can be initialized
 * from integer components.  For example:
 *
 *     int64_t i = 20;       Integer portion of fixedp
 *     uint64_t f = 3000000; Fraction portion times 1 billion
 *     fixedp x;
 *
 *     x = fptpCreate(i, f)  x = 20.003
 *
 * Note that the macro FPTP_CONST is better from initializing from constants; see
 * below.  Also see the notes below for fptpCreate for some gotchas.
 *
 * Addition, subtraction and comparison use the standard operators.  For example:
 *
 *     fixedp x, y, z;
 *
 *     z = x + y;          Adds x to y and assigns result to z
 *     z = x - y;          Substracts y from x and assigns results to z
 *     if (x > y) ...      Executes block if x is greater than y
 *     if (z == x) ...     Executes block if z equals x
 *
 * Multiplication and division require special subroutines.  For example:
 *
 *     z = fptpMult(x, y)  Assigns x*y to z
 *     z = fptpDiv(x, y)   Assigns x/y to z
 *
 * Square root operations are also supported.  For example:
 *
 *     z = fptpSqrt(x)     Assigns positive square root of x to z
 *
 * fptp2str(x) converts the value in x to a string representation.  fptpInteger
 * and fptpFraction return the integer and fraction portions of a fixedp.
 *
 * Several useful macros are defined in fixedPointMath.h:
 *
 *   FPTP_CONST(i, f)      Returns fixedp value i.f
 *   FPTP_MAX              Maximum fixedp value.  Can use -FPTP_MAX.
 *   FPTP_MAXINT           Maximum integer portion of fixedp
 *   FPTP_ZERO             0.0 as a fixedp
 *   FPTP_ONE              1.0 as a fixedp
 *   FPTP_TWO              2.0 as a fixedp
 */

#include "fixedPointMath.h"

/*
 * fptpCreate -- create a fixedp value from integer and fraction components.
 *
 * Inputs:  i -- the integer portion
 *          f -- the fraction, expressed as 1 billion times the fraction
 *
 * Returns: i.f
 *
 * e.g.  To create 11.2, call fptpCreate(11, 200000000) (although use the macro
 * FPTP_CONST -- FPTP_CONST(11, 200000000) -- for constants).  To create 11.02,
 * use fptpCreate(11, 20000000) (notice one less zero).  DON"T USE
 * fptpCreate(11, 020000000); a leading zero means octal, not decimal.
 */
fixedp fptpCreate(int64_t i, uint64_t f) {
    fixedp x;
    int sign = 1;

    if (i < 0) {
	sign = -1;
	i = -i;
    }
    x = (((uint64_t)i)*FPTP_RES) + (uint64_t)f;
    return(sign*x);
}

fixedp fptpCreateInt(uint64_t i) {
    return((fixedp)(i*FPTP_RES));
}

/*
 * fptpInteger -- returns integer portion of a fixedp
 */
int64_t fptpInteger(fixedp x) {
    return((int64_t)(x/FPTP_RES));
}

/*
 * fptpFraction -- returns fraction portion of a fixedp
 */
uint64_t fptpFraction(fixedp x) {
    return((uint64_t)(x % FPTP_RES));
}

/*
 * fptp2str -- returns string representation of a fixedp
 *
 * Returned string is statically allocated and overwritten with each call.
 */
char *fptp2str(fixedp x) {
    int64_t i;
    uint64_t f;
    int a, b, c;
    static char buf[32], bufi[10];

    /* Set sign */
    if (x < 0) {
	buf[0] = '-';
	c = 1;
	x = -x;
    } else {
	c = 0;
    }
    i = fptpInteger(x);
    f = fptpFraction(x);
    /* Convert integer - result is backwards */
    a = 0;
    do {
	bufi[a++] = (i % 10) + '0';
	i /= 10;
    } while (i != 0);
    /* Reverse integer digits */
    for (--a; a >= 0; --a, ++c) {
	buf[c] = bufi[a];
    }
    /* Do fractional part - comes out right order */
    buf[c++] = '.';
    b = 0;
    do {
	f *= 10;
	buf[c++] = (f/FPTP_RES) + '0';
	f = f % FPTP_RES;
	if (++b >= 9) break;
    } while (f != 0);
    buf[c] = 0;
    return(buf);
}

/*
 * fptpMult -- multiplies two fixedp numbers.
 *
 * Returns result.  If the multiplication overflows, returns FPTP_MAX.
 */
fixedp fptpMult(fixedp a, fixedp b) {
    uint64_t ia, ib;
    uint64_t fa, fb;
    fixedp r1, r2, r3, r4;
    int sign = 1;

    if (a < 0) {
	sign = -sign;
	a = -a;
    }
    if (b < 0) {
	sign = -sign;
	b = -b;
    }
    ia = fptpInteger(a);
    fa = fptpFraction(a);
    ib = fptpInteger(b);
    fb = fptpFraction(b);
    r1 = ia*ib;
    if ((uint64_t)r1 > (uint64_t)FPTP_MAXINT) {
	/* Overflowed */
	return(sign*FPTP_MAX);
    }
    r1 *= FPTP_RES;
    r2 = ia*fb;
    r3 = fa*ib;
    r4 = (fa*fb)/FPTP_RES;
    return(sign*(r1+r2+r3+r4));
}

/*
 * timesRes -- utility used by division routines to create the
 * 96-bit dividend.
 */
static void timesFPTP_RES(fixedp a, uint32_t *r) {
    uint32_t ahi, alo;
    uint64_t rh, rl;

    ahi = a >> 32;
    alo = a & 0xffffffff;
    rl = FPTP_RES*((uint64_t)alo);
    rh = FPTP_RES*((uint64_t)ahi);
    r[0] = rl & 0xffffffff;
    rh += (rl >> 32);
    r[1] = rh & 0xffffffff;
    r[2] = rh >> 32;
}

/*
 * div32 -- utility that divides a 96-bit dividend by a
 * 32-bit divisor.
 */
static fixedp div32(fixedp a, uint32_t b) {
    uint32_t a32[3], q[3];
    uint64_t r;

    /* Shift dividend up by resolution */
    timesFPTP_RES(a, a32);
    /* Divide top 32 bits by divisor */
    q[2] = a32[2]/b;
    /* Add remainder to middle 32 bits */
    r = (((uint64_t)(a32[2] % b)) << 32) + a32[1];
    /* Divide that to get middle 32 bits of quotient */
    q[1] = r/b;
    /* Add remainder to low 32 bits */
    r = ((r % b) << 32) + a32[0];
    /* And divide to get low 32 bits of quotient */
    q[0] = r/b;
    /* If overflow return max */
    if (q[2]) return(FPTP_MAX);
    /* Otherwise return low 64 bits of quotient */
    else return((((fixedp)q[1]) << 32) + q[0]);
}

/*
 * divShiftSub -- utility that divides a 96-bit dividend by a
 * 64-bit divisor that uses the shift-and-subtract algorithm.
 */
static fixedp divShiftSub(fixedp a, fixedp b) {
    uint32_t a32[3];
    uint32_t qhi, rhi;
    uint64_t qlo, rlo;
    int i;

    /* Shift 'a' up by resolution */
    timesFPTP_RES(a, a32);
    /* Quotient = Dividend */
    qhi = a32[2];
    qlo = a32[0] + (((uint64_t)a32[1]) << 32);
    /* Remainder = 0 */
    rhi = 0;
    rlo = 0;
    for (i = 0; i < 96; ++i) {
	/* Remainder:Quotient <<= 1 */
	rhi <<= 1;
	if (rlo & 0x8000000000000000) rhi |= 1;
	rlo <<= 1;
	if (qhi & 0x80000000) rlo |= 1;
	qhi <<= 1;
	if (qlo & 0x8000000000000000) qhi |= 1;
	qlo <<= 1;
	/* If remainder is >= Divisor */
	if ((rhi) || (rlo >= b)) {
	    /* Remainder -= Divisor */
	    if (rlo >= b) {
		rlo -= b;
	    } else {
		rlo -= b;
		--rhi;
	    }
	    /* ++Quotient */
	    if (qlo == 0xffffffffffffffff) {
		qlo = 0;
		++qhi;
	    } else {
		++qlo;
	    }
	}
    }
    /* Can't overflow because only used when b >= 1 */
    return(qlo);
}

/*
 * fptpDiv -- divide two fixedp values
 *
 * Returns a/b.  If b is zero, returns FPTP_MAX.
 */
fixedp fptpDiv(fixedp a, fixedp b) {
    int sign = 1;
    uint64_t ib, fb;

    if (a < 0) {
	sign = -sign;
	a = -a;
    }
    if (b < 0) {
	sign = -sign;
	b = -b;
    }
    if (b == FPTP_ZERO) return(FPTP_MAX);
    ib = fptpInteger(b);
    fb = fptpFraction(b);
    if (fb == 0) return(sign*(a/ib));
    if (a < (FPTP_MAX/FPTP_RES)) return(sign*((a*FPTP_RES)/b));
    if (b < FPTP_4G) return(sign*div32(a, b));
    return(sign*divShiftSub(a, b));
}

/*
 * fptpSqrt -- returns positive square root of fixedp value.
 *
 * Return value not accurate to all decimal digits.  For example,
 * 100.0 returns 9.999999963.  Negative input is converted to
 * positive.
 *
 * Uses geometric approximation algorithm.
 */
fixedp fptpSqrt(fixedp x) {
    fixedp sq, btm, top, err, maxError;

    if (x < 0) x = -x;
    /* Handle some special cases */
    if (x == FPTP_ZERO) return(FPTP_ZERO);
    if (x == FPTP_ONE) return(FPTP_ONE);
    if (x == FPTP_CONST(0, 1)) return(FPTP_CONST(0, 31627));
    if ((x < FPTP_ONE) && (x > FPTP_CONST(0, 999999995))) return(FPTP_CONST(0, 999999999));
    maxError = fptpMult(x, FPTP_SQRTERR);
    if ((FPTP_MAX - maxError) <= x) x = FPTP_MAX - maxError - FPTP_ONE;
    /* Set up boundaries */
    if (x >= FPTP_ONE) {
	btm = FPTP_ZERO;
	top = x;
	sq = fptpDiv(x, FPTP_TWO);
    } else {
	btm = x;
	top = FPTP_CONST(0, 999999999);
	sq = fptpMult(x, FPTP_TWO);
    }
    /* Search for answer */
    while (((err = x - fptpMult(sq, sq)) > maxError) || (err < -maxError)) {
	if (err > 0) {
	    /* sq is too low, go up */
	    btm = sq;
	    sq += fptpDiv(top - sq, FPTP_TWO);
	} else {
	    /* sq is too high, go down */
	    top = sq;
	    sq -= fptpDiv(sq - btm, FPTP_TWO);
	}
    }
    return(sq);
}

/*
 * fptpUsecs -- convert a fixedp value (representing seconds) to a 64-bit
 * unsigned integer in microseconds.
 */
uint64_t fptpUsecs(fixedp s) {
    return(s/1000);
}

/*
 * Convert an integer in microseconds to fixedp
 */
fixedp fptpConvertUsec(uint64_t usec) {
    return(fptpCreate(usec/1000000, (usec % 1000000)*1000));
}