/***************************************************************************/
/*                                                                         */
/*  fttrigon.c                                                             */
/*                                                                         */
/*    FreeType trigonometric functions (body).                             */
/*                                                                         */
/*  Copyright 2001-2005, 2012-2013 by                                      */
/*  David Turner, Robert Wilhelm, and Werner Lemberg.                      */
/*                                                                         */
/*  This file is part of the FreeType project, and may only be used,       */
/*  modified, and distributed under the terms of the FreeType project      */
/*  license, LICENSE.TXT.  By continuing to use, modify, or distribute     */
/*  this file you indicate that you have read the license and              */
/*  understand and accept it fully.                                        */
/*                                                                         */
/***************************************************************************/

#include "gd_ft_math.h"
#include <math.h>

#if defined(_MSC_VER)
#include <intrin.h>
static unsigned int __inline clz(unsigned int x)
{
    unsigned long r = 0;
    if (x != 0) {
        _BitScanReverse(&r, x);
    }
    return r;
}
#define GD_FT_MSB(x) (clz(x))
#elif defined(__GNUC__)
#define GD_FT_MSB(x) (31 - __builtin_clz(x))
#else
static unsigned int __inline clz(unsigned int x)
{
    int c = 31;
    x &= ~x + 1;
    if (n & 0x0000FFFF)
        c -= 16;
    if (n & 0x00FF00FF)
        c -= 8;
    if (n & 0x0F0F0F0F)
        c -= 4;
    if (n & 0x33333333)
        c -= 2;
    if (n & 0x55555555)
        c -= 1;
    return c;
}
#define GD_FT_MSB(x) (clz(x))
#endif

#define GD_FT_PAD_FLOOR(x, n) ((x) & ~((n) - 1))
#define GD_FT_PAD_ROUND(x, n) GD_FT_PAD_FLOOR((x) + ((n) / 2), n)
#define GD_FT_PAD_CEIL(x, n) GD_FT_PAD_FLOOR((x) + ((n) - 1), n)

#define GD_FT_BEGIN_STMNT do {
#define GD_FT_END_STMNT                                                                            \
    }                                                                                              \
    while (0)
/* transfer sign leaving a positive number */
#define GD_FT_MOVE_SIGN(x, s)                                                                      \
    GD_FT_BEGIN_STMNT                                                                              \
    if (x < 0) {                                                                                   \
        x = -x;                                                                                    \
        s = -s;                                                                                    \
    }                                                                                              \
    GD_FT_END_STMNT

GD_FT_Long GD_FT_MulFix(GD_FT_Long a, GD_FT_Long b)
{
    GD_FT_Int s = 1;
    GD_FT_Long c;

    GD_FT_MOVE_SIGN(a, s);
    GD_FT_MOVE_SIGN(b, s);

    c = (GD_FT_Long)(((GD_FT_Int64)a * b + 0x8000L) >> 16);

    return (s > 0) ? c : -c;
}

GD_FT_Long GD_FT_MulDiv(GD_FT_Long a, GD_FT_Long b, GD_FT_Long c)
{
    GD_FT_Int s = 1;
    GD_FT_Long d;

    GD_FT_MOVE_SIGN(a, s);
    GD_FT_MOVE_SIGN(b, s);
    GD_FT_MOVE_SIGN(c, s);

    d = (GD_FT_Long)(c > 0 ? ((GD_FT_Int64)a * b + (c >> 1)) / c : 0x7FFFFFFFL);

    return (s > 0) ? d : -d;
}

GD_FT_Long GD_FT_DivFix(GD_FT_Long a, GD_FT_Long b)
{
    GD_FT_Int s = 1;
    GD_FT_Long q;

    GD_FT_MOVE_SIGN(a, s);
    GD_FT_MOVE_SIGN(b, s);

    q = (GD_FT_Long)(b > 0 ? (((GD_FT_UInt64)a << 16) + (b >> 1)) / b : 0x7FFFFFFFL);

    return (s < 0 ? -q : q);
}

/*************************************************************************/
/*                                                                       */
/* This is a fixed-point CORDIC implementation of trigonometric          */
/* functions as well as transformations between Cartesian and polar      */
/* coordinates.  The angles are represented as 16.16 fixed-point values  */
/* in degrees, i.e., the angular resolution is 2^-16 degrees.  Note that */
/* only vectors longer than 2^16*180/pi (or at least 22 bits) on a       */
/* discrete Cartesian grid can have the same or better angular           */
/* resolution.  Therefore, to maintain this precision, some functions    */
/* require an interim upscaling of the vectors, whereas others operate   */
/* with 24-bit long vectors directly.                                    */
/*                                                                       */
/*************************************************************************/

/* the Cordic shrink factor 0.858785336480436 * 2^32 */
#define GD_FT_TRIG_SCALE 0xDBD95B16UL

/* the highest bit in overflow-safe vector components, */
/* MSB of 0.858785336480436 * sqrt(0.5) * 2^30         */
#define GD_FT_TRIG_SAFE_MSB 29

/* this table was generated for GD_FT_PI = 180L << 16, i.e. degrees */
#define GD_FT_TRIG_MAX_ITERS 23

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

/* multiply a given value by the CORDIC shrink factor */
static GD_FT_Fixed ft_trig_downscale(GD_FT_Fixed val)
{
    GD_FT_Fixed s;
    GD_FT_Int64 v;

    s = val;
    val = GD_FT_ABS(val);

    v = (val * (GD_FT_Int64)GD_FT_TRIG_SCALE) + 0x100000000UL;
    val = (GD_FT_Fixed)(v >> 32);

    return (s >= 0) ? val : -val;
}

/* undefined and never called for zero vector */
static GD_FT_Int ft_trig_prenorm(GD_FT_Vector *vec)
{
    GD_FT_Pos x, y;
    GD_FT_Int shift;

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

    shift = GD_FT_MSB(GD_FT_ABS(x) | GD_FT_ABS(y));

    if (shift <= GD_FT_TRIG_SAFE_MSB) {
        shift = GD_FT_TRIG_SAFE_MSB - shift;
        vec->x = (GD_FT_Pos)((GD_FT_ULong)x << shift);
        vec->y = (GD_FT_Pos)((GD_FT_ULong)y << shift);
    } else {
        shift -= GD_FT_TRIG_SAFE_MSB;
        vec->x = x >> shift;
        vec->y = y >> shift;
        shift = -shift;
    }

    return shift;
}

static void ft_trig_pseudo_rotate(GD_FT_Vector *vec, GD_FT_Angle theta)
{
    GD_FT_Int i;
    GD_FT_Fixed x, y, xtemp, b;
    const GD_FT_Fixed *arctanptr;

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

    /* Rotate inside [-PI/4,PI/4] sector */
    while (theta < -GD_FT_ANGLE_PI4) {
        xtemp = y;
        y = -x;
        x = xtemp;
        theta += GD_FT_ANGLE_PI2;
    }

    while (theta > GD_FT_ANGLE_PI4) {
        xtemp = -y;
        y = x;
        x = xtemp;
        theta -= GD_FT_ANGLE_PI2;
    }

    arctanptr = ft_trig_arctan_table;

    /* Pseudorotations, with right shifts */
    for (i = 1, b = 1; i < GD_FT_TRIG_MAX_ITERS; b <<= 1, i++) {
        GD_FT_Fixed v1 = ((y + b) >> i);
        GD_FT_Fixed v2 = ((x + b) >> i);
        if (theta < 0) {
            xtemp = x + v1;
            y = y - v2;
            x = xtemp;
            theta += *arctanptr++;
        } else {
            xtemp = x - v1;
            y = y + v2;
            x = xtemp;
            theta -= *arctanptr++;
        }
    }

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

static void ft_trig_pseudo_polarize(GD_FT_Vector *vec)
{
    GD_FT_Angle theta;
    GD_FT_Int i;
    GD_FT_Fixed x, y, xtemp, b;
    const GD_FT_Fixed *arctanptr;

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

    /* Get the vector into [-PI/4,PI/4] sector */
    if (y > x) {
        if (y > -x) {
            theta = GD_FT_ANGLE_PI2;
            xtemp = y;
            y = -x;
            x = xtemp;
        } else {
            theta = y > 0 ? GD_FT_ANGLE_PI : -GD_FT_ANGLE_PI;
            x = -x;
            y = -y;
        }
    } else {
        if (y < -x) {
            theta = -GD_FT_ANGLE_PI2;
            xtemp = -y;
            y = x;
            x = xtemp;
        } else {
            theta = 0;
        }
    }

    arctanptr = ft_trig_arctan_table;

    /* Pseudorotations, with right shifts */
    for (i = 1, b = 1; i < GD_FT_TRIG_MAX_ITERS; b <<= 1, i++) {
        GD_FT_Fixed v1 = ((y + b) >> i);
        GD_FT_Fixed v2 = ((x + b) >> i);
        if (y > 0) {
            xtemp = x + v1;
            y = y - v2;
            x = xtemp;
            theta += *arctanptr++;
        } else {
            xtemp = x - v1;
            y = y + v2;
            x = xtemp;
            theta -= *arctanptr++;
        }
    }

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

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

/* documentation is in fttrigon.h */

GD_FT_Fixed GD_FT_Cos(GD_FT_Angle angle)
{
    GD_FT_Vector v;

    v.x = GD_FT_TRIG_SCALE >> 8;
    v.y = 0;
    ft_trig_pseudo_rotate(&v, angle);

    return (v.x + 0x80L) >> 8;
}

/* documentation is in fttrigon.h */

GD_FT_Fixed GD_FT_Sin(GD_FT_Angle angle) { return GD_FT_Cos(GD_FT_ANGLE_PI2 - angle); }

/* documentation is in fttrigon.h */

GD_FT_Fixed GD_FT_Tan(GD_FT_Angle angle)
{
    GD_FT_Vector v;

    v.x = GD_FT_TRIG_SCALE >> 8;
    v.y = 0;
    ft_trig_pseudo_rotate(&v, angle);

    return GD_FT_DivFix(v.y, v.x);
}

/* documentation is in fttrigon.h */

GD_FT_Angle GD_FT_Atan2(GD_FT_Fixed dx, GD_FT_Fixed dy)
{
    GD_FT_Vector v;

    if (dx == 0 && dy == 0)
        return 0;

    v.x = dx;
    v.y = dy;
    ft_trig_prenorm(&v);
    ft_trig_pseudo_polarize(&v);

    return v.y;
}

/* documentation is in fttrigon.h */

void GD_FT_Vector_Unit(GD_FT_Vector *vec, GD_FT_Angle angle)
{
    vec->x = GD_FT_TRIG_SCALE >> 8;
    vec->y = 0;
    ft_trig_pseudo_rotate(vec, angle);
    vec->x = (vec->x + 0x80L) >> 8;
    vec->y = (vec->y + 0x80L) >> 8;
}

/* these macros return 0 for positive numbers,
   and -1 for negative ones */
#define GD_FT_SIGN_LONG(x) ((x) >> (GD_FT_SIZEOF_LONG * 8 - 1))
#define GD_FT_SIGN_INT(x) ((x) >> (GD_FT_SIZEOF_INT * 8 - 1))
#define GD_FT_SIGN_INT32(x) ((x) >> 31)
#define GD_FT_SIGN_INT16(x) ((x) >> 15)

/* documentation is in fttrigon.h */

void GD_FT_Vector_Rotate(GD_FT_Vector *vec, GD_FT_Angle angle)
{
    GD_FT_Int shift;
    GD_FT_Vector v;

    v.x = vec->x;
    v.y = vec->y;

    if (angle && (v.x != 0 || v.y != 0)) {
        shift = ft_trig_prenorm(&v);
        ft_trig_pseudo_rotate(&v, angle);
        v.x = ft_trig_downscale(v.x);
        v.y = ft_trig_downscale(v.y);

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

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

/* documentation is in fttrigon.h */

GD_FT_Fixed GD_FT_Vector_Length(GD_FT_Vector *vec)
{
    GD_FT_Int shift;
    GD_FT_Vector v;

    v = *vec;

    /* handle trivial cases */
    if (v.x == 0) {
        return GD_FT_ABS(v.y);
    } else if (v.y == 0) {
        return GD_FT_ABS(v.x);
    }

    /* general case */
    shift = ft_trig_prenorm(&v);
    ft_trig_pseudo_polarize(&v);

    v.x = ft_trig_downscale(v.x);

    if (shift > 0)
        return (v.x + (1 << (shift - 1))) >> shift;

    return (GD_FT_Fixed)((GD_FT_UInt32)v.x << -shift);
}

/* documentation is in fttrigon.h */

void GD_FT_Vector_Polarize(GD_FT_Vector *vec, GD_FT_Fixed *length, GD_FT_Angle *angle)
{
    GD_FT_Int shift;
    GD_FT_Vector v;

    v = *vec;

    if (v.x == 0 && v.y == 0)
        return;

    shift = ft_trig_prenorm(&v);
    ft_trig_pseudo_polarize(&v);

    v.x = ft_trig_downscale(v.x);

    *length = (shift >= 0) ? (v.x >> shift) : (GD_FT_Fixed)((GD_FT_UInt32)v.x << -shift);
    *angle = v.y;
}

/* documentation is in fttrigon.h */

void GD_FT_Vector_From_Polar(GD_FT_Vector *vec, GD_FT_Fixed length, GD_FT_Angle angle)
{
    vec->x = length;
    vec->y = 0;

    GD_FT_Vector_Rotate(vec, angle);
}

/* documentation is in fttrigon.h */

GD_FT_Angle GD_FT_Angle_Diff(GD_FT_Angle angle1, GD_FT_Angle angle2)
{
    GD_FT_Angle delta = angle2 - angle1;

    while (delta <= -GD_FT_ANGLE_PI)
        delta += GD_FT_ANGLE_2PI;

    while (delta > GD_FT_ANGLE_PI)
        delta -= GD_FT_ANGLE_2PI;

    return delta;
}

/* END */
