xref: /hardware/invensense/6515/libsensors_iio/software/core/mllite/ml_math_func.c

/*
 $License:
    Copyright (C) 2011-2012 InvenSense Corporation, All Rights Reserved.
    See included License.txt for License information.
 $
 */

/*******************************************************************************
 *
 * $Id:$
 *
 ******************************************************************************/

/**
 *   @defgroup  ML_MATH_FUNC ml_math_func
 *   @brief     Motion Library - Math Functions
 *              Common math functions the Motion Library
 *
 *   @{
 *       @file ml_math_func.c
 *       @brief Math Functions.
 */

#include "mlmath.h"
#include "ml_math_func.h"
#include "mlinclude.h"
#include <string.h>

/** @internal
 * Does the cross product of compass by gravity, then converts that
 * to the world frame using the quaternion, then computes the angle that
 * is made.
 *
 * @param[in] compass Compass Vector (Body Frame), length 3
 * @param[in] grav Gravity Vector (Body Frame), length 3
 * @param[in] quat Quaternion, Length 4
 * @return Angle Cross Product makes after quaternion rotation.
 */
float inv_compass_angle(const long *compass, const long *grav, const float *quat)
{
    float cgcross[4], q1[4], q2[4], qi[4];
    float angW;

    // Compass cross Gravity
    cgcross[0] = 0.f;
    cgcross[1] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
    cgcross[2] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
    cgcross[3] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];

    // Now convert cross product into world frame
    inv_q_multf(quat, cgcross, q1);
    inv_q_invertf(quat, qi);
    inv_q_multf(q1, qi, q2);

    // Protect against atan2 of 0,0
    if ((q2[2] == 0.f) && (q2[1] == 0.f))
        return 0.f;

    // This is the unfiltered heading correction
    angW = -atan2f(q2[2], q2[1]);
    return angW;
}

/**
 *  @brief  The gyro data magnitude squared :
 *          (1 degree per second)^2 = 2^6 = 2^GYRO_MAG_SQR_SHIFT.
 * @param[in] gyro Gyro data scaled with 1 dps = 2^16
 *  @return the computed magnitude squared output of the gyroscope.
 */
unsigned long inv_get_gyro_sum_of_sqr(const long *gyro)
{
    unsigned long gmag = 0;
    long temp;
    int kk;

    for (kk = 0; kk < 3; ++kk) {
        temp = gyro[kk] >> (16 - (GYRO_MAG_SQR_SHIFT / 2));
        gmag += temp * temp;
    }

    return gmag;
}

/** Performs a multiply and shift by 29. These are good functions to write in assembly on
 * with devices with small memory where you want to get rid of the long long which some
 * assemblers don't handle well
 * @param[in] a
 * @param[in] b
 * @return ((long long)a*b)>>29
*/
long inv_q29_mult(long a, long b)
{
#ifdef UMPL_ELIMINATE_64BIT
    long result;
    result = (long)((float)a * b / (1L << 29));
    return result;
#else
    long long temp;
    long result;
    temp = (long long)a * b;
    result = (long)(temp >> 29);
    return result;
#endif
}

/** Performs a multiply and shift by 30. These are good functions to write in assembly on
 * with devices with small memory where you want to get rid of the long long which some
 * assemblers don't handle well
 * @param[in] a
 * @param[in] b
 * @return ((long long)a*b)>>30
*/
long inv_q30_mult(long a, long b)
{
#ifdef UMPL_ELIMINATE_64BIT
    long result;
    result = (long)((float)a * b / (1L << 30));
    return result;
#else
    long long temp;
    long result;
    temp = (long long)a * b;
    result = (long)(temp >> 30);
    return result;
#endif
}

#ifndef UMPL_ELIMINATE_64BIT
long inv_q30_div(long a, long b)
{
    long long temp;
    long result;
    temp = (((long long)a) << 30) / b;
    result = (long)temp;
    return result;
}
#endif

/** Performs a multiply and shift by shift. These are good functions to write
 * in assembly on with devices with small memory where you want to get rid of
 * the long long which some assemblers don't handle well
 * @param[in] a First multicand
 * @param[in] b Second multicand
 * @param[in] shift Shift amount after multiplying
 * @return ((long long)a*b)<<shift
*/
#ifndef UMPL_ELIMINATE_64BIT
long inv_q_shift_mult(long a, long b, int shift)
{
    long result;
    result = (long)(((long long)a * b) >> shift);
    return result;
}
#endif

/** Performs a fixed point quaternion multiply.
* @param[in] q1 First Quaternion Multicand, length 4. 1.0 scaled
*            to 2^30
* @param[in] q2 Second Quaternion Multicand, length 4. 1.0 scaled
*            to 2^30
* @param[out] qProd Product after quaternion multiply. Length 4.
*             1.0 scaled to 2^30.
*/
void inv_q_mult(const long *q1, const long *q2, long *qProd)
{
    INVENSENSE_FUNC_START;
    qProd[0] = inv_q30_mult(q1[0], q2[0]) - inv_q30_mult(q1[1], q2[1]) -
               inv_q30_mult(q1[2], q2[2]) - inv_q30_mult(q1[3], q2[3]);

    qProd[1] = inv_q30_mult(q1[0], q2[1]) + inv_q30_mult(q1[1], q2[0]) +
               inv_q30_mult(q1[2], q2[3]) - inv_q30_mult(q1[3], q2[2]);

    qProd[2] = inv_q30_mult(q1[0], q2[2]) - inv_q30_mult(q1[1], q2[3]) +
               inv_q30_mult(q1[2], q2[0]) + inv_q30_mult(q1[3], q2[1]);

    qProd[3] = inv_q30_mult(q1[0], q2[3]) + inv_q30_mult(q1[1], q2[2]) -
               inv_q30_mult(q1[2], q2[1]) + inv_q30_mult(q1[3], q2[0]);
}

/** Performs a fixed point quaternion addition.
* @param[in] q1 First Quaternion term, length 4. 1.0 scaled
*            to 2^30
* @param[in] q2 Second Quaternion term, length 4. 1.0 scaled
*            to 2^30
* @param[out] qSum Sum after quaternion summation. Length 4.
*             1.0 scaled to 2^30.
*/
void inv_q_add(long *q1, long *q2, long *qSum)
{
    INVENSENSE_FUNC_START;
    qSum[0] = q1[0] + q2[0];
    qSum[1] = q1[1] + q2[1];
    qSum[2] = q1[2] + q2[2];
    qSum[3] = q1[3] + q2[3];
}

void inv_vector_normalize(long *vec, int length)
{
    INVENSENSE_FUNC_START;
    double normSF = 0;
    int ii;
    for (ii = 0; ii < length; ii++) {
        normSF +=
            inv_q30_to_double(vec[ii]) * inv_q30_to_double(vec[ii]);
    }
    if (normSF > 0) {
        normSF = 1 / sqrt(normSF);
        for (ii = 0; ii < length; ii++) {
            vec[ii] = (int)((double)vec[ii] * normSF);
        }
    } else {
        vec[0] = 1073741824L;
        for (ii = 1; ii < length; ii++) {
            vec[ii] = 0;
        }
    }
}

void inv_q_normalize(long *q)
{
    INVENSENSE_FUNC_START;
    inv_vector_normalize(q, 4);
}

void inv_q_invert(const long *q, long *qInverted)
{
    INVENSENSE_FUNC_START;
    qInverted[0] = q[0];
    qInverted[1] = -q[1];
    qInverted[2] = -q[2];
    qInverted[3] = -q[3];
}

double quaternion_to_rotation_angle(const long *quat) {
    double quat0 = (double )quat[0] / 1073741824;
    if (quat0 > 1.0f) {
        quat0 = 1.0;
    } else if (quat0 < -1.0f) {
        quat0 = -1.0;
    }

    return acos(quat0)*2*180/M_PI;
}

/** Rotates a 3-element vector by Rotation defined by Q
*/
void inv_q_rotate(const long *q, const long *in, long *out)
{
    long q_temp1[4], q_temp2[4];
    long in4[4], out4[4];

    // Fixme optimize
    in4[0] = 0;
    memcpy(&in4[1], in, 3 * sizeof(long));
    inv_q_mult(q, in4, q_temp1);
    inv_q_invert(q, q_temp2);
    inv_q_mult(q_temp1, q_temp2, out4);
    memcpy(out, &out4[1], 3 * sizeof(long));
}

void inv_q_multf(const float *q1, const float *q2, float *qProd)
{
    INVENSENSE_FUNC_START;
    qProd[0] =
        (q1[0] * q2[0] - q1[1] * q2[1] - q1[2] * q2[2] - q1[3] * q2[3]);
    qProd[1] =
        (q1[0] * q2[1] + q1[1] * q2[0] + q1[2] * q2[3] - q1[3] * q2[2]);
    qProd[2] =
        (q1[0] * q2[2] - q1[1] * q2[3] + q1[2] * q2[0] + q1[3] * q2[1]);
    qProd[3] =
        (q1[0] * q2[3] + q1[1] * q2[2] - q1[2] * q2[1] + q1[3] * q2[0]);
}

void inv_q_addf(const float *q1, const float *q2, float *qSum)
{
    INVENSENSE_FUNC_START;
    qSum[0] = q1[0] + q2[0];
    qSum[1] = q1[1] + q2[1];
    qSum[2] = q1[2] + q2[2];
    qSum[3] = q1[3] + q2[3];
}

void inv_q_normalizef(float *q)
{
    INVENSENSE_FUNC_START;
    float normSF = 0;
    float xHalf = 0;
    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    if (normSF < 2) {
        xHalf = 0.5f * normSF;
        normSF = normSF * (1.5f - xHalf * normSF * normSF);
        normSF = normSF * (1.5f - xHalf * normSF * normSF);
        normSF = normSF * (1.5f - xHalf * normSF * normSF);
        normSF = normSF * (1.5f - xHalf * normSF * normSF);
        q[0] *= normSF;
        q[1] *= normSF;
        q[2] *= normSF;
        q[3] *= normSF;
    } else {
        q[0] = 1.0;
        q[1] = 0.0;
        q[2] = 0.0;
        q[3] = 0.0;
    }
    normSF = (q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
}

/** Performs a length 4 vector normalization with a square root.
* @param[in,out] q vector to normalize. Returns [1,0,0,0] is magnitude is zero.
*/
void inv_q_norm4(float *q)
{
    float mag;
    mag = sqrtf(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
    if (mag) {
        q[0] /= mag;
        q[1] /= mag;
        q[2] /= mag;
        q[3] /= mag;
    } else {
        q[0] = 1.f;
        q[1] = 0.f;
        q[2] = 0.f;
        q[3] = 0.f;
    }
}

void inv_q_invertf(const float *q, float *qInverted)
{
    INVENSENSE_FUNC_START;
    qInverted[0] = q[0];
    qInverted[1] = -q[1];
    qInverted[2] = -q[2];
    qInverted[3] = -q[3];
}

/**
 * Converts a quaternion to a rotation matrix.
 * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
 * @param[out] rot Rotation matrix in fixed point. One is 2^30. The
 *             First 3 elements of the rotation matrix, represent
 *             the first row of the matrix. Rotation matrix multiplied
 *             by a 3 element column vector transform a vector from Body
 *             to World.
 */
void inv_quaternion_to_rotation(const long *quat, long *rot)
{
    rot[0] =
        inv_q29_mult(quat[1], quat[1]) + inv_q29_mult(quat[0],
                quat[0]) -
        1073741824L;
    rot[1] =
        inv_q29_mult(quat[1], quat[2]) - inv_q29_mult(quat[3], quat[0]);
    rot[2] =
        inv_q29_mult(quat[1], quat[3]) + inv_q29_mult(quat[2], quat[0]);
    rot[3] =
        inv_q29_mult(quat[1], quat[2]) + inv_q29_mult(quat[3], quat[0]);
    rot[4] =
        inv_q29_mult(quat[2], quat[2]) + inv_q29_mult(quat[0],
                quat[0]) -
        1073741824L;
    rot[5] =
        inv_q29_mult(quat[2], quat[3]) - inv_q29_mult(quat[1], quat[0]);
    rot[6] =
        inv_q29_mult(quat[1], quat[3]) - inv_q29_mult(quat[2], quat[0]);
    rot[7] =
        inv_q29_mult(quat[2], quat[3]) + inv_q29_mult(quat[1], quat[0]);
    rot[8] =
        inv_q29_mult(quat[3], quat[3]) + inv_q29_mult(quat[0],
                quat[0]) -
        1073741824L;
}

/**
 * Converts a quaternion to a rotation vector. A rotation vector is
 * a method to represent a 4-element quaternion vector in 3-elements.
 * To get the quaternion from the 3-elements, The last 3-elements of
 * the quaternion will be the given rotation vector. The first element
 * of the quaternion will be the positive value that will be required
 * to make the magnitude of the quaternion 1.0 or 2^30 in fixed point units.
 * @param[in] quat 4-element quaternion in fixed point. One is 2^30.
 * @param[out] rot Rotation vector in fixed point. One is 2^30.
 */
void inv_quaternion_to_rotation_vector(const long *quat, long *rot)
{
    rot[0] = quat[1];
    rot[1] = quat[2];
    rot[2] = quat[3];

    if (quat[0] < 0.0) {
        rot[0] = -rot[0];
        rot[1] = -rot[1];
        rot[2] = -rot[2];
    }
}

/** Converts a 32-bit long to a big endian byte stream */
unsigned char *inv_int32_to_big8(long x, unsigned char *big8)
{
    big8[0] = (unsigned char)((x >> 24) & 0xff);
    big8[1] = (unsigned char)((x >> 16) & 0xff);
    big8[2] = (unsigned char)((x >> 8) & 0xff);
    big8[3] = (unsigned char)(x & 0xff);
    return big8;
}

/** Converts a big endian byte stream into a 32-bit long */
long inv_big8_to_int32(const unsigned char *big8)
{
    long x;
    x = ((long)big8[0] << 24) | ((long)big8[1] << 16) | ((long)big8[2] << 8)
        | ((long)big8[3]);
    return x;
}

/** Converts a big endian byte stream into a 16-bit integer (short) */
short inv_big8_to_int16(const unsigned char *big8)
{
    short x;
    x = ((short)big8[0] << 8) | ((short)big8[1]);
    return x;
}

/** Converts a little endian byte stream into a 16-bit integer (short) */
short inv_little8_to_int16(const unsigned char *little8)
{
    short x;
    x = ((short)little8[1] << 8) | ((short)little8[0]);
    return x;
}

/** Converts a 16-bit short to a big endian byte stream */
unsigned char *inv_int16_to_big8(short x, unsigned char *big8)
{
    big8[0] = (unsigned char)((x >> 8) & 0xff);
    big8[1] = (unsigned char)(x & 0xff);
    return big8;
}

void inv_matrix_det_inc(float *a, float *b, int *n, int x, int y)
{
    int k, l, i, j;
    for (i = 0, k = 0; i < *n; i++, k++) {
        for (j = 0, l = 0; j < *n; j++, l++) {
            if (i == x)
                i++;
            if (j == y)
                j++;
            *(b + 6 * k + l) = *(a + 6 * i + j);
        }
    }
    *n = *n - 1;
}

void inv_matrix_det_incd(double *a, double *b, int *n, int x, int y)
{
    int k, l, i, j;
    for (i = 0, k = 0; i < *n; i++, k++) {
        for (j = 0, l = 0; j < *n; j++, l++) {
            if (i == x)
                i++;
            if (j == y)
                j++;
            *(b + 6 * k + l) = *(a + 6 * i + j);
        }
    }
    *n = *n - 1;
}

float inv_matrix_det(float *p, int *n)
{
    float d[6][6], sum = 0;
    int i, j, m;
    m = *n;
    if (*n == 2)
        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
    for (i = 0, j = 0; j < m; j++) {
        *n = m;
        inv_matrix_det_inc(p, &d[0][0], n, i, j);
        sum =
            sum + *(p + 6 * i + j) * SIGNM(i +
                                            j) *
            inv_matrix_det(&d[0][0], n);
    }

    return (sum);
}

double inv_matrix_detd(double *p, int *n)
{
    double d[6][6], sum = 0;
    int i, j, m;
    m = *n;
    if (*n == 2)
        return (*p ** (p + 7) - *(p + 1) ** (p + 6));
    for (i = 0, j = 0; j < m; j++) {
        *n = m;
        inv_matrix_det_incd(p, &d[0][0], n, i, j);
        sum =
            sum + *(p + 6 * i + j) * SIGNM(i +
                                            j) *
            inv_matrix_detd(&d[0][0], n);
    }

    return (sum);
}

/** Wraps angle from (-M_PI,M_PI]
 * @param[in] ang Angle in radians to wrap
 * @return Wrapped angle from (-M_PI,M_PI]
 */
float inv_wrap_angle(float ang)
{
    if (ang > M_PI)
        return ang - 2 * (float)M_PI;
    else if (ang <= -(float)M_PI)
        return ang + 2 * (float)M_PI;
    else
        return ang;
}

/** Finds the minimum angle difference ang1-ang2 such that difference
 * is between [-M_PI,M_PI]
 * @param[in] ang1
 * @param[in] ang2
 * @return angle difference ang1-ang2
 */
float inv_angle_diff(float ang1, float ang2)
{
    float d;
    ang1 = inv_wrap_angle(ang1);
    ang2 = inv_wrap_angle(ang2);
    d = ang1 - ang2;
    if (d > M_PI)
        d -= 2 * (float)M_PI;
    else if (d < -(float)M_PI)
        d += 2 * (float)M_PI;
    return d;
}

/** bernstein hash, derived from public domain source */
uint32_t inv_checksum(const unsigned char *str, int len)
{
    uint32_t hash = 5381;
    int i, c;

    for (i = 0; i < len; i++) {
        c = *(str + i);
        hash = ((hash << 5) + hash) + c;    /* hash * 33 + c */
    }

    return hash;
}

static unsigned short inv_row_2_scale(const signed char *row)
{
    unsigned short b;

    if (row[0] > 0)
        b = 0;
    else if (row[0] < 0)
        b = 4;
    else if (row[1] > 0)
        b = 1;
    else if (row[1] < 0)
        b = 5;
    else if (row[2] > 0)
        b = 2;
    else if (row[2] < 0)
        b = 6;
    else
        b = 7;  // error
    return b;
}


/** Converts an orientation matrix made up of 0,+1,and -1 to a scalar representation.
* @param[in] mtx Orientation matrix to convert to a scalar.
* @return Description of orientation matrix. The lowest 2 bits (0 and 1) represent the column the one is on for the
* first row, with the bit number 2 being the sign. The next 2 bits (3 and 4) represent
* the column the one is on for the second row with bit number 5 being the sign.
* The next 2 bits (6 and 7) represent the column the one is on for the third row with
* bit number 8 being the sign. In binary the identity matrix would therefor be:
* 010_001_000 or 0x88 in hex.
*/
unsigned short inv_orientation_matrix_to_scalar(const signed char *mtx)
{

    unsigned short scalar;

    /*
       XYZ  010_001_000 Identity Matrix
       XZY  001_010_000
       YXZ  010_000_001
       YZX  000_010_001
       ZXY  001_000_010
       ZYX  000_001_010
     */

    scalar = inv_row_2_scale(mtx);
    scalar |= inv_row_2_scale(mtx + 3) << 3;
    scalar |= inv_row_2_scale(mtx + 6) << 6;

    return scalar;
}

/** Uses the scalar orientation value to convert from chip frame to body frame
* @param[in] orientation A scalar that represent how to go from chip to body frame
* @param[in] input Input vector, length 3
* @param[out] output Output vector, length 3
*/
void inv_convert_to_body(unsigned short orientation, const long *input, long *output)
{
    output[0] = input[orientation      & 0x03] * SIGNSET(orientation & 0x004);
    output[1] = input[(orientation>>3) & 0x03] * SIGNSET(orientation & 0x020);
    output[2] = input[(orientation>>6) & 0x03] * SIGNSET(orientation & 0x100);
}

/** Uses the scalar orientation value to convert from body frame to chip frame
* @param[in] orientation A scalar that represent how to go from chip to body frame
* @param[in] input Input vector, length 3
* @param[out] output Output vector, length 3
*/
void inv_convert_to_chip(unsigned short orientation, const long *input, long *output)
{
    output[orientation & 0x03]      = input[0] * SIGNSET(orientation & 0x004);
    output[(orientation>>3) & 0x03] = input[1] * SIGNSET(orientation & 0x020);
    output[(orientation>>6) & 0x03] = input[2] * SIGNSET(orientation & 0x100);
}


/** Uses the scalar orientation value to convert from chip frame to body frame and
* apply appropriate scaling.
* @param[in] orientation A scalar that represent how to go from chip to body frame
* @param[in] sensitivity Sensitivity scale
* @param[in] input Input vector, length 3
* @param[out] output Output vector, length 3
*/
void inv_convert_to_body_with_scale(unsigned short orientation,
                                    long sensitivity,
                                    const long *input, long *output)
{
    output[0] = inv_q30_mult(input[orientation & 0x03] *
                             SIGNSET(orientation & 0x004), sensitivity);
    output[1] = inv_q30_mult(input[(orientation>>3) & 0x03] *
                             SIGNSET(orientation & 0x020), sensitivity);
    output[2] = inv_q30_mult(input[(orientation>>6) & 0x03] *
                             SIGNSET(orientation & 0x100), sensitivity);
}

/** find a norm for a vector
* @param[in] a vector [3x1]
* @param[out] output the norm of the input vector
*/
double inv_vector_norm(const float *x)
{
    return sqrt(x[0]*x[0]+x[1]*x[1]+x[2]*x[2]);
}

void inv_init_biquad_filter(inv_biquad_filter_t *pFilter, float *pBiquadCoeff) {
    int i;
    // initial state to zero
    pFilter->state[0] = 0;
    pFilter->state[1] = 0;

    // set up coefficients
    for (i=0; i<5; i++) {
        pFilter->c[i] = pBiquadCoeff[i];
    }
}

void inv_calc_state_to_match_output(inv_biquad_filter_t *pFilter, float input)
{
    pFilter->input = input;
    pFilter->output = input;
    pFilter->state[0] = input / (1 + pFilter->c[2] + pFilter->c[3]);
    pFilter->state[1] = pFilter->state[0];
}

float inv_biquad_filter_process(inv_biquad_filter_t *pFilter, float input)  {
    float stateZero;

    pFilter->input = input;
    // calculate the new state;
    stateZero = pFilter->input - pFilter->c[2]*pFilter->state[0]
                               - pFilter->c[3]*pFilter->state[1];

    pFilter->output = stateZero + pFilter->c[0]*pFilter->state[0]
                                + pFilter->c[1]*pFilter->state[1];

    // update the output and state
    pFilter->output = pFilter->output * pFilter->c[4];
    pFilter->state[1] = pFilter->state[0];
    pFilter->state[0] = stateZero;
    return pFilter->output;
}

void inv_get_cross_product_vec(float *cgcross, float compass[3], float grav[3])  {

    cgcross[0] = (float)compass[1] * grav[2] - (float)compass[2] * grav[1];
    cgcross[1] = (float)compass[2] * grav[0] - (float)compass[0] * grav[2];
    cgcross[2] = (float)compass[0] * grav[1] - (float)compass[1] * grav[0];
}

void mlMatrixVectorMult(long matrix[9], const long vecIn[3], long *vecOut)  {
        // matrix format
        //  [ 0  3  6;
        //    1  4  7;
        //    2  5  8]

        // vector format:  [0  1  2]^T;
        int i, j;
        long temp;

        for (i=0; i<3; i++)	{
                temp = 0;
                for (j=0; j<3; j++)  {
                        temp += inv_q30_mult(matrix[i+j*3], vecIn[j]);
                }
                vecOut[i] = temp;
        }
}

//============== 1/sqrt(x), 1/x, sqrt(x) Functions ================================

/** Calculates 1/square-root of a fixed-point number (30 bit mantissa, positive): Q1.30
* Input must be a positive scaled (2^30) integer
* The number is scaled to lie between a range in which a Newton-Raphson
* iteration works best. Corresponding square root of the power of two is returned.
*  Caller must scale final result by 2^rempow (while avoiding overflow).
* @param[in] x0, length 1
* @param[out] rempow, length 1
* @return scaledSquareRoot on success or zero.
*/
long inv_inverse_sqrt(long x0, int*rempow)
{
	//% Inverse sqrt NR in the neighborhood of 1.3>x>=0.65
	//% x(k+1) = x(k)*(3 - x0*x(k)^2)

	//% Seed equals 1. Works best in this region.
	//xx0 = int32(1*2^30);

	long oneoversqrt2, oneandhalf, x0_2;
	unsigned long xx;
	int pow2, sq2scale, nr_iters;
	//long upscale, sqrt_upscale, upsclimit;
	//long downscale, sqrt_downscale, downsclimit;

	// Precompute some constants for efficiency
	//% int32(2^30*1/sqrt(2))
	oneoversqrt2=759250125L;
	//% int32(1.5*2^30);
	oneandhalf=1610612736L;

	//// Further scaling into optimal region saves one or more NR iterations. Maps into region (.9, 1.1)
	//// int32(0.9/log(2)*2^30)
	//upscale = 1394173804L;
	//// int32(sqrt(0.9/log(2))*2^30)
	//sqrt_upscale = 1223512453L;
	// // int32(1.1*log(2)/.9*2^30)
	//upsclimit = 909652478L;
	//// int32(1.1/log(4)*2^30)
	//downscale = 851995103L;
	//// int32(sqrt(1.1/log(4))*2^30)
	//sqrt_downscale = 956463682L;
	// // int32(0.9*log(4)/1.1*2^30)
	//downsclimit = 1217881829L;

	nr_iters = test_limits_and_scale(&x0, &pow2);

	sq2scale=pow2%2;  // Find remainder. Is it even or odd?


	// Further scaling to decrease NR iterations
	// With the mapping below, 89% of calculations will require 2 NR iterations or less.
	// TBD


	x0_2 = x0 >>1; // This scaling incorporates factor of 2 in NR iteration below.
	// Initial condition starts at 1: xx=(1L<<30);
	// First iteration is simple: Instead of initializing xx=1, assign to result of first iteration:
	// xx= (3/2-x0/2);
	//% NR formula: xx=xx*(3/2-x0*xx*xx/2); = xx*(1.5 - (x0/2)*xx*xx)
	// Initialize NR (first iteration). Note we are starting with xx=1, so the first iteration becomes an initialization.
	xx = oneandhalf - x0_2;
 	if ( nr_iters>=2 ) {
		// Second NR iteration
		xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) );
		if ( nr_iters==3 ) {
			// Third NR iteration.
			xx = inv_q30_mult( xx, ( oneandhalf - inv_q30_mult(x0_2, inv_q30_mult(xx,xx) ) ) );
			// Fourth NR iteration: Not needed due to single precision.
		}
	}
	if (sq2scale) {
		*rempow = (pow2>>1) + 1; // Account for sqrt(2) in denominator, note we multiply if s2scale is true
		return (inv_q30_mult(xx,oneoversqrt2));
	}
	else {
		*rempow = pow2>>1;
		return xx;
	}
}


/** Calculates square-root of a fixed-point number (30 bit mantissa, positive)
* Input must be a positive scaled (2^30) integer
* The number is scaled to lie between a range in which a Newton-Raphson
* iteration works best.
* @param[in] x0, length 1
* @return scaledSquareRoot on success or zero. **/
long inv_fast_sqrt(long x0)
{

	//% Square-Root with NR in the neighborhood of 1.3>x>=0.65 (log(2) <= x <= log(4) )
    // Two-variable NR iteration:
    // Initialize: a=x; c=x-1;
    // 1st Newton Step:  a=a-a*c/2; ( or: a = x - x*(x-1)/2  )
    // Iterate: c = c*c*(c-3)/4
    //          a = a - a*c/2    --> reevaluating c at this step gives error of approximation

	//% Seed equals 1. Works best in this region.
	//xx0 = int32(1*2^30);

	long sqrt2, oneoversqrt2, one_pt5;
	long xx, cc;
	int pow2, sq2scale, nr_iters;

	// Return if input is zero. Negative should really error out.
	if (x0 <= 0L) {
		return 0L;
	}

	sqrt2 =1518500250L;
	oneoversqrt2=759250125L;
	one_pt5=1610612736L;

	nr_iters = test_limits_and_scale(&x0, &pow2);

	sq2scale = 0;
	if (pow2 > 0)
		sq2scale=pow2%2;  // Find remainder. Is it even or odd?
	pow2 = pow2-sq2scale; // Now pow2 is even. Note we are adding because result is scaled with sqrt(2)

	// Sqrt 1st NR iteration
	cc = x0 - (1L<<30);
	xx = x0 - (inv_q30_mult(x0, cc)>>1);
 	if ( nr_iters>=2 ) {
		// Sqrt second NR iteration
		// cc = cc*cc*(cc-3)/4; = cc*cc*(cc/2 - 3/2)/2;
		// cc = ( cc*cc*((cc>>1) - onePt5) ) >> 1
		cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1;
		xx = xx - (inv_q30_mult(xx, cc)>>1);
		if ( nr_iters==3 ) {
			// Sqrt third NR iteration
			cc = inv_q30_mult( cc, inv_q30_mult(cc, (cc>>1) - one_pt5) ) >> 1;
			xx = xx - (inv_q30_mult(xx, cc)>>1);
		}
	}
	if (sq2scale)
		xx = inv_q30_mult(xx,oneoversqrt2);
	// Scale the number with the half of the power of 2 scaling
	if (pow2>0)
		xx = (xx >> (pow2>>1));
	else if (pow2 == -1)
		xx = inv_q30_mult(xx,sqrt2);
	return xx;
}

/** Calculates 1/x of a fixed-point number (30 bit mantissa)
* Input must be a scaled (2^30) integer (+/-)
* The number is scaled to lie between a range in which a Newton-Raphson
* iteration works best. Corresponding multiplier power of two is returned.
*  Caller must scale final result by 2^pow (while avoiding overflow).
* @param[in] x, length 1
* @param[out] pow, length 1
* @return scaledOneOverX on success or zero.
*/
long inv_one_over_x(long x0, int*pow)
{
	//% NR for 1/x in the neighborhood of log(2) => x => log(4)
	//%    y(k+1)=y(k)*(2 \ 96 x0*y(k))
    //% with y(0) = 1 as the starting value for NR

	long two, xx;
	int numberwasnegative, nr_iters, did_upscale, did_downscale;

	long upscale, downscale, upsclimit, downsclimit;

	*pow = 0;
	// Return if input is zero.
	if (x0 == 0L) {
		return 0L;
	}

	// This is really (2^31-1), i.e. 1.99999... .
	// Approximation error is 1e-9. Note 2^31 will overflow to sign bit, so it can't be used here.
	two = 2147483647L;

	// int32(0.92/log(2)*2^30)
	upscale = 1425155444L;
	// int32(1.08/upscale*2^30)
	upsclimit = 873697834L;

	// int32(1.08/log(4)*2^30)
	downscale = 836504283L;
	// int32(0.92/downscale*2^30)
	downsclimit = 1268000423L;

	// Algorithm is intended to work with positive numbers only. Change sign:
	numberwasnegative = 0;
	if (x0 < 0L) {
		numberwasnegative = 1;
		x0 = -x0;
	}

	nr_iters = test_limits_and_scale(&x0, pow);

	did_upscale=0;
	did_downscale=0;
	// Pre-scaling to reduce NR iterations and improve accuracy:
	if (x0<=upsclimit) {
		x0 = inv_q30_mult(x0, upscale);
		did_upscale = 1;
		// The scaling ALWAYS leaves the number in the 2-NR iterations region:
		nr_iters = 2;
		// Is x0 in the single NR iteration region (0.994, 1.006) ?
		if (x0 > 1067299373 && x0 < 1080184275)
			nr_iters = 1;
	} else if (x0>=downsclimit) {
		x0 = inv_q30_mult(x0, downscale);
		did_downscale = 1;
		// The scaling ALWAYS leaves the number in the 2-NR iterations region:
		nr_iters = 2;
		// Is x0 in the single NR iteration region (0.994, 1.006) ?
		if (x0 > 1067299373 && x0 < 1080184275)
			nr_iters = 1;
	}

	xx = (two - x0) + 1; // Note 2 will overflow so the computation (2-x) is done with "two" == (2^30-1)
	// First NR iteration
	xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
 	if ( nr_iters>=2 ) {
		// Second NR iteration
		xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
		if ( nr_iters==3 ) {
			// THird NR iteration.
			xx = inv_q30_mult( xx, (two - inv_q30_mult(x0, xx)) + 1 );
			// Fourth NR iteration: Not needed due to single precision.
		}
	}

	// Post-scaling
	if (did_upscale)
		xx = inv_q30_mult( xx, upscale);
	else if (did_downscale)
		xx = inv_q30_mult( xx, downscale);

	if (numberwasnegative)
		xx = -xx;
	return xx;
}

/** Auxiliary function used by inv_OneOverX(), inv_fastSquareRoot(), inv_inverseSqrt().
* Finds the range of the argument, determines the optimal number of Newton-Raphson
* iterations and . Corresponding square root of the power of two is returned.
* Restrictions: Number is represented as Q1.30.
*               Number is betweeen the range 2<x<=0
* @param[in] x, length 1
* @param[out] pow, length 1
* @return # of NR iterations, x0 scaled between log(2) and log(4) and 2^N scaling (N=pow)
*/
int test_limits_and_scale(long *x0, int *pow)
{
	long lowerlimit, upperlimit, oneiterlothr, oneiterhithr, zeroiterlothr, zeroiterhithr;

	// Lower Limit: ll = int32(log(2)*2^30);
	lowerlimit = 744261118L;
	//Upper Limit ul = int32(log(4)*2^30);
	upperlimit = 1488522236L;
	//  int32(0.9*2^30)
	oneiterlothr = 966367642L;
	// int32(1.1*2^30)
	oneiterhithr = 1181116006L;
	// int32(0.99*2^30)
	zeroiterlothr=1063004406L;
	//int32(1.01*2^30)
	zeroiterhithr=1084479242L;

	// Scale number such that Newton Raphson iteration works best:
	// Find the power of two scaling that leaves the number in the optimal range,
	// ll <= number <= ul. Note odd powers have special scaling further below
	if (*x0 > upperlimit) {
		// Halving the number will push it in the optimal range since largest value is 2
		*x0 = *x0>>1;
		*pow=-1;
	} else if (*x0 < lowerlimit) {
		// Find position of highest bit, counting from left, and scale number
		*pow=get_highest_bit_position((unsigned long*)x0);
		if (*x0 >= upperlimit) {
			// Halving the number will push it in the optimal range
			*x0 = *x0>>1;
			*pow=*pow-1;
		}
		else if (*x0 < lowerlimit) {
			// Doubling the number will push it in the optimal range
			*x0 = *x0<<1;
			*pow=*pow+1;
		}
	} else {
		*pow = 0;
	}

	if ( *x0<oneiterlothr || *x0>oneiterhithr )
		return 3; // 3 NR iterations
	if ( *x0<zeroiterlothr || *x0>zeroiterhithr )
		return 2; // 2 NR iteration

	return 1; // 1 NR iteration
}

/** Auxiliary function used by testLimitsAndScale()
* Find the highest nonzero bit in an unsigned 32 bit integer:
* @param[in] value, length 1.
* @return highes bit position.
**/int get_highest_bit_position(unsigned long *value)
{
    int position;
    position = 0;
    if (*value == 0) return 0;

    if ((*value & 0xFFFF0000) == 0) {
		position += 16;
		*value=*value<<16;
	}
    if ((*value & 0xFF000000) == 0) {
		position += 8;
		*value=*value<<8;
	}
    if ((*value & 0xF0000000) == 0) {
		position += 4;
		*value=*value<<4;
	}
    if ((*value & 0xC0000000) == 0) {
		position += 2;
		*value=*value<<2;
	}

	// If we got too far into sign bit, shift back. Note we are using an
	// unsigned long here, so right shift is going to shift all the bits.
    if ((*value & 0x80000000)) {
		position -= 1;
		*value=*value>>1;
	}
    return position;
}

/* compute real part of quaternion, element[0]
@param[in]  inQuat, 3 elements gyro quaternion
@param[out] outquat, 4 elements gyro quaternion
*/
int inv_compute_scalar_part(const long * inQuat, long* outQuat)
{
    long scalarPart = 0;

    scalarPart = inv_fast_sqrt((1L<<30) - inv_q30_mult(inQuat[0], inQuat[0])
                                        - inv_q30_mult(inQuat[1], inQuat[1])
                                        - inv_q30_mult(inQuat[2], inQuat[2]) );
                outQuat[0] = scalarPart;
                outQuat[1] = inQuat[0];
                outQuat[2] = inQuat[1];
                outQuat[3] = inQuat[2];

                return 0;
}
/**
 * @}
 */