/* matrix.c - matrix-algebra code

 *

 * This file is Copyright by the GPSD project

 * SPDX-License-Identifier: BSD-2-clause

 */

#include "../include/gpsd_config.h"  // must be before all includes

#include <math.h>

#include <stdbool.h>

#include "../include/matrix.h"

// selected elements from 4x4 matrox inversion

bool matrix_invert(double mat[4][4], double inverse[4][4])

{

    // Find all NECESSARY 2x2 subdeterminants

    double Det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];

    double Det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];

    //double Det2_12_03 = mat[1][0]*mat[2][3] - mat[1][3]*mat[2][0];

    double Det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];

    //double Det2_12_13 = mat[1][1]*mat[2][3] - mat[1][3]*mat[2][1];

    //double Det2_12_23 = mat[1][2]*mat[2][3] - mat[1][3]*mat[2][2];

    double Det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];

    //double Det2_13_02 = mat[1][0]*mat[3][2] - mat[1][2]*mat[3][0];

    double Det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];

    //double Det2_13_12 = mat[1][1]*mat[3][2] - mat[1][2]*mat[3][1];

    double Det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];

    //double Det2_13_23 = mat[1][2]*mat[3][3] - mat[1][3]*mat[3][2];

    double Det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];

    double Det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];

    double Det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];

    double Det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];

    double Det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];

    double Det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];

    // Find all NECESSARY 3x3 subdeterminants

    double Det3_012_012 = mat[0][0] * Det2_12_12 - mat[0][1] * Det2_12_02 +

        mat[0][2] * Det2_12_01;

    //double Det3_012_013 = mat[0][0]*Det2_12_13 - mat[0][1]*Det2_12_03

    //                            + mat[0][3]*Det2_12_01;

    //double Det3_012_023 = mat[0][0]*Det2_12_23 - mat[0][2]*Det2_12_03

    //                            + mat[0][3]*Det2_12_02;

    //double Det3_012_123 = mat[0][1]*Det2_12_23 - mat[0][2]*Det2_12_13

    //                            + mat[0][3]*Det2_12_12;

    //double Det3_013_012 = mat[0][0]*Det2_13_12 - mat[0][1]*Det2_13_02

    //                            + mat[0][2]*Det2_13_01;

    double Det3_013_013 = mat[0][0] * Det2_13_13 - mat[0][1] * Det2_13_03 +

        mat[0][3] * Det2_13_01;

    //double Det3_013_023 = mat[0][0]*Det2_13_23 - mat[0][2]*Det2_13_03

    //                            + mat[0][3]*Det2_13_02;

    //double Det3_013_123 = mat[0][1]*Det2_13_23 - mat[0][2]*Det2_13_13

    //                            + mat[0][3]*Det2_13_12;

    //double Det3_023_012 = mat[0][0]*Det2_23_12 - mat[0][1]*Det2_23_02

    //                            + mat[0][2]*Det2_23_01;

    //double Det3_023_013 = mat[0][0]*Det2_23_13 - mat[0][1]*Det2_23_03

    //                            + mat[0][3]*Det2_23_01;

    double Det3_023_023 = mat[0][0] * Det2_23_23 - mat[0][2] * Det2_23_03 +

        mat[0][3] * Det2_23_02;

    //double Det3_023_123 = mat[0][1]*Det2_23_23 - mat[0][2]*Det2_23_13

    //                            + mat[0][3]*Det2_23_12;

    double Det3_123_012 = mat[1][0] * Det2_23_12 - mat[1][1] * Det2_23_02 +

        mat[1][2] * Det2_23_01;

    double Det3_123_013 = mat[1][0] * Det2_23_13 - mat[1][1] * Det2_23_03 +

        mat[1][3] * Det2_23_01;

    double Det3_123_023 = mat[1][0] * Det2_23_23 - mat[1][2] * Det2_23_03 +

        mat[1][3] * Det2_23_02;

    double Det3_123_123 = mat[1][1] * Det2_23_23 - mat[1][2] * Det2_23_13 +

        mat[1][3] * Det2_23_12;

    // Find the 4x4 determinant

    static double det;

    det = mat[0][0] * Det3_123_123 -

        mat[0][1] * Det3_123_023 +

        mat[0][2] * Det3_123_013 -

        mat[0][3] * Det3_123_012;

    // Very small determinants probably reflect floating-point fuzz near zero

    if (0.0001 > fabs(det)) {

        return false;

    }

    inverse[0][0] = Det3_123_123 / det;

    //inverse[0][1] = -Det3_023_123 / det;

    //inverse[0][2] =  Det3_013_123 / det;

    //inverse[0][3] = -Det3_012_123 / det;

    //inverse[1][0] = -Det3_123_023 / det;

    inverse[1][1] = Det3_023_023 / det;

    //inverse[1][2] = -Det3_013_023 / det;

    //inverse[1][3] =  Det3_012_023 / det;

    //inverse[2][0] =  Det3_123_013 / det;

    //inverse[2][1] = -Det3_023_013 / det;

    inverse[2][2] = Det3_013_013 / det;

    //inverse[2][3] = -Det3_012_013 / det;

    //inverse[3][0] = -Det3_123_012 / det;

    //inverse[3][1] =  Det3_023_012 / det;

    //inverse[3][2] = -Det3_013_012 / det;

    inverse[3][3] = Det3_012_012 / det;

    return true;

}

#ifdef __UNUSED_

// cppcheck-suppress unusedFunction

// symmetrize a matrix, multiply it by its transpose

void matrix_symmetrize(double mat[4][4], double prod[4][4])

{

    int i, j, k;

    for (i = 0; i < 4; ++i) {           //< rows

        for (j = 0; j < 4; ++j) {       //< cols

            prod[i][j] = 0.0;

            for (k = 0; k < 4; ++k) {

                prod[i][j] += mat[k][i] * mat[k][j];

            }

        }

    }

}

#endif  // UNUSED

// vim: set expandtab shiftwidth=4