#include <math.h>
#include "mat.h"
#include "matrixdefs.h"
#include "local_proto.h"

#define EPSILON 1.0e-8

/*
 * inverse: invert a square martix (puts pivot elements on main diagonal).
 *          returns arg2 as the inverse of arg1.
 *
 *  This routine is based on a routine found in Andrei Rogers, "Matrix
 *  Methods in Urban and Regional Analysis", (1971), pp. 143-153.
 */
int inverse(MATRIX *a, MATRIX *b)
{
    int i, j, k, l, ir = 0, ic = 0, nr, nc;
    int ipivot[MAXROWS], itemp[MAXROWS][2];
    double pivot[MAXROWS], t;
    static MATRIX m;

    if (a->nrows == 0)
        return matrix_error("inv: arg1 not defined\n");

    if (a->nrows != a->ncols)
        return matrix_error("inv: matrix not square\n");

    if (isnull(a)) {
        /* fprintf (stderr, " inv: matrix is singular\n"); */
        return matrix_error(
            "inv: matrix is singular. Check camera definitions!\n");
    }
    m_copy(&m, a);
    nr = a->nrows;
    nc = a->ncols;

    /* initialization */
    for (i = 0; i < nr; i++)
        ipivot[i] = 0;

    for (i = 0; i < nr; i++) {
        t = 0.0; /* search for pivot element */
        for (j = 0; j < nr; j++) {
            if (ipivot[j] == 1) /* found pivot */
                continue;
            for (k = 0; k < nc; k++)
                switch (ipivot[k] - 1) {
                case 0:
                    break;
                case -1:
                    if (fabs(t) < fabs(m.x[j][k])) {
                        ir = j;
                        ic = k;
                        t = m.x[j][k];
                    }
                    break;
                case 1:
                    return matrix_error(
                        "inv: matrix is singular. Check camera definitions!\n");
                    break;
                default: /* shouldn't get here */
                    return matrix_error(
                        "inv: matrix is singular. Check camera definitions!\n");
                    break;
                }
        }
        ipivot[ic] += 1;
        if (ipivot[ic] > 1) /* check for dependency */
            return matrix_error(
                "inv: matrix is singular. Check camera definitions!\n");
        /* interchange rows to put pivot element on diagonal */
        if (ir != ic)
            for (l = 0; l < nc; l++) {
                t = m.x[ir][l];
                m.x[ir][l] = m.x[ic][l];
                m.x[ic][l] = t;
            }

        itemp[i][0] = ir;
        itemp[i][1] = ic;
        pivot[i] = m.x[ic][ic];

        /* check for zero pivot */
        if (fabs(pivot[i]) < EPSILON)
            return matrix_error(
                "inv: matrix is singular. Check camera definitions!\n");

        /* divide pivot row by pivot element */
        m.x[ic][ic] = 1.0;
        for (j = 0; j < nc; j++)
            m.x[ic][j] /= pivot[i];

        /* reduce nonpivot rows */
        for (k = 0; k < nr; k++)
            if (k != ic) {
                t = m.x[k][ic];
                m.x[k][ic] = 0.0;
                for (l = 0; l < nc; l++)
                    m.x[k][l] -= (m.x[ic][l] * t);
            }
    }

    /* interchange columns */
    for (i = 0; i < nc; i++) {
        l = nc - i - 1;
        if (itemp[l][0] == itemp[l][1])
            continue;
        ir = itemp[l][0];
        ic = itemp[l][1];
        for (k = 0; k < nr; k++) {
            t = m.x[k][ir];
            m.x[k][ir] = m.x[k][ic];
            m.x[k][ic] = t;
        }
    }

    b->nrows = nr;
    b->ncols = nc;
    m_copy(b, &m);
    return 1;
}
