package awa.imu;

/*
 * The source code of this file or parts of it were copied from 
 * the autopilot onboard code package.
 *  
 * Autopilot 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 of the License, or
 * (at your option) any later version.
 * 
 * Autopilot 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 the source code; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

public class KVec
{

    static void v_add(double[] c, double[] a, double[] b, int n)
    {
        do {
            --n;
            c[n] = a[n] + b[n];
        } while (n > 0);
    }

    static double[] v_sub(Euler a, Euler b)
    {
        double[] c = new double[4];
        Euler diff = Euler.sub(a, b);
        c[0] = diff.phi;
        c[1] = diff.theta;
        c[2] = diff.psi;
        
        return c;
    }
    
    static void v_sub(double[] c, double[] a, double[] b, int n)
    {
        do {
            --n;
            c[n] = a[n] - b[n];
        } while (n > 0);
    }


    static void v_scale(double[] c, double[] a, double s, int n)
    {
        do {
            --n;
            c[n] = a[n] * s;
        } while (n > 0);
    }


    /*
     * Adds two matrices: c = a + b
     *
     * Safe to call with c == a and c == b
     */
    static void m_add(double[][] c, double[][] a, double[][] b, int m, int n)
    {
        do {
            n--;
            v_add(c[n], a[n], b[n], m);
        } while (n > 0);
    }


    static void m_sub(double[][] c, double[][] a, double[][] b, int m, int n)
    {
        do {
            n--;
            v_sub(c[n], a[n], b[n], m);
        } while (n > 0);
    }


    static void m_scale(double[][] c, double[][] a, double s, int m, int n)
    {
        do {
            n--;
            v_scale(c[n], a[n], s, m);
        } while (n > 0);
    }


    /**
     *  vc = ma * vb
     *
     * Unsafe to call with vc == vb
     */
    static void mv_mult(double[] vc, double[][] ma, double[] vb, int m, int n)
    {
        int i;
        int j;

        for (j = 0; j < m; ++j) {
            double sum = 0.0;
            double[] row = ma[j];

            for (i = 0; i < n; ++i)
                sum += row[i] * vb[i];

            vc[j] = sum;
        }
    }


    /**
     *  vc = va * mb
     *
     * Unsafe to call with vc == va
     */
    static void vm_mult(double[] vc, double[] va, double[][] mb, int m, int n)
    {
        int i;
        int j;

        for (i = 0; i < n; ++i) {
            double sum = 0.0;

            for (j = 0; j < m; j++)
                sum += va[j] * mb[j][i];

            vc[j] = sum;
        }
    }


    /**
     *  c = a * b
     *
     * Unsafe for c == a or c == b.
     *
     * Optimal form runs in 1/2 the time of the simple form due to
     * loop constant hoisting and others.
     */
    static void m_mult(double[][] c, double[][] a, double[][] b, int m, int n, int p)
    {
        int i;
        int j;
        int k;

        m_zero(c);

        for (i = 0; i < m; ++i)
            for (j = 0; j < p; ++j)
                for (k = 0; k < n; ++k)
                    c[i][j] += a[i][k] * b[k][j];

    }


    private static void m_zero(double[][] c)
    {
        for (int i = 0; i < 4; i++)
            for (int j = 0; j < 4; j++)
                c[i][j] = 0.0;

    }


    /**
     *  transpose is safe to call with c == a
     */
    static void m_transpose(double[][] c, double[][] a, int m, int n)
    {
        int i;
        int j;

        for (i = 0; i < m; ++i) {
            for (j = i; j < n; j++) {
                double u = a[i][j];
                double l = a[j][i];

                c[j][i] = u;
                c[i][j] = l;
            }
        }
    }


    /*
     *  Produce the determinant of a 3x3 matrix
     */
    static double m33_det(double[][] m)
    {
        return m[0][0] * m[1][1] * m[2][2] - m[0][0] * m[1][2] * m[2][1] - m[0][1] * m[1][0] * m[2][2] + m[0][1] * m[1][2] * m[2][0] + m[0][2]
                * m[1][0] * m[2][1] - m[0][2] * m[1][1] * m[2][0];
    }


    /**
     *  m33_inv will invert a 3x3 matrix.
     *
     * Unsafe to call with c == a
     *
     * Details were copied from:
     *
     *  http://mathworld.wolfram.com/MatrixInverse.html
     */
    static void m33_inv(double[][] D, double[][] m)
    {
        D[0][0] = m[1][1] * m[2][2] - m[1][2] * m[2][1];
        D[0][1] = m[0][2] * m[2][1] - m[0][1] * m[2][2];
        D[0][2] = m[0][1] * m[1][2] - m[0][2] * m[1][1];

        D[1][0] = m[1][2] * m[2][0] - m[1][0] * m[2][2];
        D[1][1] = m[0][0] * m[2][2] - m[0][2] * m[2][0];
        D[1][2] = m[0][2] * m[1][0] - m[0][0] * m[1][2];

        D[2][0] = m[1][0] * m[2][1] - m[1][1] * m[2][0];
        D[2][1] = m[0][1] * m[2][0] - m[0][0] * m[2][1];
        D[2][2] = m[0][0] * m[1][1] - m[0][1] * m[1][0];

        m_scale(D, D, 1.0 / m33_det(m), 3, 3);
    }


    static void m_print(String label, double[][] a, int m, int n)
    {
        int i;
        int j;

        System.out.print(label);
        for (i = 0; i < m; i++) {
            System.out.print("[");
            for (j = 0; j < n; j++)
                System.out.print(" " + a[i][j]);
            System.out.println("]");
        }
    }
}