/*
 *			GPAC - MPEG-4 Systems C Development Kit
 *
 *			Copyright (c) Jean Le Feuvre 2000-2005
 *					All rights reserved
 *
 *  This file is part of GPAC / Scene Rendering sub-project
 *
 *  GPAC 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 2, or (at your option)
 *  any later version.
 *   
 *  GPAC 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 GNU Make; see the file COPYING.  If not, write to
 *  the Free Software Foundation, 675 Mass Ave, Cambridge, MA 02139, USA. 
 *
 *
 *	fixed-point trigo routines taken from freetype
 *		Copyright 1996-2001, 2002, 2004 by                                   
 *		David Turner, Robert Wilhelm, and Werner Lemberg.
 *	License: FTL or GPL
 *
 */

#include <gpac/m4_math.h>

#ifdef M4_FIXED_POINT

Fixed m4_divfix(Fixed a, Fixed b)
{
	s32 s;
	u32 q;

    s = 1;
    if ( a < 0 ) { a = -a; s = -1; }
    if ( b < 0 ) { b = -b; s = -s; }

    if ( b == 0 )
		/* check for division by 0 */
		q = 0x7FFFFFFFL;
    else
		/* compute result directly */
		q = (u32)( ( ( (s64)a << 16 ) + ( b >> 1 ) ) / b );
    return ( s < 0 ? -(s32)q : (s32)q );
}

Fixed m4_mulfix(Fixed a, Fixed b)
{
	Fixed  s;
	u32 ua, ub;
	if ( !a || (b == 0x10000L)) return a;
	if (!b) return 0;

	s  = a; a = ABS(a);
	s ^= b; b = ABS(b);

	ua = (u32)a;
	ub = (u32)b;

	if ((ua <= 2048) && (ub <= 1048576L)) {
		ua = ( ua * ub + 0x8000L ) >> 16;
	} else {
		u32 al = ua & 0xFFFFL;
		ua = ( ua >> 16 ) * ub +  al * ( ub >> 16 ) + ( ( al * ( ub & 0xFFFFL ) + 0x8000L ) >> 16 );
	}
	if (ua & 0x80000000) s=-s;
	return ( s < 0 ? -(Fixed)(s32)ua : (Fixed)ua );
}

Fixed m4_muldiv(Fixed a, Fixed b, Fixed c)
{
    s32 s, d;
	if (!b || !a) return 0;
    s = 1;
    if ( a < 0 ) { a = -a; s = -1; }
    if ( b < 0 ) { b = -b; s = -s; }
    if ( c < 0 ) { c = -c; s = -s; }

    d = (s32)( c > 0 ? ( (s64)a * b + ( c >> 1 ) ) / c : 0x7FFFFFFFL);
    return (Fixed) (( s > 0 ) ? d : -d);
}

Fixed m4_sqrt(Fixed x)
{
	u32 root, rem_hi, rem_lo, test_div;
	s32 count;
	root = 0;
	if ( x > 0 ) {
		rem_hi = 0;
		rem_lo = x;
		count  = 24;
		do {
			rem_hi   = ( rem_hi << 2 ) | ( rem_lo >> 30 );
			rem_lo <<= 2;
			root   <<= 1;
			test_div = ( root << 1 ) + 1;

			if ( rem_hi >= test_div ) {
				rem_hi -= test_div;
				root   += 1;
			}
		} while ( --count );
	}

	return (Fixed) root;
}

Fixed m4_ceil(Fixed a)
{
	return (a >= 0) ? (a + 0xFFFFL) & ~0xFFFFL : -((-a + 0xFFFFL) & ~0xFFFFL);
}

Fixed m4_floor(Fixed a)
{
    return (a >= 0) ? (a & ~0xFFFFL) : -((-a) & ~0xFFFFL);
}




/* the following is 0.2715717684432231 * 2^30 */
#define M4_TRIG_COSCALE  0x11616E8EUL

/* this table was generated for degrees */
#define M4_TRIG_MAX_ITERS  23

static const Fixed gp_trig_arctan_table[24] =
{
	4157273L, 2949120L, 1740967L, 919879L, 466945L, 234379L, 117304L,
	58666L, 29335L, 14668L, 7334L, 3667L, 1833L, 917L, 458L, 229L, 115L,
	57L, 29L, 14L, 7L, 4L, 2L, 1L
};

/* the Cordic shrink factor, multiplied by 2^32 */
#define M4_TRIG_SCALE    1166391785UL  /* 0x4585BA38UL */

#define M4_ANGLE_PI		(180<<16)
#define M4_ANGLE_PI2	(90<<16)
#define M4_ANGLE_2PI	(360<<16)

#define M4_PAD_FLOOR( x, n )  ( (x) & ~((n)-1) )
#define M4_PAD_ROUND( x, n )  M4_PAD_FLOOR( (x) + ((n)/2), n )
#define M4_PAD_CEIL( x, n )   M4_PAD_FLOOR( (x) + ((n)-1), n )

/* multiply a given value by the CORDIC shrink factor */
static Fixed gp_trig_downscale(Fixed  val)
{
	Fixed  s;
	s64 v;
	s   = val;
	val = ( val >= 0 ) ? val : -val;
	v   = ( val * (s64)M4_TRIG_SCALE ) + 0x100000000UL;
	val = (Fixed)( v >> 32 );
	return ( s >= 0 ) ? val : -val;
}

static s32 gp_trig_prenorm(M4Point2D *vec)
{
	Fixed  x, y, z;
	s32 shift;
	x = vec->x;
	y = vec->y;
	z     = ( ( x >= 0 ) ? x : - x ) | ( (y >= 0) ? y : -y );
	shift = 0;
	if ( z < ( 1L << 27 ) ) {
      do {
		  shift++;
		  z <<= 1;
      }
	  while ( z < ( 1L << 27 ) );

      vec->x = x << shift;
      vec->y = y << shift;
    }    
	else if ( z > ( 1L << 28 ) ) {
		do {
			shift++;
			z >>= 1;
		} while ( z > ( 1L << 28 ) );

		vec->x = x >> shift;
		vec->y = y >> shift;
		shift  = -shift;
    }
    return shift;
}

#define ANGLE_RAD_TO_DEG(_th) ((s32) ( (((s64)_th)*5729582)/100000))
#define ANGLE_DEG_TO_RAD(_th) ((s32) ( (((s64)_th)*100000)/5729582))

static void gp_trig_pseudo_rotate(M4Point2D*  vec, Fixed theta)
{
    s32 i;
    Fixed x, y, xtemp;
    const Fixed  *arctanptr;

	/*trig funcs are in degrees*/
	theta = ANGLE_RAD_TO_DEG(theta);

    x = vec->x;
    y = vec->y;

    /* Get angle between -90 and 90 degrees */
    while ( theta <= -M4_ANGLE_PI2 ) {
		x = -x;
		y = -y;
		theta += M4_ANGLE_PI;
    }

    while ( theta > M4_ANGLE_PI2 ) {
		x = -x;
		y = -y;
		theta -= M4_ANGLE_PI;
    }

    /* Initial pseudorotation, with left shift */
    arctanptr = gp_trig_arctan_table;
    if ( theta < 0 ) {
		xtemp  = x + ( y << 1 );
		y      = y - ( x << 1 );
		x      = xtemp;
		theta += *arctanptr++;
    } else {
		xtemp  = x - ( y << 1 );
		y      = y + ( x << 1 );
		x      = xtemp;
		theta -= *arctanptr++;
    }
    /* Subsequent pseudorotations, with right shifts */
    i = 0;
    do {
		if ( theta < 0 ) {
			xtemp  = x + ( y >> i );
			y      = y - ( x >> i );
			x      = xtemp;
			theta += *arctanptr++;
		} else {
			xtemp  = x - ( y >> i );
			y      = y + ( x >> i );
			x      = xtemp;
			theta -= *arctanptr++;
		}
    } while ( ++i < M4_TRIG_MAX_ITERS );

    vec->x = x;
    vec->y = y;
}


static void gp_trig_pseudo_polarize(M4Point2D *vec)
{
	Fixed         theta;
	Fixed         yi, i;
	Fixed         x, y;
	const Fixed  *arctanptr;

	x = vec->x;
	y = vec->y;

	/* Get the vector into the right half plane */
	theta = 0;
	if ( x < 0 ) {
		x = -x;
		y = -y;
		theta = 2 * M4_ANGLE_PI2;
	}

	if ( y > 0 )
		theta = - theta;

	arctanptr = gp_trig_arctan_table;

	if ( y < 0 ) {
		/* Rotate positive */
		yi     = y + ( x << 1 );
		x      = x - ( y << 1 );
		y      = yi;
		theta -= *arctanptr++;  /* Subtract angle */
	} else {
		/* Rotate negative */
		yi     = y - ( x << 1 );
		x      = x + ( y << 1 );
		y      = yi;
		theta += *arctanptr++;  /* Add angle */
	}

	i = 0;
	do {
		if ( y < 0 ) {
			/* Rotate positive */
			yi     = y + ( x >> i );
			x      = x - ( y >> i );
			y      = yi;
			theta -= *arctanptr++;
		} else {
			/* Rotate negative */
			yi     = y - ( x >> i );
			x      = x + ( y >> i );
			y      = yi;
			theta += *arctanptr++;
		}
	}
	while ( ++i < M4_TRIG_MAX_ITERS );

	/* round theta */
	if ( theta >= 0 ) 
		theta = M4_PAD_ROUND( theta, 32 );
	else
		theta = - M4_PAD_ROUND( -theta, 32 );

	vec->x = x;
	vec->y = ANGLE_DEG_TO_RAD(theta);
}

Fixed m4_cos(Fixed angle)
{
	M4Point2D v;
	v.x = M4_TRIG_COSCALE >> 2;
	v.y = 0;
	gp_trig_pseudo_rotate( &v, angle );
	return v.x / ( 1 << 12 );
}

Fixed m4_sin(Fixed angle)
{
	return m4_cos( M4_PI2 - angle );
}


Fixed m4_tan(Fixed angle)
{
	M4Point2D v;
    v.x = M4_TRIG_COSCALE >> 2;
    v.y = 0;
    gp_trig_pseudo_rotate( &v, angle );
    return m4_divfix( v.y, v.x );
}

Fixed m4_atan2(Fixed  dx, Fixed  dy)
{
	M4Point2D v;
	if ( dx == 0 && dy == 0 ) return 0;
	v.x = dx;
	v.y = dy;
	gp_trig_prenorm( &v );
	gp_trig_pseudo_polarize( &v );
	return v.y;
}

/*FIXME*/
Fixed m4_acos(Fixed angle)
{
	return FLT2FIX( (Float) acos(FIX2FLT(angle)));
}

/*FIXME*/
Fixed m4_asin(Fixed angle)
{
	return FLT2FIX( (Float) asin(FIX2FLT(angle)));
}


/* these macros return 0 for positive numbers, and -1 for negative ones */
#define GP_SIGN_LONG( x )   ( (x) >> ( 32 - 1 ) )
#define GP_SIGN_INT( x )    ( (x) >> ( 32 - 1 ) )
#define GP_SIGN_INT32( x )  ( (x) >> 31 )
#define GP_SIGN_INT16( x )  ( (x) >> 15 )

static void m4_v2d_rotate(M4Point2D *vec, Fixed angle)
{
    s32 shift;
    M4Point2D v;
    
	v.x   = vec->x;
    v.y   = vec->y;
	
    if ( angle && ( v.x != 0 || v.y != 0 ) ) {
		shift = gp_trig_prenorm( &v );
		gp_trig_pseudo_rotate( &v, angle );
		v.x = gp_trig_downscale( v.x );
		v.y = gp_trig_downscale( v.y );

		if ( shift > 0 ) {
			s32 half = 1L << ( shift - 1 );

			vec->x = ( v.x + half + GP_SIGN_LONG( v.x ) ) >> shift;
			vec->y = ( v.y + half + GP_SIGN_LONG( v.y ) ) >> shift;
		} else {
			shift  = -shift;
			vec->x = v.x << shift;
			vec->y = v.y << shift;
		}
    }
}

Fixed m4_v2d_len(M4Point2D *vec)
{
    s32 shift;
	M4Point2D v;
    v = *vec;

    /* handle trivial cases */
    if ( v.x == 0 ) {
		return ( v.y >= 0 ) ? v.y : -v.y;
    } else if ( v.y == 0 ) {
		return ( v.x >= 0 ) ? v.x : -v.x;
    }

    /* general case */
    shift = gp_trig_prenorm( &v );
    gp_trig_pseudo_polarize( &v );
    v.x = gp_trig_downscale( v.x );
    if ( shift > 0 )
		return ( v.x + ( 1 << ( shift - 1 ) ) ) >> shift;

    return v.x << -shift;
}


void m4_v2d_polarize(M4Point2D *vec, Fixed *length, Fixed *angle)
{
    s32 shift;
	M4Point2D v;
    v = *vec;
    if ( v.x == 0 && v.y == 0 )
		return;

    shift = gp_trig_prenorm( &v );
    gp_trig_pseudo_polarize( &v );
    v.x = gp_trig_downscale( v.x );
    *length = ( shift >= 0 ) ? ( v.x >> shift ) : ( v.x << -shift );
    *angle  = v.y;
}

void m4_v2d_from_polar(M4Point2D *vec, Fixed length, Fixed angle)
{
    vec->x = length;
    vec->y = 0;
    m4_v2d_rotate(vec, angle);
}

#else


void m4_v2d_from_polar(M4Point2D *vec, Fixed length, Fixed angle)
{
    vec->x = length*(Float) cos(angle);
    vec->y = length*(Float) sin(angle);
}

Fixed m4_v2d_len(M4Point2D *vec)
{
	if (!vec->x) return ABS(vec->y);
	if (!vec->y) return ABS(vec->x);
	return (Fixed) sqrt(vec->x*vec->x + vec->y*vec->y);
}

/*

Fixed m4_cos(_a) { return (Float) cos(_a); }
Fixed m4_sin(_a) { return (Float) sin(_a); }
Fixed m4_tan(_a) { return (Float) tan(_a); }
Fixed m4_atan2(_x, _y) { return (Float) atan2(_y, _x); }
Fixed m4_sqrt(_x) { return (Float) sqrt(_x); }
Fixed m4_ceil(_a) { return (Float) ceil(_a); }
Fixed m4_floor(_a) { return (Float) floor(_a); }
Fixed m4_acos(_a) { return (Float) acos(_a); }
Fixed m4_asin(_a) { return (Float) asin(_a); }

*/

#endif


Fixed m4_angle_diff(Fixed angle1, Fixed angle2)
{
    Fixed delta = angle2 - angle1;
#ifdef M4_FIXED_POINT
    delta %= M4_2PI;
	if (delta < 0) delta += M4_2PI;
    if (delta > M4_PI) delta -= M4_2PI;
#else
	while (delta < 0) delta += M4_2PI;
    while (delta > M4_PI) delta -= M4_2PI;
#endif
    return delta;
}
