/*

 Compute Cholesky factor for ND slice covariance matrices


 Usage
 -----

 R = ndchol(S)


 Inputs
 -------

 S           ND slice covariance matrices (d x d x n1 x ... x np)


 Ouputs
 -------

 R           Cholesky factor (d x d x n1 x ... x np)


 Compile with:
 ------------

  mex   ndchol.c

  or

  mex  -f mexopts_intel10amd.bat -output ndchol.dll ndchol.c


 Author          S閎astien PARIS (sebastien.paris@lsis.org) (5/4/08)
 -------



*/




#include <math.h>
#include "mex.h"

/* ----------------------------------------------------------------------------------------------- */


int ndchol(double *  , double * , int , int);

/* ----------------------------------------------------------------------------------------------- */
        

void mexFunction( int nlhs, mxArray *plhs[] , int nrhs, const mxArray *prhs[] )

{

  int i , n  , sizA = 1 , numDimsA = 0, out;

  const int *dimsA = NULL;

  double   *A, *B;

 /* Check-in */
  
  if(nrhs != 1)
	  
	{
     mexErrMsgTxt("A ND semi-positive matrix is required");	
	}

    
 A        = mxGetPr(prhs[0]);
    
 numDimsA = mxGetNumberOfDimensions(prhs[0]);
    
 dimsA    = mxGetDimensions(prhs[0]);

 n        =   dimsA[1];

 
  if (n != dimsA[0])

  {
		mexErrMsgTxt("First two dimensions must be identical");
 
  }

  for (i = 2; i<numDimsA ; i++)

  {
	  sizA     *= dimsA[i];
  }
	
	plhs[0] = mxCreateNumericArray(numDimsA, dimsA, mxDOUBLE_CLASS, mxREAL);
    		

    B       = mxGetPr(plhs[0]);

    out     = ndchol(A  , B , n , sizA);

	 if (out == 0)

  {
		mexErrMsgTxt("Matrix non semi-positive!!!");
  }

	
}

/* ----------------------------------------------------------------------------------------------- */

/*
  
int ndchol(double *A  , double *B , int n , int sizA)

{
    
int i, j , k , r , nn = n*n, rnn , innn , knnn;

double sum = 0 , p = 1 , inv_p;
    
	

for (i = 0 ; i < (nn*sizA) ; i++)

B[i] = A[i];
    
for(r = 0 ; r<sizA ; r ++)
{
    	
	rnn         = r*nn;

    p           = sqrt(B[0 + rnn]);

	B[0 + rnn]  = p;

    inv_p       = 1/p;

    for(i = 1; i < n; i++)
	
	{
		 B[i + rnn]  *= p;
    }
 
	
	for(i = 1; i < n; i++)
	
	{
        innn =  i*n + rnn;

        sum  = B[i + innn];

        for(k = 0; k < i; ++k)
		
		{
			
			knnn = i + k*n + rnn;

            sum -= B[knnn]*B[knnn];
        }
        
		if(sum <= 0.0)

		{
            return 0;
        }
        
		p     = sqrt(sum);

        inv_p = 1/p; 
       
		for(j = n - 1; j > i ; --j)
		{
            sum = B[j + innn];

            for(k = 0; k < i; ++k)
			
			{
                knnn   =  k*n + rnn;

				sum   -= B[j + knnn ]*B[i + knnn];
            }
            

			B[j + innn] = sum*inv_p;

        }

		B[i + innn] = p;

        for(k = 0 ; k < i ; k++)

		{
			B[k + innn] = 0.0;
		}

    }
}

return 1;
    
}

*/

/* ----------------------------------------------------------------------------------------------- */


  
int ndchol(double *A  , double *B , int n , int sizA)

{
    
int i, i1 , j , k , r , jn , in , nn = n*n , n1 = n - 1 , rnn , innn , knnn;

double sum = 0 , p = 1 , inv_p;
    
	

for (i = 0 ; i<(nn*sizA) ; i++)
{
	B[i] = A[i];
}
for(r = 0 ; r<sizA ; r ++)
{
    	
	rnn         = r*nn;

    p           = sqrt(B[0 + rnn]);

	inv_p       = 1/p;
	
	B[0 + rnn]  = p;


    for(i = 1; i < n; i++)
	
	{
		 B[n*i + rnn]  *= inv_p;
    }
 
	
	for(i = 1; i < n; i++)
	
	{
        in   = i*n;

		i1   = i - 1;
		
		innn = in + rnn;

        sum  = B[i + innn];    //sum = B[i][i]

        for(k = 0; k < i; ++k)
		
		{			
			knnn = innn + k;

			sum -= B[knnn]*B[knnn];
        }
        
		if(sum <= 0.0)

		{
            return 0;
        }
        
		p     = sqrt(sum);

		inv_p = 1/p;
       
		for(j = n1; j > i ; --j)
		{
			jn   = j*n;

			sum  = B[jn + i + rnn];

            for(k = 0; k < i; ++k)
			
			{
                knnn   =  k + rnn;

				sum   -= B[jn + knnn]*B[in + knnn];
            }
            

			B[jn + i + rnn] = sum*inv_p;

        }

		B[i + innn] = p;

		for(k = n1  ; k>i1 ; k--)
		{
			B[k + i1*n + rnn ] = 0.0;	 
		}
    }
}

return 1;
    
}