/* Copyright (C) 1991-2000 Simon N. Wood  snw@st-and.ac.uk

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License   
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.
(www.gnu.org/copyleft/gpl.html)

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307,
USA.*/

/* Routines for basic matrix manipulation creation, destruction and file i/o
   for matrices. See end of file for update log */


 

#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include "matrix.h"
#define RANGECHECK
#define PAD 1L

matrix null_mat; /* matrix for passing when you don't actually need to */
#define PADCON (-1.234565433647588392902028934e270)

/* counter for memory used */


long memused=0L,matrallocd=0L;

/* the routines */

struct mrec
{ matrix mat;
  struct mrec *fp,*bp;
};
typedef struct mrec MREC;

void ErrorMessage(char *msg,int fatal);

matrix null_mat;
MREC *top,*bottom;

matrix initmat(rows,cols) long rows,cols;

/* Don't alter this without altering freemat() as well !! */

{ matrix A;long i,j,pad;
#ifdef RANGECHECK
  pad=PAD;
#else
  pad=0L;
#endif
  A.vec=0;
/*  if (rows*cols==0L)
  { A.original_r=A.r=rows;A.original_c=A.c=cols;A.mem=0L;A.vec=0;
    A.M=(double **)NULL;A.V=(double *)NULL;return(A);
  } */
  A.M=(double **)calloc((size_t)rows+2*pad,sizeof(double *));
  if (((cols==1L)||(rows==1L))&&((cols*(long)rows+2*pad)*(long)sizeof(double)<=65535))
  { if (A.M)
    A.M[0]=(double *)calloc((size_t)cols*rows+2*pad,sizeof(double));
    for (i=1L;i<rows+2*pad;i++)
    A.M[i]=A.M[0]+i*cols;A.vec=1;
  } else
  { if (A.M)
    for (i=0L;i<rows+2*pad;i++)
    A.M[i]=(double *)calloc((size_t)cols+2*pad,sizeof(double));
  }
  A.mem=rows*cols*sizeof(double);
  memused+=A.mem;matrallocd++;
  A.original_r=A.r=rows;A.original_c=A.c=cols;
  if (((!A.M)||(!A.M[rows-1+2*pad]))&&(rows*cols>0L))
  { ErrorMessage("Failed to initialize memory for matrix.",1);}
  if (pad)  /* This lot is debugging code that checks out matrix errors
		       on allocation and release */
  { if (A.vec)
    { A.V=A.M[0];for (i=0;i<pad;i++) { A.V[i]=PADCON;A.V[i+pad+A.r*A.c]=PADCON;}
    } else
    { for (i=0;i<A.r+2*pad;i++)
      { for (j=0;j<pad;j++) A.M[i][j]=PADCON;
	     for (j=A.c+pad;j<A.c+2*pad;j++) A.M[i][j]=PADCON;
      }
      for (i=0;i<A.c+2*pad;i++)
      { for (j=0;j<pad;j++) A.M[j][i]=PADCON;
	     for (j=A.r+pad;j<A.r+2*pad;j++) A.M[j][i]=PADCON;
      }
    }
    for (i=0;i<A.r+2*pad;i++)
    for (j=0;j<pad;j++) A.M[i]++;  /* shifting pointers forward past padding */
    if (!A.vec) for (j=0;j<pad;j++) A.M++;
    A.V=A.M[0];
  /* putting a record of the matrix on the linked list of all extant matrices */
    if (matrallocd==1) /*new list*/
    { top=bottom=(MREC *)calloc(1,sizeof(MREC));
      bottom->mat=top->mat=A;top->bp=bottom;bottom->fp=top;
    } else  /* expanding the linked list by one */
    { top->fp=(MREC *)calloc(1,sizeof(MREC));
      top->fp->mat=A;top->fp->bp=top;top=top->fp; /* crystal clear, no? */
    }
  }
  A.V=A.M[0];/* This allows vectors to be accessed using A.V[i] */
  return(A);
}

matrix initvec(rows) long rows;

{ return(initmat(1L,rows));}

void freemat(A) matrix A;

{ long i,j,pad;int ok=1;
  MREC *delet;
#ifdef RANGECHECK
  pad=PAD;
#else
  pad=0L;
#endif
/*  if (A.original_r*A.original_c!=0L) */
  { if (pad)
    { if (A.vec)
      { for (i=-pad;i<0;i++)
	     if ((A.V[i]!=PADCON)||(A.V[i+A.original_r*A.original_c+pad]!=PADCON))
	     ok=0;
      } else
      { for (i=-pad;i<A.original_r+pad;i++)
	     { for (j=A.original_c;j<A.original_c+pad;j++) if (A.M[i][j]!=PADCON) ok=0;
	       for (j=-pad;j<0;j++) if (A.M[i][j]!=PADCON) ok=0;
	     }
	     for (i=-pad;i<A.original_c+pad;i++)
	     { for (j=A.original_r;j<A.original_r+pad;j++) if (A.M[j][i]!=PADCON) ok=0;
	       for (j=-pad;j<0;j++) if (A.M[j][i]!=PADCON) ok=0;
	     }
      }
      if (!ok)
      { ErrorMessage("An out of bound write to matrix has occurred!",1);
      }
      /* find the matrix being deleted in the linked list of extant matrices */
      i=0L;delet=bottom;
      while ((i<matrallocd)&&(delet->mat.M!=A.M)) { i++;delet=delet->fp;}
      if (i==matrallocd)
      { ErrorMessage("INTEGRITY PROBLEM in the extant matrix list.",1);
      } else
      { if (i)
	     delet->bp->fp=delet->fp;
	     else bottom=delet->fp;
	     if (i!=matrallocd-1)
	     delet->fp->bp=delet->bp;
	     else top=delet->bp;
	     free(delet);
      }
      /* repositioning pointers so that what was allocated gets freed */
      if (!A.vec) for (i=0;i<pad;i++) A.M--;
      for (i=0;i<A.original_r+2*pad;i++)
      for (j=0;j<pad;j++) A.M[i]--;
    }
    if (A.vec) free(A.M[0]); else
    for (i=0;i<A.original_r+2*pad;i++) if (A.M[i]) free(A.M[i]);
    if (A.M) free(A.M);
    memused -= A.mem;matrallocd--;
  }
}

void matrixintegritycheck()

/* iff RANGECHECK is defined above then you can call this routine to check
   on the integrity of the matrix system. The routine looks for writing out
   of bounds from the matrix */

{ MREC *B;
  int ok=1;
  long pad=PAD,i,j,k=0L;
  matrix A;
#ifndef RANGECHECK
  ErrorMessage("You are trying to check matrix integrity without defining RANGECHECK.");
#endif
  B=bottom;
  while (k<matrallocd)
  { A=B->mat;
    if (A.vec)
    { for (i=-pad;i<0;i++)
      if ((A.V[i]!=PADCON)||(A.V[i+A.original_r*A.original_c+pad]!=PADCON))
      ok=0;
    } else
    { for (i=-pad;i<A.original_r+pad;i++)
      { for (j=A.original_c;j<A.original_c+pad;j++) if (A.M[i][j]!=PADCON)
	     ok=0;
	     for (j=-pad;j<0;j++) if (A.M[i][j]!=PADCON)
        ok=0;
      }
      for (i=-pad;i<A.original_c+pad;i++)
      { for (j=A.original_r;j<A.original_r+pad;j++) if (A.M[j][i]!=PADCON)
	     ok=0;
	     for (j=-pad;j<0;j++) if (A.M[j][i]!=PADCON)
	     ok=0;
      }
    }
    if (!ok)
    { ErrorMessage("An out of bound write to matrix has occurred!",1);
    }
    k++;B=B->fp;
  }
}

long fsafewrite(ptr,size,n,stream)
     double *ptr;size_t size;long n;FILE *stream;

{ long i,j,k=0L;
  for (i=0;i<(n/32000L);i++)
  k+=fwrite(ptr+i*32000L,size,(size_t)32000L,stream);
  j=n%32000L;
  k+=fwrite(ptr+i*32000L,size,(size_t)j,stream);
  return(k);
}

long fsaferead(ptr,size,n,stream)
     double *ptr;size_t size;long n;FILE *stream;

{ long i,j=32000L,k=0L;
  for (i=0;i<(n/32000L);i++)
  k+=fread(ptr+i*32000L,size,(size_t)j,stream);
  j=n%32000L;
  k+=fread(ptr+i*32000L,size,(size_t)j,stream);
  return(k);
}


void dumpmat(M,filename) matrix M;char *filename;

{ FILE *out;
  long i,j=0L;
  out=fopen(filename,"wb");
  j+=fwrite(&(M.r),sizeof(long),1,out);j+=fwrite(&(M.c),sizeof(long),1,out);
  for (i=0;i<M.r;i++)
  j+=fsafewrite(M.M[i],sizeof(double),M.c,out);
  if (j!=M.c*M.r+2*sizeof(long))
  { ErrorMessage("Output problem in dumpmat().",1);}
  fclose(out);
}

void readmat(M,filename) matrix *M;char *filename;

{ FILE *in;long i,j,k;char str[200];
  in=fopen(filename,"rb");
  if (in==NULL)
  { sprintf(str,"\n%s not found, nothing read ! ",filename);ErrorMessage(str,1);}
  fread(&i,sizeof(long),1,in);
  fread(&j,sizeof(long),1,in);
  (*M)=initmat(i,j);
  for (k=0L;k<M->r;k++)
  { i=fsaferead((*M).M[k],sizeof(double),M->c,in);
    if (i!=M->c)
    { sprintf(str,"\n problem with %s !!",filename);ErrorMessage(str,1);}
  }
  fclose(in);
}


matrix vecmult(A,x,t) matrix A,x;int t;

/* Optimised multiplication of x by A. where x is a column vector and A a matrix
   Result returns Ax and FREES x BEFORE RETURNING
   - so usual use is x=vecmult(A,x,t)
*/
{ matrix y;
  double *yV,*xV,**AM,*a;
  long Ar,Ac,i,j;
  if (t) y.r=A.c; else y.r=A.r;
  y=initmat(y.r,1L);yV=y.V;AM=A.M;Ar=A.r;Ac=A.c;
  if (t)
  { for (i=0;i<Ac;i++)
    { xV=x.V;
      for (j=0;j<Ar;j++) { *yV += AM[j][i]* *xV;xV++;}
      yV++;
    }
  }
  else
  { for (i=0;i<Ar;i++)
    { xV=x.V;
      for (a=AM[i];a<AM[i]+Ac;a++) { *yV += *a * *xV;xV++;}
      yV++;
    }
  }
  freemat(x);
  return(y);
}

void vmult(matrix *A,matrix *b,matrix *c,int t)

/* fast multiplication of vector by matrix c=Ab if t==0 c=A'b otherwise*/

{ double **AM,*bV,*cV,*p;
  long i,j,cr,br;
  cr=c->r;br=b->r;
  AM=A->M;bV=b->V;cV=c->V;
  if (t) /* then A transposed */
  for (i=0;i<cr;i++)
  { *cV=0.0;
    for (j=0;j<br;j++) *cV += AM[j][i]*bV[j];
    cV++;
  } else
  for (i=0;i<cr;i++)
  { *cV=0.0;
    p=AM[i];
    for (j=0;j<br;j++)
    *cV += p[j]*bV[j];
    cV++;
  }
}

void mcopy(matrix *A,matrix *B)

/* copies A into B */

{ long Ac;
  double *pA,*pB,**AM,**BM;
  if (A->r>B->r||A->c>B->c) ErrorMessage("Target matrix too small in mcopy",1);
  BM=B->M;Ac=A->c;
  for (AM=A->M;AM<A->M+A->r;AM++)
  { pB= *BM;
    for (pA= *AM;pA< *AM+Ac; pA++) *(pB++) = *pA;
    BM++;
  }
}

void matmult(C,A,B,tA,tB) matrix C,A,B;int tA,tB;

/* Puts A*B in C. A will be transposed in this calculation if tA is not zero.
   B will be transposed if tB is not zero */

{ long i,j,k;
  double temp,*p,*p1,*p2,**CM,**AM,**BM;
  AM=A.M;BM=B.M;CM=C.M; /* Saves address calculation involved in C.M */
  if (tA)
  { if (tB)
    { if ((A.r!=B.c)||(A.c!=C.r)||(B.r!=C.c))
      { ErrorMessage("Incompatible matrices in matmult.",1);}
      for (i=0;i<A.c;i++) for (j=0;j<B.r;j++)
      { p2=CM[i]+j;(*p2)=0.0;p=BM[j];
	for (k=0;k<A.r;k++)
	(*p2)+=AM[k][i]*(*p++);
      }
    } else
    { if ((A.r!=B.r)||(A.c!=C.r)||(B.c!=C.c))
      { ErrorMessage("Incompatible matrices in matmult.",1);}
      for (i=0;i<A.c;i++)
      for (p=CM[i];p<(CM[i]+C.c);p++)
      (*p)=0.0;
      for (k=0;k<A.r;k++) for (i=0;i<A.c;i++)
      { temp=AM[k][i];p1=BM[k];
	for (p=CM[i];p<(CM[i]+B.c);p++)
	(*p)+=temp*(*p1++);
      }
    }
  } else
  { if (tB)
    { if ((A.c!=B.c)||(A.r!=C.r)||(B.r!=C.c))
      { ErrorMessage("Incompatible matrices in matmult.",1);}
      for (i=0;i<A.r;i++) for (j=0;j<B.r;j++)
      { p2=CM[i]+j;*p2=0.0;p1=BM[j];
	for (p=AM[i];p<(AM[i]+A.c);p++)
	(*p2)+=(*p)*(*p1++);
      }
    } else
    { if ((A.c!=B.r)||(C.r!=A.r)||(C.c!=B.c))
      { ErrorMessage("Incompatible matrices in matmult.",1);}
      for (i=0;i<A.r;i++) for (p=CM[i];p<(CM[i]+B.c);p++) *p=0.0;
      for (k=0;k<A.c;k++) for (i=0;i<A.r;i++)
      { p1=BM[k];temp=AM[i][k];
	     for (p=CM[i];p<(CM[i]+B.c);p++)
	     (*p)+=temp*(*p1++);
      }
    }
  }
}


void mtest()

/* debugging routine which exercises the matrix storage system */

{ matrix M[1000];
  int i,n=1000,j,k;
  for (i=0;i<n;i++)
  { M[i]=initmat(30L,30L);
    for (j=0;j<30;j++)
    for (k=0;k<30;k++)
    M[i].M[j][k]=(double) k*i;
  }
  for (i=0;i<n;i++) freemat(M[i]);
}

#ifdef SUNC
void multi(va_alist) va_dcl
#else
void multi(int n,matrix C,...)
#endif

/* procedure for multiplying long strings of matrices. For example if we
   want to find Q=A'BAZ then use the command:
   multi(4,Q,A,B,A,Z,1,0,0,0);
   THIS FUNCTION HAS NOT BEEN TRIED USING SUN C */

{ matrix temp0,temp1,*M;
  long r,c;
  int *t,i;
  va_list argptr;

#ifdef SUNC
  matrix C;int n;
  va_start(argptr);
  n=va_arg(argptr,int);
  C=va_arg(argptr,matrix);
#else
  va_start(argptr,C);
#endif
  t=(int *)calloc(n,sizeof(int));
  M=(matrix *)calloc(n,sizeof(matrix)); /* previously no cast for sun C ??! */
  for (i=0;i<n;i++) M[i]=/*(matrix)*/ va_arg(argptr,matrix);
  for (i=0;i<n;i++) t[i]=(int) va_arg(argptr,int);
  if (t[0]) r=M[0].c; else r=M[0].r;
  if (t[1]) c=M[1].r; else c=M[1].c;
  if (n>2) temp0=initmat(r,c);else temp0=C;
  matmult(temp0,M[0],M[1],t[0],t[1]);
  for (i=1;i<n-2;i++)
  { r=temp0.r;
    if (t[i+1]) c=M[i+1].r; else c=M[i+1].c;
    temp1=initmat(r,c);
    matmult(temp1,temp0,M[i+1],0,t[i+1]);
    freemat(temp0);temp0=temp1;
  }
  if (n>2)
  { matmult(C,temp0,M[n-1],0,t[n-1]);
    freemat(temp0);
  }
  free(t);
  free(M);
  va_end(argptr);
}

void mad(C,A,B,mA,mB)
matrix C,A,B;double mA,mB;

/* forms sum C=A+B. address optimization added 27/10/94 */

{ long i;
  double *pA,*pB,*pC;
  if (C.vec)
  { pB=B.V;pA=A.V;
    for (pC=C.V;pC<C.V+C.r*C.c;pC++)
    *pC= (*pA++)*mA+(*pB++)*mB;
  } else
  for (i=0;i<A.r;i++)
  { pA=A.M[i];pB=B.M[i];
    for (pC=C.M[i];pC<(C.M[i]+A.c);pC++)
    *pC= (*pA++)*mA+(*pB++)*mB;
  }
}


void invert(matrix *A)

/* Matrix inversion by Guass-Jordan Elimination with full pivoting.
   See "Numerical Recipes", and Burden and Faires "Numerical Analysis", for basis
   of method (but not actual code). This version written as part of elimination of 
   "Numerical Recipes" routines from my code. Tested against Numerical Recipes code 
   with a variety of random matrices - fine on accuracy and speed. 13/1/2000
 */

{ double **AM,*p,*p1,max,x;
  int *c,*rp,*cp,i,j,k,pr,pc,*d,cj,ck;
  if (A->r!=A->c) ErrorMessage("Attempt to invert() non-square matrix",1);
  c=(int *)calloc((size_t)A->c,sizeof(int)); /* index of columns, used for column pivoting */
  d=(int *)calloc((size_t)A->c,sizeof(int));
  rp=(int *)calloc((size_t)A->c,sizeof(int)); /* row changes */
  cp=(int *)calloc((size_t)A->c,sizeof(int)); /* row changes */
  for (i=0;i<A->c;i++) { c[i]=i;d[i]=i;}
  AM=A->M;            /* saving adress calculations*/
  for (j=0;j<A->c;j++) /* loop through columns to be reduced */
  { max=0.0; 
    for (i=j;i<A->r;i++) /* loop through rows to search for pivot */
    { p=AM[i];
      for (k=j;k<A->c;k++) /* loop through cols to search for pivot */
      { x=p[c[k]];if (fabs(x)>max) { max=fabs(x);pr=i;pc=k;}}
    }
    /* now move pivot to element j,j */
    p=AM[j];AM[j]=AM[pr];AM[pr]=p; /* rows exchanged */
    k=c[j];c[j]=c[pc];c[pc]=k;   /* columns exchanged */
    rp[j]=pr;  /* stores row pivoted with */
    cp[j]=pc;  /* stores column pivoted with */
    cj=c[j]; // save time
    /* Now reduce the column */
    x=AM[j][cj];
    if (x==0.0) ErrorMessage("Singular Matrix passed to invert()",1);
    for (p=AM[j];p<AM[j]+A->c;p++) *p/=x; /* divide row j by pivot element */
    AM[j][cj]=1.0/x;
    for (i=0;i<A->r;i++) /* work down rows eliminating column j */
    { p=AM[i];p1=AM[j];
      if (i!=j)
      { x = -p[cj]; /* multiplier for this row */
        for (k=0;k<j;k++) /* work through columns of the inverse matrix */
        { ck=c[k];p[ck]+=x*p1[ck];}
        p[cj]=x*p1[cj]; /* new column for inverse (entries in A implicitly zeroed except jj) */
        for (k=j+1;k<A->c;k++)   /* cols of A */
        { ck=c[k];p[ck]+=x*p1[ck];}
      }
    }
  } 
 
  for (i=A->r-1;i>=0;i--) //work down through column re-ordering 
  { if (cp[i]!=i)
    { p=AM[i];AM[i]=AM[cp[i]];AM[cp[i]]=p; // row exchange
    }
  }
  
  for (j=0;j<A->c-1;j++) // implement column exchange
  if (c[j]!=j)
  { if (c[j]<j) k=c[c[j]]; else k=c[j]; 
    for (i=0;i<A->r;i++)
    { p=AM[i];x=p[j];p[j]=p[k];p[k]=x;}  
    d[k]=d[j];d[j]=c[j];
    c[d[k]]=k;
  } 
  
  for (i=A->r-1;i>=0;i--) // column exchange implied by row re-ordering
  if (rp[i]!=i)
  { for (k=0;k<A->r;k++) 
    { p=AM[k];x=p[i];p[i]=p[rp[i]];p[rp[i]]=x;} // column exchange 
  }
   
  free(c);free(rp);free(cp);free(d);
}




double triTrInvLL(matrix *l0,matrix *l1)

/* this routine finds the trace of inv(LL') where L is a bi-diagonal lower
   triangular choleski factor, with leading diagonal l0 and leading sub diagonal
   l1. */

{ double trC,x,z,*l1V,*l0V;
  long i,l0r;
  l0V=l0->V;l1V=l1->V;l0r=l0->r;
  x=l0V[l0r-1];z=1.0/(x*x);trC=z;
  for (i=l0r-2;i>=0;i--)
  { x=l1V[i];z=(1.0+x*x*z);x=l0V[i];
    z/=x*x;trC+=z;
  }
  return(trC);
}

void bicholeskisolve(matrix *A,matrix *B,matrix *l0,matrix *l1)

/* solves LL'A=B where L is a matrix with leading diagonal l0 and leading
   sub-diagonal l1. The algorithm is O(n^2) and hence much better than
   performing the same calculation by inversion of L (since inverse of L is
   fully lower triangular, rather than banded) */

{ long i,j;
  double *pA1,*pA,*pB,lv0,lv1;
  /* solve LC=B (C actually stored in A) */
  pA=A->M[0];pB=B->M[0];lv0=l0->V[0];
  for (i=0;i<A->c;i++) pA[i]=pB[i]/lv0;
  for (i=1;i<A->r;i++)
  { lv0=l0->V[i];lv1=l1->V[i-1];pA1=pA;pA=A->M[i];pB=B->M[i];
    for (j=0;j<A->c;j++)
    pA[j]=(pB[j]-lv1*pA1[j])/lv0;
  }

  /* now solve L'A=C */
  pA=A->M[A->r-1];
  lv0=l0->V[l0->r-1];
  for (i=0;i<A->c;i++) pA[i]/=lv0;
  for (i=A->r-2;i>=0;i--)
  { pA1=pA;pA=A->M[i];
    lv0=l0->V[i];lv1=l1->V[i];
    for (j=0;j<A->c;j++)
    pA[j]=(pA[j]-lv1*pA1[j])/lv0;
  }
}

void tricholeski(matrix *T,matrix *l0,matrix *l1)

/* Routine for choleski decomposition of the tridiagonal matrix T. If L is the
   matrix with leading diagonal in vector l0 and leading subdiagonal in l1, then
   T=LL'. The routine is O(n). Note that not only is multiplication of a matrix
   by L 0(n^2), but formation of A=inv(L)B can be done in O(n^2) by solution
   of LA=B. Note also that the trace of the inverse of a tri-diagonal matrix
   can be found cheaply using Elden's trick - see p137 of Wahba.
   20/11/99: Now has steps to deal with +ve semi definite matrices by a slight 
   modification of the choleski decomposition. The modification zeros 
   elements of the decomposition, as needed.

   The zeroing steps are valid for +ve semi-definite (non-negative definite)
   matrices (only), for the following reasons:
   0. a +ve semi definite matrix that is not +ve definite, must have zero eigenvalues
      as is easily seen by spectral decomposition.
   1. A properly tri-diagonal symmetric rank m (sub)matrix  has m or m+1 non-zero 
      rows and columns. This can be seen by counting up the number of possible 
      independent rows and columns of an m+1 by m+1 properly tri-diagonal matrix.
   2. So a +ve semi-definite properly tridiagonal (sub) matrix has at most 1 
      non-independent row/col, i.e. at most one zero eigenvalue.
   3. When present, this non-independence  leads to a zero on the final element of 
      the leading diagonal of the choleski decomposition of a properly tridiagonal +ve
      semi-definite (sub)matrix (I thought I had a proof of this, but am now not sure!). 
   4. Zeroing outside properly tridiagonal sections of the matrix is clearly ok.
   Note that these arguments only hold for +ve semi-definite tri-diagonal matrices.
   Merely being symmetric won't do, and neither will merely being +ve semi-definite!

   NOTE: that this routine will not detect none +ve semi-definite matrices, the only
         way to do that is to check whether the decomposition actually works (it won't 
         for a matrix that isn't +ve semi-definite)
 */

{ double **TM,*l1V,*l0V,z=1.0;
  long k1,k,tr1;
  TM=T->M;l0V=l0->V;l1V=l1->V;
  l0V[0]=sqrt(TM[0][0]);tr1=T->r-1;
  for (k=1;k<T->r;k++)
  { k1=k-1;
    if (z>0.0) l1V[k1]=TM[k][k1]/l0V[k1];   // no problem
    else l1V[k1]=0.0; // assume TM[k][k1]=0, so no problem
    z=l1V[k1];z=TM[k][k]-z*z;
    if (z>0.0) l0V[k]=sqrt(z);
    else 
    l0V[k]=0.0;
  }
}

void choleski(A,L,invert,invout) matrix A,L;int invert,invout;
/* version that halts execution on failure */
{ if (!chol(A,L,invert,invout))
  ErrorMessage("Not a +ve def. matrix in choleski().",1);
}

int chol(A,L,invert,invout) matrix A,L;int invert,invout;

/* Routine to perform inversion and Choleski decomposition A=LL'  where L is */
/* LOWER triangular (zero elements are to the top right).Inversion is       */
/* performed only if invert is not zero. If invout is non-zero then A will  */
/* be returned containing the inverse of A passed to the routine, otherwise */
/* L will contain the inverse of choleski decomposition (so that inverse of */
/* A is L'L) but A will be unchanged. If invert is zero invout is ignored.  */
/* returns 1 for a +ve def. matrix: 0 otherwise....                         */ 

{ long i,j,k;
  double sigmaL,*p,*p1,temp,**AM,**LM,**invM,z;
  matrix inv;
  AM=A.M;LM=L.M; /* A.M replaced by AM and L.M replaced by LM to save address calc. */
  for (i=0;i<L.r;i++) for (p=LM[i];p<(LM[i]+L.c);p++) *p=0.0;
  if (AM[0][0]<=0)
  return(0);
  LM[0][0]=sqrt(AM[0][0]);
  for (j=1;j<A.r;j++) LM[j][0]=AM[j][0]/LM[0][0];
  for (i=1;i<(A.r-1);i++)
  { sigmaL=0.0;
    for (p=LM[i];p<(LM[i]+i);p++) sigmaL+=(*p)*(*p);
    z=AM[i][i]-sigmaL;
    if (z<=0.0)
    return(0);
    else LM[i][i]=sqrt(z);
    for (j=i+1;j<A.r;j++)
    { sigmaL=0.0;p1=LM[i];
      for (p=LM[j];p<(LM[j]+i);p++) sigmaL+=(*p)*(*p1++);
      LM[j][i]=(AM[j][i]-sigmaL)/LM[i][i];
    }
  }
  sigmaL=0.0;j=A.r-1;
  for (p=LM[j];p<(LM[j]+j);p++)
  { sigmaL+=(*p)*(*p);
    j=j;
  }
  z=AM[j][j]-sigmaL;
  if (z<=0)
  return(0);
  LM[j][j]=sqrt(z);
  /* Inverting L (it stays lower triangular)*/
  if (invert)
  { inv=initmat(A.r,A.c);invM=inv.M;
    for (i=0;i<A.r;i++) invM[i][i]=1.0;
    for (i=0;i<A.r;i++)
    { temp=LM[i][i];
      for (p=invM[i];p<=(invM[i]+i);p++) (*p)/=temp;
      for (j=i+1;j<A.r;j++)    /* original code had rows in inner loop */
      { temp=LM[j][i];p1=invM[j];
	     for (p=invM[i];p<=(invM[i]+i);p++)
	     (*p1++) -= (*p)*temp;
      }
    }
    for (i=0;i<inv.r;i++)
    { p1=invM[i];
      for (p=LM[i];p<=(LM[i]+i);p++) *p = *p1++;
    }
    /* Getting the Inverse of A */
    if (invout)
    { for (i=0;i<A.r;i++) for (p=AM[i];p<(AM[i]+A.r);p++) *p = 0.0;
      for (i=0;i<A.r;i++) for (k=i;k<A.r;k++)
      { temp=invM[k][i];p1=AM[i];
        for (p=invM[k];p<(invM[k]+A.r);p++) (*p1++) += temp * (*p);
      }
    }
    freemat(inv);
  }
  return(1);
}





matrix choleskiupdate(L,a) matrix L,a;

/* produces the modified choleski decomposition of the matrix A' constructed
   by adding vector a as the last row and column of matrix A                */


{ long i,Lr;
  double rr,*p,*p1,**LM,**RM,*aV;
  matrix R;
  Lr=L.r;
  R=initmat(Lr+1,Lr+1);
  LM=L.M;RM=R.M;aV=a.V;
  for (i=0;i<Lr;i++)
  { p1=LM[i];
    for (p=RM[i];p<=(RM[i]+i);p++) *p = *p1++;
  }
  for (i=0;i<R.c;i++)
  { rr=0.0;p1=RM[Lr];
    for (p=RM[i];p<(RM[i]+i);p++) rr+=(*p)*(*p1++);
    if (i!=Lr) RM[Lr][i]=(aV[i]-rr)/RM[i][i];
    else
    { if (aV[i]-rr<0.0) RM[Lr][i]=2e-15; // ERROR condition!
      else RM[Lr][i]=sqrt(aV[i]-rr);
    }
  }
  freemat(L);
  return(R);
}

void choleskir1ud(L,u,alpha) matrix L,u;double alpha;

/* Finds the choleski factors of LL'+ alpha*uu' where u is a (column) vector,
   using algorithm from Gill, Gulub, Murray and Saunders (1974). This operation
   is O(n^2), and mildly innefficient since the given algorithm assumes that
   the choleski decomposition is LDL' where L is unit lower triangular (1s on
   leading diagonal) and D is diagonal) */

{ double *v,*dV,*pV,**LM,t0,bj,qj,sj,rj2,si,tj;
  int i,j;
  matrix p,d;

  /* convert to LDL' form where L are unit lower triangular */
  d=initmat(u.r,1L);
  dV=d.V;LM=L.M;
  for (i=0;i<u.r;i++)
  { dV[i]=LM[i][i];
    for (j=i;j<u.r;j++)
    { LM[j][i]/=dV[i];
    }
    dV[i]*=dV[i];
  }
  /* solve Lp=u */
  p=initmat(u.r,1L);pV=p.V;
  for (i=0;i<p.r;i++)
  { t0=0.0;
    for (j=0;j<i;j++) t0+=LM[i][j]*pV[j];
    pV[i]=(u.V[i]-t0)/LM[i][i];
  }
  /* Householder based stable method */
  v=u.V;
  sj=0.0;
  for (i=0;i<p.r;i++) sj+=pV[i]*pV[i]/dV[i];
  if (alpha*sj>-1.0)
  si=alpha/(1.0+sqrt(1.0+alpha*sj));
  else
  si=alpha;
  for (j=0;j<u.r;j++)
  { qj=pV[j]*pV[j]/dV[j];
    tj=1.0+si*qj;
    sj -= qj;
    rj2=tj*tj + si*si*sj*qj;
    d.V[j] *=rj2;
    bj=alpha*pV[j]/dV[j];
    alpha/=rj2;
    if (rj2>0.0) rj2=sqrt(rj2);
    else
    rj2=2e-15;
    si*=(1.0+rj2)/(rj2*(tj+rj2));
    for (i=j+1;i<u.r;i++)
    { v[i] -= pV[j]*LM[i][j];
      LM[i][j] += bj*v[i];
    }
  }

  /* choleski based (not so stable) and assumes alpha=1 */
/*  t0=1.0;v=u.V;
  for (j=0;j<u.r;j++)
  { pj=v[j];
    t1=t0+pj*pj/d.V[j];
    dj=d.V[j]*t1/t0;
    bj=pj/(t1*d.V[j]);
    t0=t1;
    for (i=j+1;i<u.r;i++)
    { v[i] -= pj* L.M[i][j];
      L.M[i][j] += bj*v[i];
    }
    d.V[j]=dj;
  } */
  for (i=0;i<u.r;i++)
  { if (d.V[i]<=0.0)
    d.V[i]=2e-15;
    else d.V[i]=sqrt(d.V[i]);
    for (j=i;j<u.r;j++)
    { L.M[j][i]*=d.V[i];
    }
  }
  freemat(d);freemat(p);
}

void choleskisolve(L,z,y) matrix L,z,y;

/*                                                                         */
/* Solves the system LL'z=y for z. L is a lower triangular choleski factor */
/*						                                          		   */

{ long i,j,n;
  matrix x;
  double xsum,zsum,*p,**LM,*zV,*xV,*yV;
  x=initmat(y.r,1L);
  n=y.r;LM=L.M;zV=z.V;xV=x.V;yV=y.V;
  for (i=0;i<n;i++)
  { xsum=0.0;p=LM[i];
    for (j=0;j<i;j++) xsum+=(*p++)*xV[j];
    xV[i]=(yV[i]-xsum)/LM[i][i];
  }
  for (i=(n-1);i>=0;i--)
  { zsum=0.0;
    for (j=(i+1);j<n;j++) zsum+=LM[j][i]*zV[j];
    zV[i]=(xV[i]-zsum)/LM[i][i];
  }
  freemat(x);
}



double variance(a) matrix a;

{ long i,ar;
  double y2=0.0,y=0.0,*aV;
  ar=a.r;aV=a.V;
  for (i=0;i<ar;i++) { y2+=aV[i]*aV[i];y+=aV[i];}
  y=y2/ar-y*y/(ar*ar);
  if (y<0.0)
  { y=0.0;}    /* can get here by rounding ! */
  return(y);
}

double cov(a,b) matrix a,b;

{ long i;
  double y2=0.0,y=0.0,y1=0.0;
  if (a.r*a.c!=b.r*b.c)
  { ErrorMessage("Error in Covariance(a,b) - a,b not same length.",1);}
  for (i=0;i<a.r;i++) { y2+=a.V[i]*b.V[i];y+=a.V[i];y1+=b.V[i];}
  return(y2/a.r-y*y1/(a.r*a.r));
}

double corr(a,b) matrix a,b;

{ return(cov(a,b)/(sqrt(variance(a)*variance(b))));}

double acf(matrix s,int lag)

/* calculates autocorrelation with prescribed lag */

{ double r;
  matrix t;
  if (lag==0) return(1.0);
  s.r-=lag;t=s;t.V+=lag;
  if (s.r>2) r=cov(s,t); else r=0.0;
  return(r);
}

double dot(a,b) matrix a,b;

{ long i,k=0L;double c=0.0,*p,*p1;
  if (a.vec) { p1=b.V;for (p=a.V;p<a.V+a.c*a.r;p++) c+=(*p)*(*p1++);}
  else
  for (i=0;i<a.r;i++) for (p=a.M[i];p<(a.M[i]+a.c);p++)
  { c+=(*p)*b.M[k/b.c][k%b.c];k++;}
  return(c);
}

double mean(a) matrix a;

/* gets mean of a */

{ int i;
  double m=0.0,*aV;
  aV=a.V;
  for (i=0;i<a.r*a.c;i++) m+=aV[i];
  m/=(a.r*a.c);
  return(m);
}

double trace(matrix *A)

{ double **p,tr=0.0;long i;
  p=A->M;
  for (i=0;i<A->r;i++) tr+=p[i][i];
  return(tr);
}

double absdev(a) matrix a;

/* gets mean absolute deviation from mean */

{ int i;
  double m,d=0.0,*aV;
  aV=a.V;
  m=mean(a);
  for (i=0;i<a.r*a.c;i++) d+=fabs(aV[i]-m);
  d/=(a.r*a.c);
  return(d);
}

double enorm(d) matrix d;

/* Euclidian norm of vector d, or rms for matrix */

{ double e=0.0,m=0.0,y,*p;
  long i;
  if (d.vec) for (p=d.V;p<d.V+d.r*d.c;p++) { y=fabs(*p); if (y>m) m=y; }
  else for (i=0;i<d.r;i++) for (p=d.M[i];p<d.M[i]+d.c;p++) 
  { y=fabs(*p);if (y>m) m=y;}// m=max(m,fabs(*p));
  if (!m) return(0.0);
  if (d.vec) for (p=d.V;p<d.V+d.r*d.c;p++)
  { y= *p / m; e+=y*y;} else
  for (i=0;i<d.r;i++) for (p=d.M[i];p<(d.M[i]+d.c);p++)
  { y= *p / m;e+=y*y;}
  e=sqrt(e)*m;
  return(e);
}


double matrixnorm(M) matrix M;

/* This is not the Euclidean norm of a matrix, it is the Frobenius norm
e.g. Watkins D.S. 1991 Fundamentals of matrix computation ,Wiley New York
p. 90.*/

{ double res=0.0;
  long i;
  for (i=0;i<M.r*M.c;i++)
  res+=(M.M[i/M.c][i%M.c]*M.M[i/M.c][i%M.c]);
  res=sqrt(res);
  return(res);
}

double m1norm(M) matrix M;

/* returns ||M||_1
*/
{ double maxsum=0.0,sum;
  long i,j;
  for (j=0;j<M.c;j++)
  { sum=0.0;
    for (i=0;i<M.r;i++) sum+=fabs(M.M[i][j]);
    if (sum>maxsum) maxsum=sum;
  }
  return(maxsum);
}






void householder(u,a,b,t1) matrix *u,a,b;long t1;

/* transforms a to b, iff they are of equal Euclidian length. u is the
   (t1+1) vector such that the full post multiplying householder matrix is
   H' = [ I - vv' ]   where v'  = [u',0] where 0 is a vector of zeroes.   */

{ long i;double v,*aV,*bV,*uV;
  aV=a.V;bV=b.V;uV=u->V;
  u->r=t1+1;
  for (i=0;i<u->r;i++) uV[i]=aV[i]-bV[i];
  v=enorm((*u))/sqrt(2.0);
  for (i=0;i<u->r;i++) uV[i]/=v;
}



void Hmult(C,u) matrix C,u;

/* This routine is for post multiplication by Housholder matrices only */

{ double temp,*p,*p1,*uV,**CuM,**CM;
  long i,j;
  matrix Cu;
  Cu=initmat(C.r,u.c);
  uV=u.V;CuM=Cu.M;CM=C.M;
  for (i=0;i<Cu.r;i++)
  { p1=CuM[i];(*p1)=0.0;p=CM[i];
    for (j=0;j<u.r;j++) (*p1)+=(*p++)*uV[j];
  }
  for (i=0;i<Cu.r;i++)
  { temp=Cu.V[i];p=CM[i];
    for (j=0;j<u.r;j++)
    (*p++) -= temp*uV[j];
  }
  freemat(Cu);
}

void HQmult(C,U,p,t) matrix C,U;int p,t;

/* This routine is for multiplying by householder matrices, stored as a
   series of vectors of the type u defined in routine householder above.
   If ui is ith row of U, then Hi=(I-ui ui') = Hi'. Let Q = H1 H2 H3 ....
   then Q' = .... H3 H2 H1.
   Returned in C: p==0,t==0 => CQ; p==0,t==1 => CQ'; p==1,t==0 => QC;
		  p==1,t==1 => Q'C
   Tested for premultiplication by untransposed matrix (p==1, t==0) other
   combinations untested 15/8/96 (speed up awesome!)
   NOTE that the routine expects C to be compatible with the Hi's - if
   this routine is being used for projection in and out of Null spaces, then
   make sure that C is appropriately packed with zeroes.

   If appropriate zero packing conventions have been used then OrthMult() is
   more efficient....
   */

{ double *u,*CuV,**CM;
  matrix Cu;
  long i,j,k;
  if (p) Cu=initmat(C.c,1L);else Cu=initmat(C.r,1L);
  CuV=Cu.V;CM=C.M;
  if (p)
  { if (t)
    { for (k=0;k<U.r;k++) /* loop through the householder matrices */
      { u=U.M[k];
        for (i=0;i<C.c;i++)
	     { CuV[i]=0.0;
          for (j=0;j<C.r;j++) CuV[i]+=CM[j][i]*u[j];
	     }
	     for (i=0;i<C.r;i++) for (j=0;j<C.c;j++) CM[i][j] -= CuV[j]*u[i];
      }
    }  else
    { for (k=U.r-1;k>=0;k--) /* loop through the householder matrices */
      { u=U.M[k];
	     for (i=0;i<C.c;i++)
	     { CuV[i]=0.0;
	       for (j=0;j<C.r;j++) CuV[i]+=CM[j][i]*u[j];
	     }
	     for (i=0;i<C.r;i++) for (j=0;j<C.c;j++) CM[i][j] -= CuV[j]*u[i];
      }
    }
  } else  /* postmultiplication */
  { if (t)
    { for (k=U.r-1;k>=0;k--) /* loop through the householder matrices */
      { u=U.M[k];
	     for (i=0;i<C.r;i++)
	     { CuV[i]=0.0;
	       for (j=0;j<C.c;j++) CuV[i]+=CM[i][j]*u[j];
	     }
	     for (i=0;i<C.r;i++) for (j=0;j<C.c;j++) CM[i][j] -= CuV[i]*u[j];
      }
      } else
      { for (k=0;k<U.r;k++) /* loop through the householder matrices */
      { u=U.M[k];
	     for (i=0;i<C.r;i++)
	     { CuV[i]=0.0;
	       for (j=0;j<C.c;j++) CuV[i]+=CM[i][j]*u[j];
	     }
	     for (i=0;i<C.r;i++) for (j=0;j<C.c;j++) CM[i][j] -= CuV[i]*u[j];
      }
    }
  }
  freemat(Cu);
}


void QT(Q,A,fullQ) matrix Q,A;int fullQ;

/* Uses householder matrices to perform the factorization of A (nxm),n<=m: */
/*                  AQ=[0,T]  where Tij=0 if i+j<n for T an (nxn) matrix   */
/*                    i.e. T reverse lower triangular - top left is zero   */
/* if fullQ!=0 then Q is formed explicitly (what a waste!), otherwise the  */
/* first A.r rows of Q will be used to store the vectors u used to define  */
/* the householder matrix. These can be used to multiply some other matrix */
/* using the routine HQmult. Revised 13/1/2000 - more efficient, over/under*/
/* flow protected, cancellation error free, but still behaves as before.   */
/* Tested using variety of random matrices.                                */

{ long i,j,Ar,Ac,k;
  double lsq,*p,*p1,**QM,**AM,g,x,m;
  QM=Q.M;AM=A.M;Ar=A.r;Ac=A.c;
  if (fullQ) for (i=0;i<Ac;i++) 
  { p=QM[i];
    for (j=0;j<Ac;j++) if (i==j)
    p[j]=1.0; else p[j]=0.0;
  }
  if (Ar>0)
  { for (i=0;i<Ar;i++)
    { // rotate elements 0 to A.c-i-1 row i of A into element A.c-i-1 of that row 
      p=AM[i];
      m=0.0;for (j=0;j<Ac-i;j++) { x=p[j];x=fabs(x);if (x>m) m=x;} // scale factor
      if (m) for (j=0;j<Ac-i;j++) p[j]/=m; // avoid over/underflow
      lsq=0.0;for (j=0;j<Ac-i;j++) lsq+=p[j]*p[j];
      lsq=sqrt(lsq);
      if (p[Ac-i-1]<0.0) lsq= -lsq;
      p[Ac-i-1]+=lsq;
      if (lsq)
      g=1/(lsq*p[Ac-i-1]); // multiplier for HH rotation (I-g*uu')
      else g=0.0;
      lsq*=m; // Element to end up on A.M[i][A.c-i-1]
      for (j=i+1;j<Ar;j++) // Apply rotation down through the rows of A
      { x=0.0;p1=AM[j];
        for (k=0;k<Ac-i;k++) x+=p[k]*p1[k];
        x*=g;
        for (k=0;k<Ac-i;k++) p1[k] += -x*p[k];
      } 
      if (fullQ)
      for (j=0;j<Q.r;j++) // down through rows of Q
      { x=0.0;p=AM[i];p1=QM[j];
        for (k=0;k<Ac-i;k++) x+=p[k]*p1[k];
        x*=g;
        for (k=0;k<Ac-i;k++) p1[k] += -x*p[k];
      } else
      { g=sqrt(g);
        p=QM[i];p1=AM[i]; // address saving
        for (j=0;j<Ac-i;j++) p[j]=p1[j]*g;
        for (j=Ac-i;j<Ac;j++) p[j]=0.0;  
      }
      AM[i][Ac-i-1]=-lsq;
      for (j=0;j<Ac-i-1;j++) AM[i][j]=0.0;
    }
  }
}

void InvertTriangular(matrix *R)

/* inverts a triangular matrix - returns result in R */

{ long i,j,k,n;
  double Lii,s;
  n=R->r;
  for (i=n-1;i>=0;i--)
  { Lii=1.0/R->M[i][i];
    for (j=n-1;j>i;j--)
    { s=0.0;
      for (k=i+1;k<=j;k++) s+=R->M[i][k]*R->M[k][j];
      R->M[i][j] = -s/R->M[i][i];
    }
    R->M[i][i]=Lii;
  }
}

void OrthoMult(matrix *Q,matrix *A,int off,int rows,int t,int pre,int o_pre)

/* The first `rows' of Q are vectors for a householder transformation. The ith
   vector starts with i+off zero elements (i starts at 0). The vectors are
   stored in the order that they should be applied. If o_pre==1 then they
   originally pre-multiplied, otherwise they originally post multiplied.
   if t==1 then the transform is transposed. If pre==1 then it is applied from
   the left.
   Each householder transform has the same form: (I-uu') (treating u as a column
   vector).
   The transformation is applied to A.
*/

{ double au,*u,*a,**AtM,**AM,**QM;
  long i,j,k,Ar,Qc,kk;
  matrix At;
  if (o_pre) t=1-t; /* default assumption is that creation was for post mult. */
  if (pre) /* use fact that QA=(A'Q')' and Q'A=(A'Q)' */
  { At=initmat(A->c,A->r);
    AM=A->M;AtM=At.M;
    for (i=0;i<A->r;i++) for (j=0;j<A->c;j++) AtM[j][i]=AM[i][j];
    t=1-t;
  } else At=*A;
  AM=At.M;QM=Q->M;Ar=At.r;Qc=Q->c;
  for (kk=0;kk<rows;kk++)
  { if (t) k=rows-1-kk; else k=kk;
    u=QM[k];
    for (i=0;i<Ar;i++)
    { a=AM[i];au=0.0;
      for (j=off+k;j<Qc;j++) au+=a[j]*u[j];
      for (j=off+k;j<Qc;j++) a[j] -= au*u[j];
    }
  }
  if (pre)
  { AM=A->M;
    for (i=0;i<At.r;i++) for (j=0;j<At.c;j++) AM[j][i]=AtM[i][j];
    freemat(At);
  }
}


void Rsolv(matrix *R,matrix *p,matrix *y)

/* Solves Rp=y for p when R is upper triangular - i.e. lower left 0.
   dimensions of p and y are not checked
*/

{ int i,j;
  double x,*pV,*yV,*RMi;
  pV=p->V;yV=y->V;
  for (i=R->r-1;i>=0;i--)
  { RMi=R->M[i];
    x=0.0;for (j=i+1;j<R->r;j++) x+=RMi[j]*pV[j];
    pV[i]=(yV[i]-x)/RMi[i];
  }
}


int QR(matrix *Q,matrix *R)

/* Does a QR factorisation of the matrix supplied in R. In Q the householder
   vectors are supplied to perform the transformation
   QR(in) -> R(out)
   R(out) is upper triangular (elements are 0 below leading diagonal).
   If Q->r is none zero then the vectors u are stored in successive rows of
   Q. The u vectors make up Q as a series of (stable) householder transformations.
   (I-uu'). The transformations are to be applied from the left in row order.
   The first i elements of the ith u are zero (i starting at zero).
   If A is the matrix input in R then QA=R, so that A=Q'R.
   Q can be used with OrthoMult(). 
   Under/overflow avoidance added 13/1/2000 along with more efficient calculation
   of length of u (modifications tested).
   
*/

{ long i,j,k,n,Rr;
  double *u,t,z,**RM,*p,m;
  RM=R->M;Rr=R->r;
  if (Rr<R->c) n=Rr; else n=R->c;
  u=(double *)calloc((size_t)Rr,sizeof(double));
  for (k=0;k<n;k++)
  { m=0.0;for (i=k;i<Rr;i++) { z=RM[i][k];z=fabs(z);if (z>m) m=z;}
    if (m) for (i=k;i<Rr;i++) RM[i][k]/=m; // avoid over/underflow problems
    t=0.0;for (i=k;i<Rr;i++) { z=RM[i][k];t+=z*z;} /* get euclidean length of column */
    if (RM[k][k]>0.0) t = -sqrt(t);else t= sqrt(t);  /* value of new RM[k][k] (stable) */
    for (i=k+1;i<Rr;i++) { u[i]=RM[i][k];RM[i][k]=0.0;}
    z=RM[k][k]; 
    u[k]=RM[k][k]-t;RM[k][k]=t*m;
    t=t*t;t+=u[k]*u[k]-z*z; // efficient t calculation
    // t=0.0;for (p=u+k;p<u+Rr;p++) t+= *p * *p; - old inefficient calculation
    t=sqrt(t/2);
    if (t==0.0) {free(u);return(0);} /* singular matrix */
    for (p=u+k;p<u+Rr;p++) *p /= t;
    for (j=k+1;j<R->c;j++)
    { t=0.0;for (i=k;i<Rr;i++) t+=u[i]*RM[i][j];
      for (i=k;i<Rr;i++) RM[i][j]-=u[i]*t;
    }
    if (Q->r) /* store vectors u for making Q */
    { p=Q->M[k];
      for (i=k;i<Rr;i++) p[i]=u[i];
    }
  }
  free(u);
  return(1);
}


void interchange(matrix *M,long i,long j,int col)

/* interchanges rows i and j of M if col==0 otherwise cols i and j */

{ int k;
  double t,**MM;
  MM=M->M;
  if (col)
  for (k=0;k<M->r;k++) { t=MM[k][i];MM[k][i]=MM[k][j];MM[k][j]=t;}
  else
  for (k=0;k<M->c;k++) { t=MM[i][k];MM[i][k]=MM[j][k];MM[j][k]=t;}
}

void UTU(matrix *T,matrix *U)

/* does orthogonal tridiagonalisation of the symmetric matrix supplied in T;
   U is returned with successive householder vectors in successive
   rows of U. The first i+1 elements of the ith row will be zero (i starts at 0).
   There are only T->r - 2 non-zero rows. The transformations must be applied
   in order from the right.
   Recall that a householder transformation to take a->b is constructed as
   follows: u=a-b;  u=u/sqrt(u'u/2);  H=(I-uu'); then Ha=b and a'H=b'
   (of course to form Ha...  form u'a; form u(u'a); form a-u(u'a); never
    form H first!).
   If A is the input matrix then U'AU=T => A=UTU'
   Underflow and overflow protection added 13/1/2000, and lt 
   efficiency improved. Improved code tested with variety of 
   random matrices. (Householder rotations are stable.)
   OrthoMult() works with U storage convention used here.
*/

{ long i,j,k;
  double *u,*t,lt,x,m;
  for (i=0;i<T->r-2;i++)
  { u=U->M[i];t=T->M[i];lt=0.0;
    m=0.0;for (j=i+1;j<T->c;j++) { x=fabs(t[j]); if (m<x) m=x;} // find max_j(|t_j|) for scaling
    if (m) for (j=i+1;j<T->c;j++) t[j]/=m; // avoid over/underflow
    for (j=i+1;j<T->c;j++) lt+=t[j]*t[j];
    if (t[i+1]>0.0) lt= -sqrt(lt);else lt=sqrt(lt);  /* ensures stability (by maximising element i+1 of u) */
    x=t[i+1]; // stored for altering lt efficiently
    u[i+1]=lt-t[i+1];T->M[i+1][i]=t[i+1]=lt*m;
    lt*=lt;lt+= -x*x+u[i+1]*u[i+1];
    for (j=i+2;j<T->c;j++)
    { u[j]= -t[j];T->M[j][i]=t[j]=0.0;}
    if (lt>0.0) /* only do this if u is non-zero */
    { lt=sqrt(0.5*lt);
      for (j=i+1;j<T->c;j++) u[j]/=lt;
    }
    for (j=i+1;j<T->c;j++) /* apply rotations to remaining rows */
    { t=T->M[j];lt=0.0;
      for (k=i+1;k<T->c;k++) lt+=u[k]*t[k];
      for (k=i+1;k<T->c;k++) t[k] -= u[k]*lt;
    }
    /* Apply rotations from the left */
    for (j=i+1;j<T->c;j++)
    { lt=0.0;
      for (k=i+1;k<T->c;k++) lt+=u[k]*T->M[k][j];
      for (k=i+1;k<T->c;k++) T->M[k][j] -= u[k]*lt;
    }
  }
}




void root(matrix *M,matrix *C,double tol)

/* produces a square root of non-negative definite M, by obtaining M=UTU',
   then getting the choleski decomposition of the non-zero part of T. T=LL'
   so C=UL and M=CC'.... C will have as few columns as possible. C is
   initialised in this routine.

   Upgraded 20/11/99: Previous version assumed that zero rows and columns only
   occured at the end of T. This is no longer the case. tricholeski() has been
   modified to deal with non-negative definite T (although this may still be
   weakest link). If L contains a column of zeros then this column is ommitted
   from C altogether. zero is judged relative to tol multiplied by the maximum
   element on the leading diagonal of L if tol>0, otherwise zero is any number 
   that leads to no change in the maximum element when added to it. Algorithm 
   appears to be substantially better than svdroot() (and should be much quicker).

   Commented out code is the old code, left in in case of future difficulties:
   when it is removed some further tidying could be done.

   16/1/2000: tested with variety of random rank deficient matrices, and theoretical
   basis re-checked more carefully - see tricholeski() comments.

*/

{ matrix T,U,u0,u1;
  long i,j,k,rows;
  int fswap=0,ok;
  double max,uc,*u,x,m;
  T=initmat(M->r,M->c);
  U=initmat(M->r,M->c);
  for (i=0;i<T.r;i++) for (j=0;j<T.c;j++) T.M[i][j]=M->M[i][j];
  UTU(&T,&U);
  // make absolutely symmetric
  for (i=0;i<T.r-1;i++) T.M[i][i+1]=T.M[i+1][i]=(T.M[i+1][i]+T.M[i][i+1])*0.5;
  rows=T.r;
  u0=initmat(T.r,1L);u1=initmat(T.r-1,1L);
  u0.r=T.c=T.r=rows;u1.r=rows-1;
  tricholeski(&T,&u0,&u1);
  /* now check result, since I'm not quite happy with step 4 of theoretical justification
     of treatment of +ve semi-definiteness in tricholeski() */
  max=0.0;for (i=0;i<u0.r;i++) if (fabs(T.M[i][i])>max) max=fabs(T.M[i][i]);
  ok=1;x=u0.V[0]*u0.V[0]-T.M[0][0];m=0.0;
  if (x>m) m=x;
  for (i=1;i<T.r;i++) 
  { x=u1.V[i-1]*u0.V[i-1]-T.M[i][i-1];x=fabs(x);
    if (x>m) { m=x;k=i;}
    x=u1.V[i-1]*u1.V[i-1]+u0.V[i]*u0.V[i]-T.M[i][i];x=fabs(x);
    if (x>m) { m=x;k=i;}
  }
  if (m>1e-14*max) ok=0;
  if (!ok)
  { ErrorMessage("Using svd to find root of penalty!",0); 
    (*C)=svdroot(*M,tol);
    freemat(U);freemat(T);freemat(u0);freemat(u1);
    return; 
  }
  freemat(T);
  T=initmat(U.r,u0.r);  
  
  /* now apply householder rotations from the left to obtain C=UL */
  for (i=0;i<u0.r;i++) // row counter
  { T.M[i][i]=u0.V[i];
    if (i<u0.r-1) T.M[i+1][i]=u1.V[i];
  }
  for (i=U.r-3;i>=0;i--)
  { u=U.M[i]; /* first i+1 elements of u are zero */
    for (j=0;j<T.c;j++)
    { uc=0.0;for (k=i+1;k<U.c;k++) uc += u[k]*T.M[k][j];
      for (k=i+1;k<U.c;k++) T.M[k][j] -= u[k]*uc;
    }
  }
  // now remove zero columns
  *C=initmat(U.r,u0.r);
  k=0;
  for (j=0;j<T.c;j++)
  { ok=0;
    for (i=0;i<T.r;i++)
    if (tol<=0.0) 
    { if ((T.M[i][j]+max!=max)||(T.M[i][j]+max!=max)) {ok=1;break;}}
    else
    { if ((fabs(T.M[i][j])>tol*max)||(fabs(T.M[i][j])>tol*max)) {ok=1;break;}}
    if (ok) // then include this column
    { for (i=0;i<C->r;i++) C->M[i][k]=T.M[i][j]; 
      k++;
    } 
  }
  C->c=k;
  if (fswap)
  { interchange(C,1L,0L,0);}
  freemat(T);freemat(U);freemat(u0);freemat(u1);
}


void bidiag(matrix *A,matrix *wl,matrix *ws,matrix *V)


/* This routine bi-diagonalizes A using Housholder rotations applied from 
   left and right. wl and ws are vectors of dimension A.c and A.c-1 respectively.
   V is an orthogonal A.c by A.c matrix. Let W be the matrix with wl as leading 
   diagonal and ws as leading superdiagonal, then: A = UWV' where U is 
   orthogonal and output in A. The routine's main use is as the first stage in 
   a singular value decomposition of A. 
   The Orthogonal matrices are composed of Householder rotations applied left, 
   right, left, right, left, right etc. The left rotations (reflectors) zero 
   elements of A in columns below the leading diagonal starting from the left. 
   The right reflectors zero elements of A in rows left of the first super diagonal
   starting from the top. Reflectors are calculated as described on p149 of 
   Watkins (1991) Fundamentals of Matrix Computations, Wiley (but note that 
   \gamma<-1/\sigma x_1 should read \gamma<-(\sigma x_1 )!!).  
   Reflectors are of the form H=(I-g*uu'), where u is a vector and g a scalar. 

   This routine has been tested on a variety of random matrices of differing dimensions
   as well as on strictly singular matrices. The tests checked that A = UWV'.

   Most important address optimization has been performed.

   9/1/2000
	 
*/

{ double m,s,g,temp,**AM,**VM,*p,*p1;
  int i,j,k,nv,nu;
  nv=0; // counts up number of v_i's
  AM=A->M;VM=V->M;
  for (i=0;i<A->c;i++)
  { wl->V[i]=0.0;if (i<A->c-1) ws->V[i]=0.0;
    if (i<A->r) /* zero subdiagonal column i */
	{ m=0.0;for (j=i;j<A->r;j++) { s=fabs(AM[j][i]); if (s>m) m=s;} // max of column for scaling 
      if (m==0.0) g=0.0; 
	  else /* work out reflector elements */
	  { s=0.0;for (j=i;j<A->r;j++) { AM[j][i]/=m;s+=AM[j][i]*AM[j][i];} // scale reflector etc. 
	    s=sqrt(s);
	    if (AM[i][i]<0.0) s = -s; // avoid cancellation error
        AM[i][i]+=s;
	    g=1/(AM[i][i]*s);
	    s*=m;
	  } // Now u is stored in rows i to A.r of column i of A
	  wl->V[i] = -s;
	  VM[i][i] = g; // temporary storage for g, for use in later assembly of U 
	  // must apply reflector to remaining columns
      for (j=i+1;j<A->c;j++)
      { s=0.0;for (k=i;k<A->r;k++) s+=AM[k][i]*AM[k][j];
	    s*=g;for (k=i;k<A->r;k++) AM[k][j] += -s*AM[k][i];
      }
    } 

	/* zero elements i+2 to A.c of row i ..... */
    if ((i<A->r) && (i<A->c-1))
    { m=0.0;//for (j=i+1;j<A->c;j++) { s=fabs(AM[i][j]); if (s>m) m=s;} // max for scaling
      for (p=AM[i]+i+1;p<AM[i]+A->c;p++) { s=fabs(*p);if (s>m) m=s;} // max for scaling
      if (m==0.0) g=0.0;
      else
      { s=0.0;//for (j=i+1;j<A->c;j++) { AM[i][j]/=m;s+=AM[i][j]*AM[i][j];}
        for (p=AM[i]+i+1;p<AM[i]+A->c;p++) { *p/=m;s+=(*p)*(*p);}
        s=sqrt(s);
        if (AM[i][i+1]<0.0) s = -s; // avoid cancellation error
        AM[i][i+1] += s;
        g=1.0/(AM[i][i+1]*s);
        s*=m; 
      }
      ws->V[i] = -s;
      VM[i][i+1]=g; // temporary storage
      // Now apply reflector to remaining rows
      for (j=i+1;j<A->r;j++)
      { s=0.0;//for (k=i+1;k<A->c;k++) s+=AM[i][k]*AM[j][k];
        p1=AM[j]+i+1;for (p=AM[i]+i+1;p<AM[i]+A->c;p++) { s+=(*p)*(*p1);p1++;}
        s*=g;//for (k=i+1;k<A->c;k++) AM[j][k] += -s*AM[i][k];
        p1=AM[j]+i+1;for (p=AM[i]+i+1;p<AM[i]+A->c;p++) { *p1 += -s*(*p);p1++;}
      } 
      nv++; // number of v_i's
    }  
  } 
  /* At this point wl and ws are complete, but U and V are stored in A as reflector 
     vectors, with associated g values stored on the leading diagonal and leading
     superdiagonal of V. Now form U and V. 
     U is the first A.c columns of  U_1 U_2 U_3 .... i.e. U_1 U_2 U_3 ... [I,0]'(same 
     dim as A) where U_i = (I - g_i u_i u_i'): g_i is in V->M[i][i] and u_i is zero up to 
     element i, with the remaining elements stored in rows i to A.r of column i of A.
     V= V_1 V_2 V_3 ... where V_i = (I-d_i v_i v_i') where d_i=V->M[i][i+1]. 
     v_i is zero until element i+1, with remaining elements stored in columns i+1 to A.c 
     of row i of A. */
  /* first form V in order to free up space in A */
  // initialize rows nv to A->c of V
  nu=A->c; if (A->r<nu) nu=A->r; // number of U_i's
  for (i=nv+1;i<A->c;i++) 
  for (p=VM[i];p<VM[i]+A->c;p++) *p=0.0;
  for (i=A->c-1;i>nv;i--) { if (i<nu) AM[i-1][i]=VM[i][i];VM[i][i]=1.0;}
  for (i=nv-1;i>=0;i--) // working down through the V_i's
  { temp=VM[i+1][i+1];
    // for (j=0;j<A->c;j++) VM[i+1][j]=0.0;
    for (p=VM[i+1];p<VM[i+1]+A->c;p++) *p=0.0;
    VM[i+1][i+1]=1.0; // initialize row of V
    for (j=A->c-1;j>i;j--) // columns affected by V_i
    { s=0.0;p=AM[i]+i+1;for (k=i+1;k<A->c;k++) { s+=VM[k][j]*(*p);p++;}
      s*=VM[i][i+1];
      p=AM[i]+i+1;for (k=i+1;k<A->c;k++) { VM[k][j] += -s*(*p);p++;}
    }
    AM[i][i+1]=temp; // temporary storage for g_i's 
  }        
  /* Now all but first row and column of V are formed, but V->M[0][0] still contains
     g_0, while g_i is in AM[i-1][i] otherwise, so form U now and then finish off V */
  
  for (i=nu-1;i>=0;i--) // work down through the u_i's
  { if (i>0) g=AM[i-1][i]; else g=VM[0][0];
    for (j=0;j<i;j++) AM[j][i]=0.0; // zeroing column above the diagonal
    for (j=A->c-1;j>i;j--) // columns above i are affected
    { s=0.0;for (k=i;k<A->r;k++) s+= AM[k][i]*AM[k][j];
      s*=g;
      for (k=i;k<A->r;k++) AM[k][j] += -s*AM[k][i]; 
    } 
    // as is column i itself....
    for (j=A->r-1;j>i;j--) AM[j][i]*= -g*AM[i][i];
    AM[i][i] = 1 - g*AM[i][i]*AM[i][i]; 
  }
  // now finish off V
  p=VM[0];for (i=0;i<A->c;i++) { *p=VM[i][0]=0.0;p++;}
  VM[0][0]=1.0; 
}   


svdcheck(matrix *U,matrix *w,matrix *ws,matrix *wl,matrix *V)

/* this is a debugging routine for checking that svd() has not messed up the decomposition,
   by checking that the bidiagonal decomposition is still ok. */
{ matrix W,A;
  int i;
  W=initmat(w->r,w->r);
  for (i=0;i<w->r-1;i++) 
  { W.M[i][i]=w->V[i];
    W.M[i][i+1]=ws->V[i];
    W.M[i+1][i]=wl->V[i];
  }
  W.M[i][i]=w->V[i];
  A=initmat(U->r,U->c);
  multi(3,A,*U,W,*V,0,0,1);
  printmat(W," %7.3g");
  printmat(A," %7.3g");
  freemat(A);freemat(W);
  getc(stdin);
}


   
void svd(matrix *A, matrix *w, matrix *V)


/* This routine produces a singular value decomposition of A. On exit V will be 
   an A.c by A.c orthogonal matrix, w will be a vector of A.c singular values 
   and A will contain U of the same dimension as A such that U'U=I. If W is
   the diagonal matrix with w as its leading diagonal then: 
                        A=UWV'   - the singluar value decomposition.
   This routine is based on:
   Watkins (1991) Fundamentals of Matrix Computations, Wiley. (see section 7.2)
   The algorithm has 2 steps:
   1. Bi-diagonalise A using reflectors (Householder transformations) from left 
      and right - this is achieved by routine bidiag(), above.
   2. Find singular values of Bi-diagonal matrix.
   Because the bi-diagonal matrix may not always be properly bidiagonal step 2, is
   a little complicated....

   i) Deflate the problem if possible, which may involve zeroing an element of the
      super-diagonal.
   ii) Check whether (deflated) bi-diagonal matrix can be partioned, if so find 
       start of final partition (again may need to zero an element on the super 
       diagonal)
   iii) Apply iteration of implicit QR algorithm (p405 Watkins) to the sub matrix 
        identified above.
   iv) Return to (i) assuming that there are singular values left to find.

   Note that the Givens Rotators used here are calculated as follows to avoid rounding problems:

   assume xj to be zeroed into xi:
   m = max(fabs(xi),fabs(xj));
   xi/=m;xj/=m
   r=sqrt(xi*xi+xj*xj);
   c=xi/r;s=xj/r;
   xi=m*r;xj=0.0;

   (c and s can obviously be applied to other vectors without needing to know m.)

   See page 271 of Watkins, for suggestion of how to test for zeroes.

   Most important address optimization has been done (stopped when it was as efficient as
   Numerical Recipes code). (Commented out code is pre-optimization, left in for readability)

   Tested against NR routine for a variety of random matrices and matrices that are rank 
   deficient in various ways.

   Check for m>0.0 added 15/5/00 - otherwise division by zero is possible, leading to failure
   of routine!

*/

{ double wnorm=0.0,x,y,s,c,m,r,a,b,sig,**VM,**UM,*wV,*wsV,*p1,*p2;
  matrix ws,*U; 
  int finished=0,end,start,i,j,k,maxreps=100;
  ws=initmat(w->r-1,1L);
  bidiag(A,w,&ws,V); // bi-diagonalize A....
  U=A;
  VM=V->M;UM=U->M;wV=w->V;wsV=ws.V;
  for (i=0;i<ws.r;i++) // get something against which to judge zero
  { x=fabs(wV[i]);y=fabs(wsV[i]);if (x<y) x=y;if (wnorm<x) wnorm=x;}
  end=w->r-1;
  while (!finished)
  { for (k=0;k<maxreps;k++) // QR iteration loop
    { if (wV[end]+wnorm==wnorm) // zero singular value - can deflate
      { if (wsV[end-1]+wnorm!=wnorm) // need to zero wsV[end-1] before deflating
        { /* Series of rotators (Givens rotations from right) zero this element */
          y=wsV[end-1];wsV[end-1]=0.0;
          for (i=end-1;i>=0;i--) // work out sequence of rotations
          { m=fabs(y);x=fabs(wV[i]); if (x>m) m=x;
            x=wV[i];
            if (m>0.0) 
            { y/=m;x/=m; // now rotate y into x
              r=sqrt(y*y+x*x);
              c=x/r;s=y/r;
            } else {r=0.0;c=1.0;s=0.0;}
            wV[i]=r*m; // rotation zeros y (implicitly)
            if (i>0) // propagate the problem element!
            { y= -wsV[i-1]*s;
              wsV[i-1]*=c;
            } 
            /* Need to update V as well V -> V G where G is the rotation just applied.... */ 
            for (j=0;j<V->r;j++) // work down the rows
            { p2=VM[j]+end;p1=VM[j]+i;x=*p1; //x=VM[j][i];
              //VM[j][i]=c*x+s*VM[j][end];
              *p1=c*x+s*(*p2);
              //VM[j][end]*=c;VM[j][end] += -s*x;
              *p2 *= c; *p2 += -s*x;
            }
          }
        }
        end--; //  
       // Check here for termination ..... 
        if (end<=0) finished=1;
        break; // successfully deflated, so start new QR iteration cycle or finish
      } else 
      //if (fabs(wsV[end-1])<=1e-15*wnorm) // condition slacker than below
      if  (wsV[end-1]+wnorm==wnorm) //too restrictive??/* wV[end] is a singular value => deflate */
      { end--;
        if (end==0) finished=1; // all elements of ws are zeroed so we're done
        break; // deflated so start new QR cycle or finish
      } else // no deflation possible, search for start of sub-matrix 
      { start=end-1;
        while ((wnorm+wV[start]!=wnorm)&&(wnorm+wsV[start]!=wnorm)&&(start>=0)) start--;
        start++; // this is now the row and column starting the sub-matrix
        if ((start>0)&&(wnorm+wV[start-1]==wnorm)&&(wnorm+wsV[start-1]!=wnorm)) 
        { // ws.V[start-1] must be zeroed....
          y=wsV[start-1];wsV[start-1]=0.0;
          for (i=start;i<=end;i++) // get sequence of rotators from left....
          { m=fabs(y);x=fabs(wV[i]); if (x>m) m=x;
            x=wV[i];
            if (m>0.0)
            { x/=m;y/=m;
              r=sqrt(x*x+y*y);
              c=x/r;s=y/r;
            } else {r=1.0;c=1.0;s=0.0;}
            wV[i]=r*m; // y zeroed implicitly
            if (i<end) // propagate the problem element now at (start-1,i)
            { y= -s*wsV[i];
              wsV[i]*=c;
            }  
            /* Now U must be updated, by transposed rotators - from the right */
            for (j=0;j<U->r;j++) // work down the rows
            { p1=UM[j]+start-1;x = *p1;p2=UM[j]+i;//x=UM[j][start-1];
              // UM[j][start-1] = c*x-s*UM[j][i];
              *p1 = c*x - s*(*p2);
              // UM[j][i]*=c; UM[j][i] += +s*x;
              *p2 *= c; *p2 += s*x; 
            } 
          }
        }
      }  
      /* iterate QR algorithm on sub-matrix */
      /* First find the Wilkinson shift which is given by the eigenvalue of the 
         bottom right 2 by 2 submatrix, closest to the final matrix element. 
         The required eigenvalues are found directly from the characteristic equation.
         See page 405 of Watkins.*/
      
      a=wV[end-1]*wV[end-1]+wsV[end-1]*wsV[end-1];b=wV[end];b*=b;
      c=wV[end]*wsV[end-1];
      y=sqrt((a-b)*(a-b)+4*c*c)/2;
      x=(a+b)/2+y;y=(a+b)/2-y; // x and y are the eigenvalues
      if (fabs(x-b)<fabs(y-b)) sig=x; else sig=y;
     
      // ...... this could be improved!!
      /* Now apply first step and then chase the bulge ...... */
    
      x=wV[start];
      y=wsV[start]*x;
      x=x*x-sig; // x and y are the first and second elements of the rotator starting implicit QR step
      m=fabs(x);if (fabs(y)>m) m=fabs(y);
      if (m>0.0)
      { y/=m;x/=m; // avoid over/underflow
        r=sqrt(y*y+x*x);
        c=x/r;s=y/r; // elements of rotator to apply from right operating in start,start+1 plane
      } else { r=1.0;c=1.0;s=0.0;}
      for (i=start;i<end;i++) 
      { // start with post-multiplication
        if (start<i) // then rotator needs to be calculated to remove element at (i-1,i+1) (stored in y)
        { x=wsV[i-1]; // location y rotated into
          m=fabs(y);if (fabs(x)>m) m=fabs(x);
          if (m>0.0)
          { x/=m;y/=m;   // avoiding overflow
            r=sqrt(x*x+y*y);
            c=x/r;s=y/r;
          } else {r=1.0;c=1.0;s=0.0;} // rotator for zeroing y (at i-1,i+1) int x at (i-1,i)
          wsV[i-1]=r*m;y=0.0;
        }
        // now apply rotator from right to rows i and i+1....
        x=wV[i];
        wV[i]=c*x+s*wsV[i];
        wsV[i]=c*wsV[i]-s*x;
        y=s*wV[i+1];wV[i+1]*=c; // y contains the bulge at (i+1,i)
        // and also apply from right to V....
        for (j=0;j<V->r;j++) // work down the rows
        { p1=VM[j]+i;x= *p1;p2=VM[j]+i+1; //x=VM[j][i];
          //VM[j][i]=c*x+s*VM[j][i+1];
          *p1=c*x + s* (*p2);
          //VM[j][i+1]*=c;VM[j][i+1] += -s*x;
          *p2 *= c; *p2 += -s*x;
        }  
          /* Obtain rotator from left to zero element at (i+1,i) into element at (i,i) 
           thereby creating new bulge at (i,i+2) */ 
        x=wV[i];
        m=fabs(y);if (fabs(x)>m) m = fabs(x);
        if (m>0.0)
        { x/=m;y/=m; // avoid overflow
          r=sqrt(x*x+y*y);
          c=x/r;s=y/r;
        } else {r=1.0;c=1.0;s=0.0;} // transform to zero y into x (i+1,i) into (i,i)
        wV[i]=r*m;y=0.0; 
        // apply from left....
        x=wsV[i];
        wsV[i]=c*x+s*wV[i+1];
        wV[i+1]=c*wV[i+1]-s*x;
        if (i<end-1)
        { y=wsV[i+1]*s;
          wsV[i+1]*=c;
        } 
        // and apply transposed rotator from right to U 
        for (j=0;j<U->r;j++) // work down the rows
        { p1=UM[j]+i;x= *p1;p2=UM[j]+i+1;//x=UM[j][i];
          //UM[j][i]=c*x+s*UM[j][i+1];
          *p1=c*x+s*(*p2);
          //UM[j][i+1]*=c; UM[j][i+1] += -s*x;
          *p2 *= c; *p2 += -s*x;
        } 
      }
    }  
    if (k==maxreps) 
    ErrorMessage("svd() not converged",1);
  }
  freemat(ws);
  /* make all singular values  non-negative */
  for (i=0;i<w->r;i++) 
  if (wV[i]<0.0)
  { wV[i]= -wV[i];
    for (j=0;j<V->r;j++) VM[j][i]= -VM[j][i];
  } 
}








void symproduct(A,B,C,trace,chol) matrix A,B,C;int trace,chol;

/* This routine aims to find the product C=ABA' as fast as possible, when
   B is symmetric.
   If 'trace' is non-zero then only the elements on the trace of C are
   returned.
   If 'chol' is non-zero then it is assumed that B contains L the
   choleski factor of B and is lower triangular (zeros at top right).
   If chol is 1 then it is assumed that the decomposition is LL' otherwise
   it is assumed to be L'L.  The routine choleski returns LL' for the decomp
   of the matrix or L'L for the decomp of the inverse.
*/

{ long i,j,k,Mr,Mc,Ac,Ar,Bc; 
  double temp,*p,*p1,*p2,**AM,**BM,**CM,**MM;
  matrix M;
  AM=A.M;BM=B.M;CM=C.M;Ac=A.c;Ar=A.r;Bc=B.c;
  if (chol)
  { M=initmat(Ar,B.c);
    MM=M.M;Mr=M.r;Mc=M.c;
    if (chol==1)
    for (i=0;i<Mr;i++)
    for (j=0;j<Mc;j++)
    { p2=MM[i]+j;p=AM[i]+j;
      for (k=j;k<Ac;k++) (*p2)+=(*p++)*BM[k][j];
    } else
    for (i=0;i<Mr;i++)
    for (j=0;j<Mc;j++)
    { p2=MM[i]+j;p1=AM[i];
      for (p=BM[j];p<=(BM[j]+j);p++) (*p2)+=(*p1++)*(*p);
    }
    if (trace)
    { for (i=0;i<Mr;i++)
      { CM[i][i]=0.0;p2=CM[i]+i;
	     for (p=MM[i];p<(MM[i]+Mc);p++) { (*p2)+= (*p)*(*p);}
      }
    } else
    { for (i=0;i<Mr;i++)
      { for (j=i;j<Mr;j++)
   	  { CM[i][j]=0.0;p2=CM[i]+j;p1=MM[j];
	       for (p=MM[i];p<(MM[i]+Mc);p++) (*p2) += (*p)*(*p1++);
	       CM[j][i]=(*p2);
	     }
      }
    }
    freemat(M);
  } else
  { if (trace)
    { for (i=0;i<C.c;i++)
      { CM[i][i]=0.0;
	     for (j=0;j<Bc;j++)
	     { temp=0.0;p1=AM[i]+j+1;
	       for (p=BM[j]+j+1;p<(BM[j]+Bc);p++)
	       temp += (*p)*(*p1++);
          CM[i][i]+=temp*AM[i][j];
	     }
	     CM[i][i]*=2.0;
	     p2=CM[i]+i;p=AM[i];
	     for (j=0;j<Bc;j++) { (*p2)+= (*p)*(*p)*BM[j][j];p++;}
      }
    } else
    { M=initmat(Ac,Ar);
      MM=M.M;Mc=M.c;
      matmult(M,B,A,0,1);
      for (i=0;i<Ar;i++)
      { for (j=i;j<Mc;j++)
	     { CM[i][j]=0.0;p2=CM[i]+j;p=AM[i];
	       for (k=0;k<Ac;k++) (*p2)+=(*p++)*MM[k][j];
	       CM[j][i]=(*p2);
	     }
      }
      freemat(M);
    }
  }
}

matrix getmask(index,smalln,bign) int *index,smalln,bign;

/* This is a routine for returning the matrix M such that
   a=Mp where a.M[i]=p.M[index[i]] */

{ int i;
  matrix M;
  double **MM;
  M=initmat((long)smalln,(long)bign);
  MM=M.M;
  for (i=0;i<smalln;i++) MM[i][index[i]]=1.0;
  return(M);
}



void gettextmatrix(M,name) matrix M;char *name;

/* reads a text file with M.r rows and M.c columns into matrix M */

{ FILE *f;
  long i,j;
  char c,str[200];
  f=fopen(name,"rt");
  if (!f)
  { sprintf(str,"%s not found by routine gettextmatrix().\n",name);ErrorMessage(str,1);}
  for (i=0;i<M.r;i++)
  { for (j=0;j<M.c;j++)
    { fscanf(f,"%lf",M.M[i]+j);
    }
    c=' ';while ((c!='\n')&&(!feof(f))) c=(char)fgetc(f);
  }
  fclose(f);
}


void notinv(J,K,B) matrix J,K,B;

/* solves the problem of finding a K such that JK=B when J has more
   columns than rows and B is square with same number of rows as J. Note that
   J gets overwritten by this routine, during QT factorisation. The algorithm
   is as follows: Factorise J=[0,T]Q'; solve [0,T]Z=B for Z allowing first
   J.c-J.r cols of Z to be zero; then K = QZ. This algorithm makes maximum use
   of info in J. 14/1/96 */

{ matrix Q,Z;
  long i,j,k,Jc1,Zr1,Jr;
  double e,**JM,**ZM,**BM,**KM;
  Jr=J.r;
  Q=initmat(J.r,J.c);
  QT(Q,J,0);
  Z=initmat(J.c,Jr);
  JM=J.M;ZM=Z.M;BM=B.M;KM=K.M;
  Zr1=Z.r-1L;Jc1=J.c-1L;
  for (i=0;i<Jr;i++)
  for (j=0;j<Jr;j++)
  { e=0.0;
    for (k=0;k<i;k++) e+=JM[i][Jc1-k]*ZM[Zr1-k][j];
    ZM[Zr1-i][j]=(BM[i][j]-e)/JM[i][Jc1-i];
  }
  for (i=0;i<Z.r;i++) for (j=0;j<Z.c;j++) KM[i][j]=ZM[i][j];
  HQmult(K,Q,1,0);
  freemat(Z);
  freemat(Q);
}

void leastsq(A,p,y,w) matrix A,p,y,w;

/* dummy routine for fullLS */

{ matrix T,z;T.r=0L;z.r=0L;
  fullLS(A,p,y,w,T,z,0);
}


void fullLS(A,p,y,w,T,zo,Tzout) matrix A,p,y,w,T,zo;int Tzout;

/* Minimises ||w(Ap-y)|| for p using TQ orthogonal factorisation of A' -
   This is the way it should be done (i.e. not by forming inv(A'A)A'
   explicitly! ) 14/1/96 - Could be made a bit faster by using a QR
   factorisation in place of TQ - speeded up by sensible use
   of orthogonal factorisation, call QT with Qfull=0, and Q same dimension
   as At, then instead of premultiplying y by Q in matmult, use
   HQmult(z,Q,1,0) having set values in z to y.
   Method is ||Ap-y|| = ||y'-p'A'||, so use a QT factorisation of A'
   A'Q = [0,T] where Q is orthogonal. Then solve the soluble equations
   from y'Q = p'[0,T] to get p.
   NOTE: w vector with square roots of normal weights - i.e. 1 over s.d.s
   for no weights set w.r==0L;
   The matrices T (p.r by p.r) and the zo last p.r elements of Q'y are output
   if Tzout is non-zero.
*/

{ matrix At,Q,z;
  double e,**AM,**AtM,*pV,*zV;
  long i,j,Atr,Atc,pr,zr;
  At=initmat(A.c,A.r);
  Q=initmat(At.r,At.c);
  AtM=At.M;AM=A.M;Atr=At.r;Atc=At.c;pV=p.V;pr=p.r;
  if (!w.r)
  for (i=0;i<A.r;i++) for (j=0;j<A.c;j++) AtM[j][i]=AM[i][j];
  else
  for (i=0;i<A.r;i++) for (j=0;j<A.c;j++) AtM[j][i]=AM[i][j]*w.V[i];
  QT(Q,At,0);
  z=initmat(y.r,1L);zV=z.V;zr=z.r;
  if (w.r)
  for (i=0;i<zr;i++) zV[i]=y.V[i]*w.V[i]; /* new code for faster orthogonal transforms */
  else
  for (i=0;i<zr;i++) zV[i]=y.V[i];
  HQmult(z,Q,1,1);                 /* does orthogonal rotation the fast way */
  /* end of debug code */
  for (i=0;i<Atr;i++)
  { e=0.0;
    for (j=0;j<i;j++) e+=AtM[Atr-j-1][Atc-Atr+i]*pV[pr-j-1];
    pV[pr-1-i]=(zV[zr-Atr+i]-e)/AtM[Atr-i-1][Atc-Atr+i];
  }
  if (Tzout) /* outputs the factorized problem, to allow updates */
  { for (i=0;i<zo.r;i++) zo.V[i]=zV[zr-zo.r+i];
    for (i=0;i<T.r;i++) for (j=T.r-i-1;j<T.r;j++)
    T.M[i][j]=AtM[i][Atc-Atr+j];
  }
  freemat(At);
  freemat(Q);
  freemat(z);
}

void updateLS(T,z,x,y,w) matrix T,z,x;double y,w;

/* This routine takes the matrices T and z from the orthogonal factorisation
   used to solve the least squares problem in fullLS(). Given a single extra
   observation, y, with a corresponding weight, w, and design matrix row, x,
   T and z are updated efficiently using Givens rotations so that solution
   of Tp=z will now yield the solution of the ammended least squares problem.
   This routine has been tested in the weighted case.
   
   13/1/2000 Givens rotations are protected against under/overflow as follows...
   Assume xj to be zeroed into xi:
   m = max(fabs(xi),fabs(xj));
   xi/=m;xj/=m
   r=sqrt(xi*xi+xj*xj);
   c=xi/r;s=xj/r;
   xi=m*r;xj=0.0;
*/

{ double xi,ti,*xp,*tp,r,s,c,m;
  int i,j;
  matrix xx;
  xx=initmat(x.r,1L); /* needed because xx has to be over-written */
  for (i=0;i<x.r;i++) xx.V[i]=w*x.V[i];y*=w;
  for (i=0;i<T.r;i++) /* work through the columns of x rows of T */
  { xi=xx.V[i];ti=T.M[i][T.r-i-1];
    m=fabs(xi);r=fabs(ti);if (r>m) m=r; // scale factor
    if (m) { xi/=m;ti/=m;} // avoid over/underflow
    r=sqrt(xi*xi+ti*ti);
    if (r) { s=xi/r;c= -ti/r;} else {s=0.0;c=1.0;}
   /* T.M[i][T.r-i-1]=r;*/
    for (j=i;j<T.r;j++)
    { xp=xx.V+j;tp=T.M[j]+T.r-i-1;
      xi=c*(*xp)+s*(*tp);
      *tp=s*(*xp)-c*(*tp);
      *xp=xi;
    }
    xi=c*y+s*z.V[z.r-i-1];
    z.V[z.r-i-1]=s*y-c*z.V[z.r-i-1];
    y=xi;
  }
  freemat(xx);
}

void rtsolve(T,p,z) matrix T,p,z;

/* solves p'T=z' for p when T is reverse lower triangular (as from a QT
   factorization) */

{ int i,j;
  double e;
  for (i=p.r-1;i>=0;i--)
  { e=0.0;
    for (j=i+1;j<p.r;j++) e+= p.V[j]*T.M[j][T.c-i-1];
    p.V[i]=(z.V[z.r-i-1]-e)/T.M[i][T.c-i-1];
  }
}




long alias(X,aliased,tol) matrix X;long *aliased;double tol;

/* checks that all columns of X are independent and if not produces a list
   of those that are not. aliased must have the dimension of the X.r on
   entry. On exit its first n elements contain the columns to be removed
   to get a full rank matrix. The return value is the number of aliased
   columns. The algorithm checks each column in turn for independence of
   the preceding set of independent columns, by regressing the column on the
   previous independent set and checking that the rss is large enough.*/

{ matrix F,x,xp,p,w;
  double e,x2,y;
  long i,j,k,n;
  w.r=0L;
  F=initmat(X.r,X.c);x=initmat(X.r,1L);p=initmat(X.c,1L);
  xp=initmat(x.r,1L);
  j=1L;n=0L;
  for (k=0;k<X.r;k++) F.M[k][0]=X.M[k][0];F.c=1L;
  for (i=1;i<X.c;i++)
  { for (k=0;k<X.r;k++) x.V[k]=X.M[k][i];p.r=F.c;
    leastsq(F,p,x,w);
    matmult(xp,F,p,0,0);
    x2=0.0;e=0.0;
    for (k=0;k<x.r;k++)
    { y=x.V[k];x2+=y*y;
      y -= xp.V[k];e+=y*y;
    }
    if (e>tol*x2) /* column sufficiently independent */
    { for (k=0;k<X.r;k++) F.M[k][j]=x.V[k];
      F.c++;j++;
    } else
    { aliased[n]=i;n++;}
  }
  freemat(F);freemat(xp);freemat(x);freemat(p);
  return(n);
}




matrix svdroot(matrix A,double reltol)

/* Finds smallest squareroot of a non-negative definite matrix. reltol is
   used to decide which columns to remove... */

{ long k=0l,i,j;
  double tol=0.0;
  matrix a,v,w;
  a=initmat(A.r,A.c);mcopy(&A,&a);
  v=initmat(A.r,A.c);
  w=initmat(A.r,1L);
  svd(&a,&w,&v);   //a * diag(w) * v'
  for (i=0;i<w.r;i++) { w.V[i]=sqrt(w.V[i]);if (w.V[i]>tol) tol=w.V[i];}
  tol*=reltol;
  for (i=0;i<w.r;i++)
  { if (w.V[i]>tol)
    { for (j=0;j<a.c;j++) v.M[j][k]=a.M[j][i]*w.V[i];k++;}
  }
  v.c=k;
  freemat(a);freemat(w);
  return(v);
}

void svdLS(A,p,y,tol) matrix A,p,y; double tol;

/* Minimises ||Ap-y|| for p, whilst minimising ||p|| if A not of full rank.
   Rank is estimated as the number of singular values greater than (tol
   multiplied by the largest singular value). Minimisation is performed
   by use of (truncated) S.V.D. */

{ matrix u,s,v;
  long i,j,Ar,Ac;
  double **uM,**AM,*sV,maxsv;
  AM=A.M;Ar=A.r;Ac=A.c;
  u=initmat(A.r,A.c);
  s=initmat(A.c,1L);
  v=initmat(A.c,A.c);
  sV=s.V;uM=u.M;
  for (i=0;i<Ar;i++) for (j=0;j<Ac;j++) uM[i][j]=AM[i][j];
  svd(&u,&s,&v);
  if (tol<0.0) tol=0.0;if (tol>1.0) tol=1.0;
  maxsv=0.0;for (i=0;i<s.r;i++) if (maxsv<sV[i]) maxsv=sV[i];
  for (i=0;i<s.r;i++)
  if (sV[i]>tol*maxsv)   /* estimate rank and do truncation */
  { sV[i]=1.0/sV[i];} else sV[i]=0.0;
  for (i=0;i<Ar;i++) for (j=0;j<Ac;j++) uM[i][j]*=sV[j];
  freemat(s);
  s=initmat(u.c,1L);
  matmult(s,u,y,1,0);
  matmult(p,v,s,0,0);
  freemat(u);freemat(s);freemat(v);
}

long rank(a) matrix a;

/* estimates rank of a */

{ double max;
  long i,j,r=0L;
  matrix b,c,d;
  b=initmat(a.r,a.c);
  for (i=0;i<a.r;i++) for (j=0;j<a.c;j++) b.M[i][j]=a.M[i][j];
  c=initmat(a.c,1L);d=initmat(a.c,a.c);
  svd(&b,&c,&d);
  max=c.V[0];
  for (i=1;i<c.r;i++)
  { if (fabs(c.V[i])>max) max=fabs(c.V[i]);}
  max*=1e-14;
  for (i=0;i<c.r;i++) if (fabs(c.V[i])>max) r++;
  freemat(b);freemat(c);freemat(d);
  return(r);
}




double condition(a) matrix a;

/* finds the condition number of a as the ratio of the smallest to largest
   singular values - returns -ve for infinity */

{ int i,j;
  double res,min,max;
  matrix b,c,d;
  b=initmat(a.r,a.c);
  for (i=0;i<a.r;i++) for (j=0;j<a.c;j++) b.M[i][j]=a.M[i][j];
  c=initmat(a.c,1L);d=initmat(a.c,a.c);svd(&b,&c,&d);
  //for (i=0;i<c.r;i++) { printf("%g",c.V[i]); getc(stdin);} // comment out usually
  min=max=c.V[0];
  for (i=1;i<c.r;i++)
  { if (c.V[i]<min) min=c.V[i]; else if (c.V[i]>max) max=c.V[i];}
  if (!min) res=-1.0; else res=max/min;
  freemat(b);freemat(c);freemat(d);
  return(res);
}

void specd(U,W) matrix U,W;

/* obtains the spectral decomposition of a symmetric matrix U:
   it is returned as U diag(W) U'.
   The routine uses svd, and then checks the signs of the eigenvalues
   for correctness - not exactly the most efficient method!! */

{ matrix V;
  double dot;
  long i,j;
  V=initmat(U.r,U.r);
  svd(&U,&W,&V);
  for (j=0;j<U.c;j++) /* work through eigenvectors */
  { dot=0.0;for (i=0;i<U.r;i++) dot+=U.M[i][j]*V.M[i][j]; /* have to do all elements to avoid round off near zero problems */
    if (dot<0.0) W.V[j]= -W.V[j];
  }
  freemat(V);
}

long pinv(matrix *A,double trunc)

/* forms pseudoinverse of A - returning it in A, return value is rank estimate
   for A. If trunc is >=1.0 then this is taken as the required rank of the
   pseudoinvese, otherwise its the tolerance used for truncation 
*/

{ matrix ws,B,w,C;
  long i,j,r=0L;
  double max;
  B=initmat(A->c,A->c);w=initmat(A->c,1L);
  svd(A,&w,&B); /* now original A is A diag(w) B' */
  /* form pseudoinverse B diag(1/w) A' - but truncating small w_i's */
  C=initmat(A->r,A->c);
  if (trunc<1.0)
  { max=0.0;for (i=0;i<w.r;i++) if (fabs(w.V[i])>max) max=fabs(w.V[i]);
    max*=trunc;
  }
  else /* the rank of the required pinverse has been supplied */
  { if (r-(long)floor(trunc)>0.5) r=(long)floor(trunc)+1;else r=(long)floor(trunc);
    ws=initmat(w.r,1L);
    for (i=0;i<ws.r;i++) ws.V[i]=fabs(w.V[i]);
    sort(ws);
    max=ws.V[ws.r-r];
    freemat(ws);
  }
  r=0L;
  for (j=0;j<A->c;j++)
  if (fabs(w.V[j])>=max)
  { for (i=0;i<A->r;i++)
    C.M[i][j]=A->M[i][j]/w.V[j];
    r++;
  }
  if (A->r!=A->c)
  { freemat(*A);
    *A=initmat(B.r,C.r);
  }
  matmult(*A,B,C,0,1);
  freemat(w);
  freemat(C);
  freemat(B);
  return(r);
}




void suminvert(A,B,U,W)
matrix A,B,U,W;

/* If A and B are symmetric matrices then this routine returns U,W, where
   U is a square matrix and W a vector of eigenvalues. The inverse of
   A+cB where c is a constant can now be obtained from:
   U Diag(1.0/(W.V[i]+c)) U'
   NOTE: B must be +ve definite.
*/

{ matrix L,M,MA,MAM,T;
  long i,j,k;
  double *p,temp,*p1,**MM,**LM;
  T=initmat(A.r,A.r);
/* first get choleski factor of B */
  L=initmat(B.r,B.r);
  choleski(B,L,0,0); /* L contains choleski factor */
/* Inverting L (it stays lower triangular)*/
  M=initmat(B.r,B.c);MM=M.M;LM=L.M;
  for (i=0;i<B.r;i++) MM[i][i]=1.0;
  for (i=0;i<B.r;i++)
  { temp=LM[i][i];
    for (p=MM[i];p<=(MM[i]+i);p++) (*p)/=temp;
    for (j=i+1;j<B.r;j++)    /* original code had rows in inner loop */
    { temp=LM[j][i];p1=MM[j];
      for (p=MM[i];p<=(MM[i]+i);p++)
      (*p1++) -= (*p)*temp;
    }
  }
  /* now if LL' = A and M=inv(L) so that M'M= inv (A) then:
   A+cB = A + cLL' = LM(A+cLL')M'L' = L(MAM'+cI)L'
   and inv(A+cB) = M'inv(MAM'+cI)M - the convoluted way of doing things ensures
   the symmetry of MAM' so that its spectral decomp. can be obtained by svd */
  /* Form MAM efficiently */
  MA=initmat(A.r,A.r);
  for (i=0;i<A.r;i++) for (j=0;j<A.r;j++)
  for (k=0;k<=i;k++) MA.M[i][j]+=M.M[i][k]*A.M[k][j];
  MAM=initmat(A.r,A.r);
  for (i=0;i<A.r;i++) for (j=0;j<=i;j++)
  { for (k=0;k<=j;k++) MAM.M[i][j]+=MA.M[i][k]*M.M[j][k];
    MAM.M[j][i]=MAM.M[i][j];
  }
  /* get its svd/ spectral decomp. the decomp of MAM will be returned as
     MAM Diag(W) MAM' */

  specd(MAM,W);
  /* form U = M' MAM  */

  for (i=0;i<U.r;i++) for (j=0;j<U.c;j++)
  { U.M[i][j]=0.0; for (k=i;k<U.r;k++) U.M[i][j]+=M.M[k][i]*MAM.M[k][j];}

  freemat(M);freemat(MAM);freemat(MA);freemat(L);
}

int elemcmp(const void *a,const void *b)

{
  if (*(double *)a<*(double *)b) return(-1);
  if (*(double *)a>*(double *)b) return(1);
  return(0);
}


void sort(matrix a)

/* sorts a vector, in situ, using standard routine qsort */

{ int i;
  qsort(a.V,(size_t)a.r*a.c,sizeof(a.V[0]),elemcmp);
  for (i=0;i<a.r*a.c-1;i++) if (a.V[i]>a.V[i+1])
  ErrorMessage("Sort failed",1);

}


void printmat(matrix A,char *fmt)

{ int i,j;
  double n;
  n=matrixnorm(A);
  for (i=0;i<A.r;i++)
  { printf("\n");
    for (j=0;j<A.c;j++) 
    if (fabs(A.M[i][j])>1e-14*n) printf(fmt,A.M[i][j]);
    else printf(fmt,0.0);
  }
  printf("\n");
}



/*********************************************************************************/
/* Update Log (started Jan 2000)                                                 */
/*********************************************************************************/

/* 1. Numerical Recipes code removed. This meant writing 2 new routines: svd() and
      invert(), to replace versions that were just modified from Numerical Recipes.
      An extra routine bidiag() was also written to bidiagonalize a matrix.
      Routines were heavily tested against old NR routines and perform at least as 
      well. See internal documentation for algorithm description and references.
      Both written to be as robust as possible. 12/1/2000
   2. UTU() and QR() modified to remove danger of under/overflow in householder 
      calculations, and increase efficiency. Tested using random matrices. 13/1/2000
   3. Dependence on inverse.h removed, options for SUNC flag removed. 8/1/2000
   4. QT() re-written - now much more efficient (consistent with producing output
      in its old format), no cancellation error, under/overflow protected, address
      optimized. NOTE: householder() and Hmult() no longer used in matrix.c -
      check qp.c before removing!
   5. updateLS() has had its Givens rotations protected against under/overflow. 
      (not tested). 
   6. NOTE: choleskir1ud() has not been checked for underflow/overflow and stability.
   7. tricholeski() has been checked again with a variety of rank deficient tridiagonal
      matrices. Theoretical basis for approach has been much strengthened, but I still
      can not *prove* that the choleski factor will require a zero only on the *final* 
      element of the main diagonal for a properly tri-diagonal matrix. Hence root() now 
      has a check that the  "choleski" factors of the tridiagonal matrix are adequate - 
      if not singular value decomposition is used instead. root() also has an improved 
      column removal strategy. 16/1/2000 
  
   8. This -> if (wsV[end-1]+wnorm==wnorm) test for convergence of singular values is 
      rather too stringent! Replaced with something more reasonable. 25/2/2000
      15/5/2000 - convergence criteria changed back - eigenvalue used for shift was
      calculated in a manner that allowed bad cancellation errors - this caused 
      convergence problems AND occasional imaginary eigenvalues for the shift. Fixing
      this fixed the convergence problem.

   9. svd() bug fixed - it division by zero was possible while trying to avoid over/
      underflow in Givens rotations - check added to fix problem. 15/5/00

*/