/* ========================================================================== */
/* === diagdom ============================================================== */
/* ========================================================================== */

/*

  DIAGDOM - a Matlab mexFunction

      [B, i] = diagdom (A, tol)

  DIAGDOM returns a diagonally dominant matrix B by modifying the
  diagonal of A.  If row i violates diagonal dominance, then A(i,i) is
  modified by setting it so that its absolute value is (1+tol)*f, where
  f is the sum of the absolute values of the off-diagonal entries in row i
  (assuming f is large enough).  If f is smaller than tol, then the absolute
  value of A(i,i) is set to (1+tol)*tol.  The sign of A(i,i) is preserved.
  The input argument tol is optional; if not present, a default value of
  100*eps is used.  The outputs are B (the modified matrix) and a vector i
  that contains a list of the row indices of the rows that violated
  diagonal dominance.

  Type "help diagdom" in Matlab to display the above documentation.
  (see diagdom.m).

  MATLAB Primer, 6th Edition
  Kermit Sigmon and Timothy A. Davis
  Section 9.3, page 49.
*/

/* -------------------------------------------------------------------------- */
/* include files and definitions */
/* -------------------------------------------------------------------------- */

#include "mex.h"
#include "matrix.h"
#include <stdlib.h>
#include <float.h>

/* INDEX (i,j,m) returns the 1-dimensional index into an m-by-n array, stored */
/* in column major order, of the entry A(i,j). */

#define INDEX(i,j,m) ((i)+(j)*(m))
#define ABS(x) ((x) >= 0 ? (x) : -(x))
#define MAX(x,y) (((x) > (y)) ? (x) : (y))

/* -------------------------------------------------------------------------- */
/* diagdom computational routine.  compare with ddom6.m (below) */ 
/* -------------------------------------------------------------------------- */

void diagdom
(
    double *A,
    int n,
    double *B,
    double tol,
    int *List,
    int *nList
)
{
    int i, j, k ;
    double d, a, f, bij, bii ;
    for (k = 0 ; k < n*n ; k++)
    {
	B [k] = A [k] ;
    }
    if (tol < 0)
    {
	tol = 100 * DBL_EPSILON ;
    }
    k = 0 ;
    for (i = 0 ; i < n ; i++)
    {
	d = B [INDEX (i,i,n)] ;
	a = ABS (d) ;
	f = 0 ;
	for (j = 0 ; j < n ; j++)
	{
	    if (i != j)
	    {
	        bij = B [INDEX (i,j,n)] ;
	        f += ABS (bij) ;
	    }
	}
	if (f >= a)
	{
	    List [k++] = i ;
	    bii = (1 + tol) * MAX (f, tol) ;
	    if (d < 0)
	    {
		bii = -bii ;
	    }
	    B [INDEX (i,i,n)] = bii ;
	}
    }
    *nList = k ;
}

/* -------------------------------------------------------------------------- */
/* ddom6 - the Matlab equivalent to diagdom.c */
/* -------------------------------------------------------------------------- */

/*
(this Matlab function does not return the list of row indices, however)

function B = ddom6 (A,tol)
%
% MATLAB Primer, 6th Edition
% Kermit Sigmon and Timothy A. Davis
% Section 8.5, page 43.

[m n] = size (A) ;
if (nargin == 1)
    tol = 100 * eps ;
end

for i = 1:n
    d = A (i,i) ;
    a = abs (d) ;
    f = 0 ;
    for j = 1:n
        if (i ~= j)
	    f = f + abs (A (i,j)) ;
	end
    end
    if (f >= a)
        aii = (1 + tol) * max (f, tol) ;
	if (d < 0)
	    aii = -aii ;
	end
	A (i,i) = aii ;
    end
end

B = A ;

*/

/* -------------------------------------------------------------------------- */
/* error */
/* -------------------------------------------------------------------------- */

void error (char *s)
{
    mexPrintf ("Usage: [B,i] = diagdom (A,tol)\n") ;
    mexErrMsgTxt (s) ;
}

/* -------------------------------------------------------------------------- */
/* mexFunction gateway routine for diagdom */
/* -------------------------------------------------------------------------- */

void mexFunction
(
    int nlhs,			/* number of left-hand sides */
    mxArray *plhs [ ],		/* array of pointers to left-hand sides */
    int nrhs,			/* number of right-hand sides */
    const mxArray *prhs [ ]	/* array of pointers to right-hand sides */
)
{
    int n,			/* input matrix A is n-by-n */
	k,			/* index variable */
	*List, nList ;		/* List [0..nList-1]: list of row indices */
				/* that violate diagonal dominance (0-based) */
    double *A,			/* input matrix */
	*B,			/* diagonally dominant output matrix */
	*I,			/* list of row indices (1-based) */
	tol ;			/* tolerance */

    /* ---------------------------------------------------------------------- */
    /* get inputs A and tol */
    /* ---------------------------------------------------------------------- */

    if (nlhs > 2 || nrhs > 2 || nrhs == 0)
    {
	error ("Wrong number of arguments") ;
    }
    if (mxIsEmpty (prhs [0]))
    {
        plhs [0] = mxCreateDoubleMatrix (0, 0, mxREAL) ;
        plhs [1] = mxCreateDoubleMatrix (0, 0, mxREAL) ;
        return ;
    }
    n = mxGetN (prhs [0]) ;
    if (n != mxGetM (prhs [0]))
    {
        error ("A must be square") ;
    }
    if (mxIsSparse (prhs [0]))
    {
        error ("A cannot be sparse") ;
    }
    A = mxGetPr (prhs [0]) ;
    tol = -1 ;
    if (nrhs > 1 && !mxIsEmpty (prhs [1]))
    {
        tol = mxGetScalar (prhs [1]) ;
    }

    /* ---------------------------------------------------------------------- */
    /* create output B */
    /* ---------------------------------------------------------------------- */

    plhs [0] = mxCreateDoubleMatrix (n, n, mxREAL) ;
    B = mxGetPr (plhs [0]) ;

    /* ---------------------------------------------------------------------- */
    /* get temporary workspace */
    /* ---------------------------------------------------------------------- */

    List = (int *) mxMalloc (n * sizeof (int)) ;

    /* ---------------------------------------------------------------------- */
    /* do the computation */
    /* ---------------------------------------------------------------------- */

    diagdom (A, n, B, tol, List, &nList) ;

    /* ---------------------------------------------------------------------- */
    /* create output I (1-based indexing) */
    /* ---------------------------------------------------------------------- */

    plhs [1] = mxCreateDoubleMatrix (nList, 1, mxREAL) ;
    I = mxGetPr (plhs [1]) ;
    for (k = 0 ; k < nList ; k++)
    {
	I [k] = (double) (List [k] + 1) ;
    }

    /* ---------------------------------------------------------------------- */
    /* free the workspace */
    /* ---------------------------------------------------------------------- */

    mxFree (List) ;
}