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

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


//CONSTRUCTS THE (WEIGHTED DISCRETE) L2-APPROXIMATION BY SPLINES OF ORDER
//  K  WITH KNOT SEQUENCE  T(1), ..., T(N+K)  TO GIVEN DATA POINTS 
//  ( TAU(I), GTAU(I) ), I=1,...,NTAU. THE B-SPLINE COEFFICIENTS 
//  B C O E F   OF THE APPROXIMATING SPLINE ARE DETERMINED FROM THE 
//  NORMAL EQUATIONS USING CHOLESKY'S METHOD. 
// ******  I N P U T  ****** 
//  NTAU....NUM OF DATA POINTS 
//  TAU(NTAU).....DATA POINT ABSCISA 
//  GTAU(IG,M)..DATA POINT ORDINATE 
//  WEIGHT(IW,M)..WEIGHTING VALUE OF LEAST SQUARE NORM 
//  IG.....ROW DIMENSION OF THE VARIABLE GTAU 
//  IW....ROW DIMENSION OF THE VARIABLE WEIGHT 
//  IRC...ROW DIMENSION OF THE VARIABLE RCOEF 
//  T(1), ..., T(N+K)  THE KNOT SEQUENCE 
//  N.....THE DIMENSION OF THE SPACE OF SPLINES OF ORDER K WITH KNOTS T.
//  K.....THE ORDER 
//  M....NUM OF LINES TO PERFORM THE L2APPR 
// ******  W O R K  A R R A Y S  ****** 
//  Q(K,N)...A WORK ARRAY OF SIZE (AT LEAST) K*N.ITS FIRST K ROWS ARE 
//           USED FOR THE  K  LOWER DIAGONALS OF THE GRAMIAN MATRIX  C .
//  WORK(N,2)....A WORK ARRAY OF LENGTH  2*N . 
// ******  O U T P U T  ****** 
//  RCOEF(NL,1), ..., RCOEF(NL,N)  THE B-SPLINE COEFFS. OF THE L2-APPR. 
//                               STORED IN ROW WISE. 1<=NL<=M 
// ******  M E T H O D  ****** 
//  THE B-SPLINE COEFFICIENTS OF THE L2-APPR. ARE DETERMINED AS THE SOL- 
//    UTION OF THE NORMAL EQUATIONS 
//     SUM ( (B(I),B(J))*RCOEF(J) : J=1,...,N)  = (B(I),G), 
//                                               I = 1, ..., N . 
//  HERE,  B(I)  DENOTES THE I-TH B-SPLINE,  G  DENOTES THE FUNCTION TO 
//  BE APPROXIMATED, AND THE  I N N E R   P R O D U C T  OF TWO FUNCT- 
//  IONS  F  AND  G  IS GIVEN BY 
//      (F,G)  :=  SUM ( F(TAU(I))*G(TAU(I))*WEIGHT(I,.) : I=1,...,NTAU) .
//   THE RELEVANT FUNCTION VALUES OF THE B-SPLINES  B(I), I=1,...,N, ARE 
//  SUPPLIED BY THE SUBPROGRAM  B S P L V B . 
//     THE COEFF.MATRIX  C , WITH 
//           C(I,J)  :=  (B(I), B(J)), I,J=1,...,N, 
//  OF THE NORMAL EQUATIONS IS SYMMETRIC AND (2*K-1)-BANDED, THEREFORE 
//  CAN BE SPECIFIED BY GIVING ITS K BANDS AT OR BELOW THE DIAGONAL. FOR 
//    I=1,...,N,  WE STORE 
//   (B(I),B(J))  =  C(I,J)  IN  Q(I-J+1,J), J=I,...,MIN0(I+K-1,N) 
//  AND THE RIGHT SIDE 
//   (B(I), G )  IN  RCOEF(I) . 
// SINCE B-SPLINE VALUES ARE MOST EFFICIENTLY GENERATED BY FINDING SIM- 
//  ULTANEOUSLY THE VALUE OF  E V E R Y  NONZERO B-SPLINE AT ONE POINT, 
//  THE ENTRIES OF  C  (I.E., OF  Q ), ARE GENERATED BY COMPUTING, FOR 
//  EACH LL, ALL THE TERMS INVOLVING  TAU(LL)  SIMULTANEOUSLY AND ADDING 
//  THEM TO ALL RELEVANT ENTRIES. 
void blgl2a_(int ntau, const double *tau, const double *gtau,
	 const double *weight, int ig, int iw, int irc, int kw,
	 const double *t, int n, int k, int m, double *work, double *q, double *rcoef
){
    int q_offset;
    int left, i, j;
    int jj, ll, mm, nl;
    double dw;
    int leftmk, npk;
    double riatx_buf[20];
	double* riatx=riatx_buf;
	if(k>20)
		riatx=(double*)malloc(sizeof(double)*(k));

    // Parameter adjustments 
    --tau;
    --t;
    rcoef -= irc+1;
    work -=  n+1;
    q_offset = k+1;
    q -= q_offset;
    weight -= iw+1;;
    gtau -= ig+1;

    // Function Body 
    npk = n+k;
    for(nl=1; nl<=m; ++nl){

	for(j=1; j<=n; ++j){
	    work[j+n] = 0.f;
	    for(i=1; i<=k; ++i)
			q[i + j * k] = 0.f;
	}
	left = k;
	leftmk = 0;
	for(ll=1; ll<=ntau; ++ll){
		//LOCATE  L E F T  S.T. TAU(LL) IN (T(LEFT),T(LEFT+1)) 
		while(left<n && tau[ll]>=t[left+1]){
			++left; ++leftmk;
		}
	    b2vb_(k,npk,&t[1],tau[ll],left,riatx);
//   RIATX(MM) CONTAINS THE VALUE OF B(LEFT-K+MM) AT TAU(LL). 
//   HENCE, WITH  DW := RIATX(MM)*WEIGHT(LL,.), THE NUMBER DW*GTAU(LL) 
//        IS A SUMMAND IN THE INNER PRODUCT 
//           (B(LEFT-K+MM), G)  WHICH GOES INTO  RCOEF(LEFT-K+MM)
//        AND THE NUMBER BIATX(JJ)*DW IS A SUMMAND IN THE INNER PRODUCT 
//           (B(LEFT-K+JJ), B(LEFT-K+MM)), INTO  Q(JJ-MM+1,LEFT-K+MM) 
//        SINCE  (LEFT-K+JJ) - (LEFT-K+MM) + 1  =  JJ - MM + 1 . 
	    for(mm=1; mm<=k; ++mm){
			if(kw==1)
				dw = riatx[mm-1]*weight[ll+nl*iw];
			else if(kw==18)
				dw = riatx[mm-1]*weight[nl+ll*iw];

			j = leftmk + mm;
			work[j+n] = dw*gtau[ll+nl*ig]+work[j+n];
			i = 1;
			for(jj=mm; jj<=k; ++jj){
			    q[i+j*k]=riatx[jj-1]*dw+q[i+j*k];
				++i;
			}
	    }
	}

//        CONSTRUCT CHOLESKY FACTORIZATION FOR  C  IN  Q , THEN USE 
//        IT TO SOLVE THE NORMAL EQUATIONS 
//               C*X  =  RCOEF 
//        FOR  X , AND STORE  X  IN  RCOEF . 
	b1hfac_(&q[q_offset],k,n,&work[(n<<1)+1]);
	b1hslv_(&q[q_offset],k,n,&work[n+1]);
	for(i=1; i<=n; ++i)
	    rcoef[nl+i*irc]=work[i+n];

    }
	if(k>20) free(riatx);
}