/*
 $License:
    Copyright (C) 2011 InvenSense Corporation, All Rights Reserved.
 $
 */
#include <cfxbios.h>
#include <cfxpico.h>

#include "mlmath.h"
#include "mlMathFunc.h"
#include "mlinclude.h"

/** 
 * Performs one iteration of the filter, generating a new y(0) 
 *         1     / N                /  N             \\
 * y(0) = ---- * |SUM b(k) * x(k) - | SUM a(k) * y(k)|| for N = length
 *        a(0)   \k=0               \ k=1            //
 * 
 * The filters A and B should be (sizeof(long) * state->length).
 * The state variables x and y should be (sizeof(long) * (state->length - 1))
 *
 * The state variables x and y should be initialized prior to the first call
 * 
 * @param state Contains the length of the filter, filter coefficients and
 *              filter state variables x and y.
 * @param x New input into the filter.
 */
#ifndef UMPL_ELIMINATE_64BIT
/* UMPL_ELIMINATE_64BIT Notes:
 * An alternate implementation using float instead of long long accumulators
 * is provided for q29_mult and q30_mult.
 * When long long accumulators are used and an alternate implementation is not
 * available, we eliminate the entire function and header with a macro.
 */

void inv_filter_long(struct filter_long *state, long x)
{
    const long *b = state->b;
    const long *a = state->a;
    long length = state->length;
    long long tmp;
    int ii;

    /* filter */
    tmp = (long long)x *(b[0]);
    for (ii = 0; ii < length - 1; ii++) {
        tmp += ((long long)state->x[ii] * (long long)(b[ii + 1]));
    }
    for (ii = 0; ii < length - 1; ii++) {
        tmp -= ((long long)state->y[ii] * (long long)(a[ii + 1]));
    }
    tmp /= (long long)a[0];

    /* Delay */
    for (ii = length - 2; ii > 0; ii--) {
        state->y[ii] = state->y[ii - 1];
        state->x[ii] = state->x[ii - 1];
    }
    /* New values */
    state->y[0] = (long)tmp;
    state->x[0] = x;
}
#endif // UMPL_ELIMINATE_64BIT


#ifdef UMPL_ELIMINATE_64BIT

/* Rewritten in 68K assy, 7/25/2012, rah, mbari
 * Register usage:
 * D0    Return value
 * D1.0  Sign
 * D2/D3 Scratch
 * After saving D1-D3 on stack,
 *    a is at 16(sp)
 *    b is at 20(sp)
*/
asm long inv_q29_mult(long a, long b):__d0
{
    movem.l	d1-d3, -(sp)
    clr.l	d0
    clr.l	d1

    /* We're going to do unsigned mulu's, so get sign of operands and make them positive */
    tst.l	20(sp)
    beq		q29done
    bpl		b29plus
    neg.l       20(sp)
    moveq	#-1, d1
b29plus:
    tst.l	16(sp)
    beq		q29done
    bpl		a29plus
    neg.l	16(sp)
    bchg	#0, d1
a29plus:
    /* Calc (AL * BL) >> 29 */
    move.w	16(sp), d0
    mulu.w	20(sp), d0
    moveq	#29, d2
    lsr.l	d2, d0

    /* Calc ((AH * BL) + (AL * BH)) >> 13 */
    move.w	16(sp), d2
    mulu.w	22(sp), d2
    move.w	18(sp), d3
    mulu.w	20(sp), d3
    add.l	d3, d2
    lsr.l	#7, d2
    lsr.l	#6, d2
    add.l	d2, d0

    /* Calc (AH * BH) << 3 */
    move.w	16(sp), d2
    mulu.w	20(sp), d2
    lsl.l	#3, d2
    add.l	d2, d0

    /* Set sign of result */
    btst	#0, d1
    beq	        q29done
    neg.l	d0
q29done:
    movem.l	(sp)+, d1-d3
    rts
}

/* Rewritten in 68K assy, 7/25/2012, rah, mbari
 * Register usage:
 * D0    Return value
 * D1.0  Sign
 * D2/D3 Scratch
 * After saving D1-D3 on stack,
 *    a is at 16(sp)
 *    b is at 20(sp)
*/
asm long inv_q30_mult(long a, long b):__d0
{
//    move.w	#4,0xfffffe1a		//    PinSet(TPU1)
    movem.l	d1-d3, -(sp)
    clr.l	d0
    clr.l	d1

    /* We're going to do unsigned mulu's, so get sign of operands and make them positive */
    tst.l	20(sp)
    beq		q30done
    bpl		bplus
    neg.l       20(sp)
    moveq	#-1, d1
bplus:
    tst.l	16(sp)
    beq		q30done
    bpl		aplus
    neg.l	16(sp)
    bchg	#0, d1
aplus:
    /* Calc (AL * BL) >> 30 */
    move.w	16(sp), d0
    mulu.w	20(sp), d0
    moveq	#30, d2
    lsr.l	d2, d0

    /* Calc ((AH * BL) + (AL * BH)) >> 14 */
    move.w	16(sp), d2
    mulu.w	22(sp), d2
    move.w	18(sp), d3
    mulu.w	20(sp), d3
    add.l	d3, d2
    lsr.l	#7, d2
    lsr.l	#7, d2
    add.l	d2, d0

    /* Calc (AH * BH) << 2 */
    move.w	16(sp), d2
    mulu.w	20(sp), d2
    lsl.l	#2, d2
    add.l	d2, d0

    /* Set sign of result */
    btst	#0, d1
    beq	        q30done
    neg.l	d0
q30done:
    movem.l	(sp)+, d1-d3
//    move.w	#8,0xfffffe1a		//    PinClear(TPU1)
    rts
}

#else

/** 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)
{
    long long temp;
    long result;
    temp = (long long)a *b;
    result = (long)(temp >> 29);
    return result;
}

/** 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)
    long long temp;
    long result;
    temp = (long long)a *b;
    result = (long)(temp >> 30);
    return result;
}
#endif

long inv_q30_div(long a, long b)
{
#ifdef UMPL_ELIMINATE_64BIT
    long result;
    result = (long)( (double)(a / b) * (double)(1L<<30) );
    return result;
#else
    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 
 * @param[in] b
 * @return ((long long)a*b)<<shift
*/
#ifdef UMPL_ELIMINATE_64BIT
long inv_q_shift_mult(long a, long b, int shift)
{
    long result;
    result = (long)( (float)a * b / (float)(1L<<shift) );
    return result;
}
#else
long inv_q_shift_mult(long a, long b, int shift)
{
    long result;
    result = (long)( ((long long)a * b) >> shift );
    return result;
}
#endif

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]);
}

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_q_normalize(long *q)
{
    INVENSENSE_FUNC_START;
    double normSF = 0;
    int i;
    for (i = 0; i < 4; i++) {
        normSF += ((double)q[i]) / 1073741824L * ((double)q[i]) / 1073741824L;
    }
    if (normSF > 0) {
        normSF = 1 / sqrt(normSF);
        for (i = 0; i < 4; i++) {
            q[i] = (int)((double)q[i] * normSF);
        }
    } else {
        q[0] = 1073741824L;
        q[1] = 0;
        q[2] = 0;
        q[3] = 0;
    }
}

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];
}

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(float *q1, 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] 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 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 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 + 10 * k + l) = *(a + 10 * 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 + 10 * k + l) = *(a + 10 * i + j);
        }
    }
    *n = *n - 1;
}

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

    return (sum);
}

double inv_matrix_detd(double *p, int *n)
{
    double d[10][10], sum = 0;
    int i, j, m;
    m = *n;
    if (*n == 2)
        return (*p ** (p + 11) - *(p + 1) ** (p + 10));
    for (i = 0, j = 0; j < m; j++) {
        *n = m;
        inv_matrix_det_incd(p, &d[0][0], n, i, j);
        sum =
            sum + *(p + 10 * 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;
}