#include "MGCLStdAfx.h"
/********************************************************************/
/* Copyright (c) 2017 System fugen G.K. and Yuzi Mizuno          */
/* All rights reserved.                                             */
/********************************************************************/

//The original codes of this program comes from the FORTRAN code BCHFAC of
//"A Practical Guide to Splines" by Carl de Boor.

//  FROM  * A PRACTICAL GUIDE TO SPLINES *  BY C. DE BOOR 
// CONSTRUCTS CHOLESKY FACTORIZATION 
//                     C  =  L * D * L-TRANSPOSE 
//  WITH L UNIT LOWER TRIANGULAR AND D DIAGONAL, FOR GIVEN MATRIX C OF 
//  ORDER  N R O W , IN CASE  C  IS (SYMMETRIC) POSITIVE SEMIDEFINITE 
//  AND  B A N D E D , HAVING  N B A N D S  DIAGONALS AT AND BELOW THE 
//  MAIN DIAGONAL. 
// ******  I N P U T  ****** 
//  NROW.....IS THE ORDER OF THE MATRIX  C . 
//  NBANDS.....INDICATES ITS BANDWIDTH, I.E., 
//          C(I,J) = 0 FOR ABS(I-J) .GT. NBANDS . 
//  W.....WORKARRAY OF SIZE (NBANDS,NROW)  CONTAINING THE  NBANDS  DIAGO-
//        NALS IN ITS ROWS, WITH THE MAIN DIAGONAL IN ROW  1 . PRECISELY, 
//        W(I,J)  CONTAINS  C(I+J-1,J), I=1,...,NBANDS, J=1,...,NROW. 
//          FOR EXAMPLE, THE INTERESTING ENTRIES OF A SEVEN DIAGONAL SYM-
//        METRIC MATRIX  C  OF ORDER  9  WOULD BE STORED IN  W  AS 
//                       11 22 33 44 55 66 77 88 99   
//                       21 32 43 54 65 76 87 98      
//                       31 42 53 64 75 86 97         
//                       41 52 63 74 85 96            
//        ALL OTHER ENTRIES OF  W  NOT IDENTIFIED IN THIS WAY WITH AN EN-
//        TRY OF  C  ARE NEVER REFERENCED . 
//  DIAG.....IS A WORK ARRAY OF LENGTH  NROW . 
// ******  O U T P U T  ****** 
//  W.....CONTAINS THE CHOLESKY FACTORIZATION  C = L*D*L-TRANSP, WITH 
//        W(1,I) CONTAINING  1/D(I,I) 
//        AND  W(I,J)  CONTAINING  L(I-1+J,J), I=2,...,NBANDS. 
// ******  M E T H O D  ****** 
//   GAUSS ELIMINATION, ADAPTED TO THE SYMMETRY AND BANDEDNESS OF  C , IS
//   USED . 
//     NEAR ZERO PIVOTS ARE HANDLED IN A SPECIAL WAY. THE DIAGONAL ELE- 
//  MENT C(N,N) = W(1,N) IS SAVED INITIALLY IN  DIAG(N), ALL N. AT THE N- 
//  TH ELIMINATION STEP, THE CURRENT PIVOT ELEMENT, VIZ.  W(1,N), IS COM- 
//  PARED WITH ITS ORIGINAL VALUE, DIAG(N). IF, AS THE RESULT OF PRIOR 
//  ELIMINATION STEPS, THIS ELEMENT HAS BEEN REDUCED BY ABOUT A WORD 
//  LENGTH, (I.E., IF W(1,N)+DIAG(N) .LE. DIAG(N)), THEN THE PIVOT IS DE- 
//  CLARED TO BE ZERO, AND THE ENTIRE N-TH ROW IS DECLARED TO BE LINEARLY 
//  DEPENDENT ON THE PRECEDING ROWS. THIS HAS THE EFFECT OF PRODUCING 
//   X(N) = 0  WHEN SOLVING  C*X = B  FOR  X, REGARDLESS OF  B. JUSTIFIC- 
//  ATION FOR THIS IS AS FOLLOWS. IN CONTEMPLATED APPLICATIONS OF THIS 
//  PROGRAM, THE GIVEN EQUATIONS ARE THE NORMAL EQUATIONS FOR SOME LEAST- 
//  SQUARES APPROXIMATION PROBLEM, DIAG(N) = C(N,N) GIVES THE NORM-SQUARE 
//  OF THE N-TH BASIS FUNCTION, AND, AT THIS POINT,  W(1,N)  CONTAINS THE 
//  NORM-SQUARE OF THE ERROR IN THE LEAST-SQUARES APPROXIMATION TO THE N- 
//  TH BASIS FUNCTION BY LINEAR COMBINATIONS OF THE FIRST N-1 . HAVING 
//  W(1,N)+DIAG(N) .LE. DIAG(N) SIGNIFIES THAT THE N-TH FUNCTION IS LIN-  
//  EARLY DEPENDENT TO MACHINE ACCURACY ON THE FIRST N-1 FUNCTIONS, THERE 
//  FORE CAN SAFELY BE LEFT OUT FROM THE BASIS OF APPROXIMATING FUNCTIONS 
//     THE SOLUTION OF A LINEAR SYSTEM 
//                       C*X = B 
//   IS EFFECTED BY THE SUCCESSION OF THE FOLLOWING  T W O  CALLS: 
//     CALL B1HFAC ( W, NBANDS, NROW, DIAG )       , TO GET FACTORIZATION 
//     CALL B1HSLV ( W, NBANDS, NROW, B, X )            , TO SOLVE FOR X. 
void b1hfac_(double *w, int nbands, int nrow, double *diag
){
    int imax, jmax, i, j, n;
    double ratio;

    // Parameter adjustments 
    --diag;
    w -= nbands + 1;;

    if(nrow<=1){
		if(w[nbands+1]>0.f)
			w[nbands+1] = 1.f/w[nbands+1];
		return;
	}

//STORE DIAGONAL OF  C  IN  DIAG.
	for(n=1; n<=nrow; ++n)
		diag[n] = w[n*nbands+1];

//FACTORIZATION . 
    for(n=1; n<=nrow; ++n){
		if(w[n*nbands+1]+diag[n]<=diag[n]){
			for(j=1; j<=nbands; ++j)
				w[j+n*nbands] = 0.f;
			continue;
		}

		w[n*nbands+1] = 1.f/w[n*nbands+1];
		imax = nrow-n;
		if(imax>nbands-1)
			imax = nbands-1;
		if(imax<1)
			continue;

		jmax = imax;
		for(i=1; i<=imax; ++i){
			ratio = w[i+1+n*nbands]*w[n*nbands+1];
		    for(j=1; j<=jmax; ++j)
				w[j+(n+i)*nbands] -= w[j+i+n*nbands]*ratio;
			--jmax;
		    w[i+1+n*nbands] = ratio;
		}
    }
}