// TODO: CopyRight .. =???

#include <math.h>
#include "lsq.h"
//#include <malloc.h>

void xdisaster() {
}

void lsq::startup(int nvar, bool fit_const) {
/*
!     Allocates dimensions for arrays and initializes to zero
!     The calling program must set nvar = the number of variables, and
!     fit_const = true if a constant is to be included in the model,
!     otherwise fit_const = false
!
	!--------------------------------------------------------------------------*/
	
	int i;
	
	nobs = 0;
	if (fit_const) {
		ncol = nvar + 1;
	} else {
		ncol = nvar;
	}
	
	if (initialized) {
		free(d);
		free(rhs);
		free(r);
		free(tol);
		free(rss);
		free(vorder);
	}
	
	r_dim = ncol * (ncol - 1)/2;
	
	++ncol;
	++r_dim;
	d = (double*)malloc(sizeof(double)*ncol);
	rhs = (double*)malloc(sizeof(double)*ncol);
	tol = (double*)malloc(sizeof(double)*ncol);
	rss = (double*)malloc(sizeof(double)*ncol);
	vorder = (int*)malloc(sizeof(int)*ncol);
	r = (double*)malloc(sizeof(double)*r_dim);
	--ncol;
	--r_dim;
	
	for (i = 0; i <= ncol; ++i) {
		d[i] = zero;
		rhs[i] = zero;
	}
//nt tmpc=0;
	for (i = 0; i <= r_dim; ++i) {
/*		if (i%((ncol+ncol-tmpc)/2 + 1)==0) {
			tmpc++;
			r[i]=0.5;
		}
		else*/
			r[i] = zero;
	}
	sserr = zero;
	
	if (fit_const) {
		for (i = 1; i <= ncol; ++i) {
			vorder[i] = i-1;
		}
	} else { 	
		for (i = 1; i <= ncol; ++i) {
			vorder[i] = i;
		}
	}

	initialized = true;
	tol_set = false;
	rss_set = false;
}

void lsq::includ(double weight, double *xrow, double yelem) {
/*
!     ALGORITHM AS75.1  APPL. STATIST. (1974) VOL.23, NO. 3
!     Calling this routine updates D, R, RHS and SSERR by the
!     inclusion of xrow, yelem with the specified weight.
!     *** WARNING  Array XROW is overwritten.
!     N.B. As this routine will be called many times in most applications,
!          checks have been eliminated.
!
!--------------------------------------------------------------------------
	*/
	int i, k, nextr;
	double w, y, xi, di, wxi, dpi, cbar, sbar, xk;
	
	nobs = nobs + 1;
	w = weight;
	y = yelem;
	rss_set = false;
	nextr = 1;
	for (i = 1; i <= ncol; ++i) {
		//!     Skip unnecessary transformations.   Test on exact zeroes must be
		//!     used or stability can be destroyed.
		
		if (fabs(w) < vsmall)
			return;
		xi = xrow[i];
		if (fabs(xi) < vsmall)
			nextr = nextr + ncol - i;
		else {
			di = d[i];
			wxi = w * xi;
			dpi = di + wxi*xi;
			cbar = di / dpi;
			sbar = wxi / dpi;
			w = cbar * w;
			d[i] = dpi;
			for (k = i+1; k <= ncol; ++k) {
 				xk = xrow[k];
				xrow[k] = xk - xi * r[nextr];
				r[nextr] = cbar * r[nextr] + sbar * xk;
				nextr = nextr + 1;
			}
			xk = y;
			y = xk - xi * rhs[i];
			rhs[i] = cbar * rhs[i] + sbar * xk;
		}
	}
	
	//!     Y * sqrt(W) is now equal to the Brown, Durbin & Evans recursive
	//!     residual.
	
	sserr = sserr + w * y * y;
	
}


void lsq::regcf(double *beta, int nreq, int& ifault) {
/*
!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2
!     Modified version of AS75.4 to calculate regression coefficients
!     for the first NREQ variables, given an orthogonal reduction from
!     AS75.1.
!
!--------------------------------------------------------------------------
	*/
	int i, j, nextr;
	
	ifault = 0;
	if (nreq < 1 || nreq > ncol) 
		ifault = ifault + 4;
	if (ifault != 0) 
		return;
	
	if (!tol_set) 
		tolset();
	
	for (i = nreq; i >= 1; --i) {
		if (sqrt(d[i]) < tol[i]) {
			beta[i] = zero;
			d[i] = zero;
		} else {
			beta[i] = rhs[i];
			nextr = (i-1) * (ncol+ncol-i)/2 + 1;
			for(j = i+1; j <= nreq; ++j) {
				beta[i] -= r[nextr] * beta[j];
				nextr = nextr + 1;
			}
		}
	}
	
	return;
}




void lsq::tolset() {
/*
!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2

  !     Sets up array TOL for testing for zeroes in an orthogonal
  !     reduction formed using AS75.1.
	*/
	int col, row, pos,i;
	double eps, ten = 10.0, total, *work;
	
	work = new double[ncol+1];
	eps = .2220e-7;
	
	/*
	!     Set tol(i) = sum of absolute values in column I of R after
	!     scaling each element by the square root of its row multiplier,
	!     multiplied by EPS.
	*/
	
	for (i = 1; i <= ncol; ++i) {
		work[i] = sqrt(d[i]);
	}
	for(col = 1; col <= ncol; ++col) {
		pos = col - 1;
		total = work[col];
		for(row = 1; row < col; ++row) {
			total = total + fabs(r[pos]) * work[row];
			pos = pos + ncol - row - 1;
		}
		tol[col] = eps * total;
	}
	
	tol_set = true;
	delete[] work;
}



void lsq::sing(bool* lindep, int& ifault) {
/*
!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2
!     Checks for singularities, reports, and adjusts orthogonal
!     reductions produced by AS75.1.
!--------------------------------------------------------------------------
	*/
	
	double temp, *x, *work, y, weight;
	int col, pos, row, pos2,i,l;
	
	x = new double[ncol+1];
	work = new double[ncol+1];
	
	ifault = 0;

	if(!tol_set)
		tolset();
	
	for (i = 1; i <= ncol; ++i) {
		work[i] = sqrt(d[i]);
	}	
	for( col = 1; col <= ncol; ++col) {
		
	/*
	!     Set elements within R to zero if they are less than tol(col) in
	!     absolute value after being scaled by the square root of their row
	!     multiplier.
		*/
		
		temp = tol[col];
		pos = col - 1;
		for(row = 1; row <= col-1; ++row) {
			if (fabs(r[pos]) * work[row] < temp) 
				r[pos] = zero;
			pos = pos + ncol - row - 1;
		}
		/*
		!     If diagonal element is near zero, set it to zero, set appropriate
		!     element of LINDEP, and use INCLUD to augment the projections in
		!     the lower rows of the orthogonalization.
		*/
//		printf("%f %f\n",work[col],temp);
		lindep[col] = false;
		if (work[col] <= temp) {
			lindep[col] = true;
			ifault = ifault - 1;
			if (col < ncol) {
				pos2 = pos + ncol - col + 1;
				for(l = 1; l <= ncol; ++l) {
					x[l] = zero;
				}
				for(l = col+1; l <= ncol; ++l) {
					x[l] = r[pos+l-col];
				}
				y = rhs[col];
				weight = d[col];
				for(l = pos+1; l <= pos2-1; ++l) {
					r[l] = zero;
				}
				d[col] = zero;
				rhs[col] = zero;
				includ(weight, x, y);
				nobs--;
			}
			else
				sserr = sserr + d[col] * rhs[col]*rhs[col];
		}
	}
	
	delete[] x;
	delete[] work;
	return;
}


void lsq::ss() {
/*
!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2
!     Calculates partial residual sums of squares from an orthogonal
!     reduction from AS75.1.
!
!--------------------------------------------------------------------------
	*/
	
	int i;
	double total;
	
	total = sserr;
	rss[ncol] = sserr;
	for (i = ncol; i >= 2; --i) {
		total = total + d[i] * rhs[i]*rhs[i];
		rss[i-1] = total;
	}
	rss_set = false;
	return;
}

void lsq::cov(int nreq, double& var, double *covmat, int dimcov, double *sterr, int& ifault) {
/*
!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2
!     Calculate covariance matrix for regression coefficients for the
!     first nreq variables, from an orthogonal reduction produced from
!     AS75.1.
	*/
	
	int dim_rinv, pos, row, start, pos2, col, pos1, k;
	double total;
	double *rinv;
	
	//!     Check that dimension of array covmat is adequate.
	
	if (dimcov < nreq*(nreq+1)/2) {
		ifault = 1;
		return;
	}

	if (!rss_set)
		ss();

	
	//!     Check for small or zero multipliers on the diagonal.
	
	ifault = 0;
	for (row = 1;row<= nreq; ++row) {
		if (fabs(d[row]) < vsmall) 
			ifault = -row;
	}
	if (ifault != 0) 
		return;
	
	//!     Calculate estimate of the residual variance.
	
	if (nobs > nreq) 
		var = rss[nreq] / (nobs - nreq);
	else {
		ifault = 2;
		return;
	}
	
	dim_rinv = nreq*(nreq-1)/2;

	rinv = (double *)malloc(sizeof(double)*(dim_rinv+1));	

	inv(nreq, rinv);
	pos = 1;
	start = 1;
	for(row = 1; row <= nreq; ++row) {
		pos2 = start;
		for(col = row; col <= nreq; ++col) {
			pos1 = start + col - row;
			if (row == col) 
				total = one / d[col];
			else
				total = rinv[pos1-1] / d[col];
			
			for(k = col+1; k <= nreq; ++k) {
				total = total + rinv[pos1] * rinv[pos2] / d[k];
				pos1 = pos1 + 1;
				pos2 = pos2 + 1;
			}
			covmat[pos] = total * var;
			if (row == col)
				sterr[row] = sqrt(covmat[pos]);
			pos = pos + 1;
		}
		start = start + nreq - row;
	}
	free(rinv);
	return;
}


void lsq::inv(int nreq, double *rinv) {
	
/*
!     ALGORITHM AS274  APPL. STATIST. (1992) VOL.41, NO. 2
!     Invert first nreq rows and columns of Cholesky factorization
!     produced by AS 75.1.
!
!--------------------------------------------------------------------------
	*/
	
	int pos, row, col, start, k, pos1, pos2;
	double total;
	
	//!     Invert R ignoring row multipliers, from the bottom up.
	
	pos = nreq * (nreq-1)/2;
	for (row = nreq-1; row >=  1; row--) {
		start = (row-1) * (ncol+ncol-row)/2 + 1;
		for (col = nreq; col >= row+1; col--) {