```/*
All functions needed for Linear Algrebra
This is especially used for Least Square Estimates
*/

/*
All functions needed for Linear Algrebra
This is especially used for Least Square Estimates

date: Nov 2103
author: gachaz
*/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "LinearAlgebra.h"

/*
Memory functions
*/

struct matrix MemMat( int m, int n ){

int i;
struct matrix M;

M.r=m;
M.c=n;

M.val = (double **) malloc( (size_t) M.r * sizeof(double *) );
if( ! M.val ) fprintf(stderr, "MemMat: cannot allocate matrix, bye\n"), exit(1);

for(i=0; i< M.r ; i++){
M.val[i] = (double *) malloc( (size_t) M.c * sizeof(double) );
if(!M.val[i]) fprintf(stderr, "MemMat: cannot allocate M[%d], bye\n",i), exit(1);
}

return M;
}

void IdentMat(struct matrix *M)
{

int i, j;

for (i = 0; i < M->r; i++)
{
for (j = 0; j < M->c; j++)
{
if (i==j)
{
M->val[i][i] = 1.;
}
else
{
M->val[i][j] = 0.;
}
}
}
}

void FreeMat( struct matrix *M ){

int i;

if(M->val == NULL)
return;

for(i=0;i<M->r; i++)
free( M->val[i] );

free(M->val);

M->val =NULL;
}

/*
Output a matrix
*/
void PrintMat( struct matrix *M, char *name ){

int i,j;

printf("%s [%d,%d]\n", name, M->r, M->c  );

for(i=0;i<M->r; i++, printf("\n") )
for(j=0;j<M->c;j++)
printf("%8.3f", M->val[i][j]);

printf("//\n");
}

struct matrix MatrixTranspose( struct matrix *M, char opt_free ){

int i,j;
struct matrix T = MemMat( M->c, M->r );

for(i=0 ; i<M->r ; i++)
for(j=0 ; j<M->c ; j++)
T.val[j][i] = M->val[i][j];

if(opt_free)
FreeMat(M);

return T;

}

/*
From a vector, create a Diag Matrix
*/
struct matrix CreateMatrixDiag( struct matrix *V, char opt_free )
{
int i,j;

struct matrix D = MemMat( V->r,V->r );

for(i=0; i<D.r; i++ )
{
for(j=0; j<D.c; j++ )
{
D.val[i][j]=0;

if( i == j )
{
D.val[i][j] = V->val[i][0];
}
}

}

if(opt_free)
{
FreeMat(V);
}

return D;
}

struct matrix CreateMatrixDiagMatrix( struct matrix *M1, char opt_free )
{
int i,j;

struct matrix D = MemMat( M1->r,M1->c );

for(i=0; i<D.r; i++ )
{
for(j=0; j<D.c; j++ )
{
D.val[i][j] = 0;
if( i == j )
{
D.val[i][j] = M1->val[i][i];
}

}
}

if(opt_free)
{
FreeMat(M1);
}

return (D);
}

struct matrix Invert_MatrixDiag(struct matrix *D, char opt_free )
{
int i,j;
struct matrix M;

M = MemMat( D->r, D->c );

for(i=0; i<M.r; i++ )
for(j=0; j<M.c; j++ )
{
M.val[i][j]=0;

if( i == j )
M.val[i][j] = 1.0/D->val[i][j];

}

if(opt_free)
FreeMat(D);

return M;

}

struct matrix MatrixProduct(struct matrix *M_left, struct matrix *M_right, char opt_free)
{

int i,j,k;
struct matrix P;

if( M_left->c != M_right->r )
fprintf(stderr, "MatrixProduct: The matrices cannot be multiplied together. Incompatible dimensions, bye\n"),exit(1);

P=MemMat( M_left->r, M_right->c );

for(i=0; i<P.r; i++ )
{
for(j=0; j<P.c; j++ )
{
P.val[i][j]=0;

for(k=0; k<M_left->c; k++ )
{
P.val[i][j] += M_left->val[i][k] * M_right->val[k][j];
}
}
}

if(opt_free)
{
FreeMat(M_right);
FreeMat(M_left);
}

return P;

}

void MatrixProductRef(struct matrix *M_left, struct matrix *M_right, struct matrix * Prod, char opt_free)
{

int i,j,k;

if( M_left->c != M_right->r )
fprintf(stderr, "MatrixProduct: The matrices cannot be multiplied together. Incompatible dimensions, bye\n"),exit(1);

for(i=0; i<Prod->r; i++ )
{
for(j=0; j<Prod->c; j++ )
{
Prod->val[i][j]=0;

for(k=0; k<M_left->c; k++ )
{
Prod->val[i][j] += M_left->val[i][k] * M_right->val[k][j];
}
}
}

if(opt_free)
{
FreeMat(M_right);
FreeMat(M_left);
}

}

struct matrix MatrixSum(struct matrix *M1, struct matrix *M2, char opt_free)
{

int i,j;
struct matrix S;

if( M1->r != M2->r || M1->c != M2->c )
fprintf(stderr, "MatrixSum: The matrices cannot be summed together. Incompatible dimensions, bye\n"),exit(1);

S = MemMat( M1->r, M1->c );

for(i=0; i<S.r; i++ )
{
for(j=0; j<S.c; j++ )
{
S.val[i][j] = M1->val[i][j] + M2->val[i][j];
}
}

if(opt_free)
{
FreeMat(M1);
FreeMat(M2);
}

return S;
}

void MatrixSumRef(struct matrix *M1, struct matrix *M2, struct matrix *Sum, char opt_free)
{

int i,j;

if( M1->r != M2->r || M1->c != M2->c )
fprintf(stderr, "MatrixSum: The matrices cannot be summed together. Incompatible dimensions, bye\n"),exit(1);

for(i=0; i<Sum->r; i++ )
{
for(j=0; j<Sum->c; j++ )
{
Sum->val[i][j] = M1->val[i][j] + M2->val[i][j];
}
}

if(opt_free)
{
FreeMat(M1);
FreeMat(M2);
}
}

struct matrix MatrixDiff(struct matrix *M1, struct matrix *M2, char opt_free)
{

int i,j;
struct matrix S;

if( M1->r != M2->r || M1->c != M2->c )
fprintf(stderr, "MatrixSum: The matrices cannot be substracted. Incompatible dimensions, bye\n"),exit(1);

S = MemMat( M1->r, M1->c );

for(i=0; i<S.r; i++ )
{
for(j=0; j<S.c; j++ )
{
S.val[i][j] = M1->val[i][j] - M2->val[i][j];
}
}

if(opt_free)
{
FreeMat(M1);
FreeMat(M2);
}

return S;
}

void MatrixDiffRef(struct matrix *M1, struct matrix *M2, struct matrix * Diff, char opt_free)
{

int i,j;

if( M1->r != M2->r || M1->c != M2->c )
fprintf(stderr, "MatrixSum: The matrices cannot be substracted. Incompatible dimensions, bye\n"),exit(1);

for(i=0; i<Diff->r; i++ )
{
for(j=0; j<Diff->c; j++ )
{
Diff->val[i][j] = M1->val[i][j] - M2->val[i][j];
}
}

if(opt_free)
{
FreeMat(M1);
FreeMat(M2);
}

}

struct matrix MatrixHadarmardProduct(struct matrix *M1, struct matrix *M2, char opt_free)
{
int i, j;

if( M1->r != M2->r || M1->c != M2->c )
{
fprintf(stderr, "MatrixHadarmardProduct: The matrices cannot be multiplied together. Incompatible dimensions, bye\n"),exit(1);
}

for(i = 0; i < M1->r; i++)
{
for(j = 0; j < M1->c; j++)
{
}
}

if(opt_free)
{
FreeMat(M1);
FreeMat(M2);
}

}

void MatrixHadarmardProductRef(struct matrix *M1, struct matrix *M2, struct matrix * HadarmardProduct, char opt_free)
{
int i, j;

if( M1->r != M2->r || M1->c != M2->c )
{
fprintf(stderr, "MatrixHadarmardProduct: The matrices cannot be multiplied together. Incompatible dimensions, bye\n"),exit(1);
}

for(i = 0; i < M1->r; i++)
{
for(j = 0; j < M1->c; j++)
{
}
}

if(opt_free)
{
FreeMat(M1);
FreeMat(M2);
}

}

struct matrix scaleMatrix(struct matrix *M1, float scale, char opt_free)
{
struct matrix M2;
int i, j;

M2 = MemMat(M1->r, M1->c);
for(i = 0; i < M1->r; i++)
{
for(j = 0; j < M1->c; j++)
{
M2.val[i][j] = scale * M1->val[i][j];
}
}

if(opt_free)
{
FreeMat(M1);
}

return (M2);
}

void scaleMatrixRef(struct matrix *M1, 	struct matrix * M2, float scale, char opt_free)
{
int i, j;

for(i = 0; i < M1->r; i++)
{
for(j = 0; j < M1->c; j++)
{
M2->val[i][j] = scale * M1->val[i][j];
}
}

if(opt_free)
{
FreeMat(M1);
}

}

/*
Do the SVD decomposition of any A matrix --dimension (m,n)--
A = U.w.Vt
U is orthogonal and dimension (m,n)
w is a diagonal matrix of size (n,n)
V is a square orthogonal matrix (n,n)

The code has been adapted from numrec 92 -- reset mat from 0 to n-1 as in std C --

*/

#define CONVERGENCE 100

#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))

static int iminarg1,iminarg2;
#define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
(iminarg1) : (iminarg2))

static double dsqrarg;
#define DSQR(a) ((dsqrarg=(a)) == 0.0 ? 0.0 : dsqrarg*dsqrarg)

static double dmaxarg1,dmaxarg2;
#define DMAX(a,b) (dmaxarg1=(a),dmaxarg2=(b),(dmaxarg1) > (dmaxarg2) ?\
(dmaxarg1) : (dmaxarg2))

/*
Apparently clever way to do (a^2 + b^2)^1/2
*/
static float pythag(float a, float b)
{
float absa=fabs(a),
absb=fabs(b);

if (absa > absb)
return absa*sqrt(1.0+DSQR(absb/absa));
else
return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+DSQR(absa/absb)));
}

static void svdcmp(double **A, int m, int n, double **u, double *w, double **v)
{
int flag,i,its,j,jj,k,l,nm=0;

double anorm,c,f,g,h,s,scale,x,y,z,
*rv1;

if(u==NULL || w==NULL || v==NULL)
fprintf(stderr, "you should get the memory before using this function, bye\n"),exit(3);

for(i=0;i<m;i++)
for(j=0;j<n;j++)
u[i][j]= A[i][j];

rv1=(double *)malloc( (size_t) n*sizeof(double) );
if(!rv1)fprintf(stderr, "cannot allocate rv1, bye");

g=scale=anorm=0.0;

for (i=0;i<n;i++) {

l=i+1;
rv1[i]=scale*g;
g=s=scale=0.0;

if (i < m)
{

for (k=i;k<m;k++)
scale += fabs(u[k][i]);

if (scale)
{

for (k=i;k<m;k++)
{
u[k][i] /= scale;
s += u[k][i]*u[k][i];
}

f=u[i][i];
g = -SIGN(sqrt(s),f);
h=f*g-s;
u[i][i]=f-g;

for (j=l;j<n;j++)
{
for (s=0.0,k=i;k<m;k++)
s += u[k][i]*u[k][j];
f=s/h;

for (k=i;k<m;k++)
u[k][j] += f*u[k][i];
}

for (k=i;k<m;k++)
u[k][i] *= scale;
}
}

w[i]=scale *g;
g=s=scale=0.0;

if (i < m && i != n-1)
{
for (k=l;k<n;k++)
scale += fabs(u[i][k]);

if (scale)
{

for (k=l;k<n;k++)
{
u[i][k] /= scale;
s += u[i][k]*u[i][k];
}

f=u[i][l];
g = -SIGN(sqrt(s),f);
h=f*g-s;
u[i][l]=f-g;

for (k=l;k<n;k++)
rv1[k]=u[i][k]/h;

for (j=l;j<m;j++)
{
for (s=0.0,k=l;k<n;k++)
s += u[j][k]*u[i][k];

for (k=l;k<n;k++)
u[j][k] += s*rv1[k];
}

for (k=l;k<n;k++)
u[i][k] *= scale;
}
}

anorm = DMAX( anorm , (fabs(w[i])+fabs(rv1[i])) );

}

for (i=n-1;i>=0;i--)
{
if (i < n-1){

if (g) {
for (j=l;j<n;j++)
v[j][i]=(u[i][j]/u[i][l])/g;

for (j=l;j<n;j++)
{
for (s=0.0,k=l;k<n;k++)
s += u[i][k]*v[k][j];

for (k=l;k<n;k++)
v[k][j] += s*v[k][i];
}
}

for (j=l;j<n;j++)
v[i][j]=v[j][i]=0.0;
}
v[i][i]=1.0;
g=rv1[i];
l=i;
}

for (i=IMIN(m-1,n-1);i>=0;i--) {

l=i+1;
g=w[i];

for (j=l;j<n;j++)
u[i][j]=0.0;

if (g)
{
g=1.0/g;

for (j=l;j<n;j++)
{
for (s=0.0,k=l;k<m;k++)
s += u[k][i]*u[k][j];

f=(s/u[i][i])*g;

for (k=i;k<m;k++)
u[k][j] += f*u[k][i];
}

for (j=i;j<m;j++)
u[j][i] *= g;
}
else
for (j=i;j<m;j++)
u[j][i]=0.0;
++u[i][i];
}

for (k=n-1;k>=0;k--) {

for (its=1;its<=CONVERGENCE;its++) {

flag=1;

for (l=k;l>0;l--) {

nm=l-1;
if ( (fabs(rv1[l])+anorm) == anorm ){
flag=0;
break;
}

if ( (fabs(w[nm])+anorm) == anorm )
break;
}

if (flag) {

c=0.0;
s=1.0;

for (i=l;i<k;i++) {

f=s*rv1[i];
rv1[i]=c*rv1[i];
if ( (fabs(f)+anorm) == anorm ) break;
g=w[i];
h=pythag(f,g);
w[i]=h;
h=1.0/h;
c=g*h;
s = -f*h;

for (j=0;j<m;j++) {

y=u[j][nm];
z=u[j][i];
u[j][nm]=y*c+z*s;
u[j][i]=z*c-y*s;
}
}
}

z=w[k];

if (l == k) {

if (z < 0.0)
{
w[k] = -z;
for (j=0;j<n;j++)
v[j][k] = -v[j][k];
}
break;
}

if (its == CONVERGENCE)
fprintf(stderr, "no convergence in %d svdcmp iterations, bye\n", CONVERGENCE), exit(5);

x=w[l];
nm=k-1;
y=w[nm];
g=rv1[nm];
h=rv1[k];
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g=pythag(f,1.0);
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
c=s=1.0;

for (j=l;j<=nm;j++) {

i=j+1;
g=rv1[i];
y=w[i];
h=s*g;
g=c*g;
z=pythag(f,h);
rv1[j]=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g = g*c-x*s;
h=y*s;
y *= c;

for (jj=0;jj<n;jj++) {

x=v[jj][j];
z=v[jj][i];
v[jj][j]=x*c+z*s;
v[jj][i]=z*c-x*s;
}

z=pythag(f,h);
w[j]=z;

if (z)
{
z=1.0/z;
c=f*z;
s=h*z;
}

f=c*g+s*y;
x=c*y-s*g;

for (jj=0;jj<m;jj++)
{
y=u[jj][j];
z=u[jj][i];
u[jj][j]=y*c+z*s;
u[jj][i]=z*c-y*s;
}
}
rv1[l]=0.0;
rv1[k]=f;
w[k]=x;
}
}

free(rv1);
}

/*
Calculating the inverse of a matrix M1 by SVD
*/

// :: FOR LARGE MATRICES THIS BECOMES NUMERICALLY UNSTABLE :: ANY BETTER OPTION AVAILABLE? :: ALTERNATIVELY ONE COULD JUST DO BRUTE FORCE MATRIX MULTIPLICATION :: WOULD BE OF COMPLEXITY O(x n^2) FOR A MATRIX OF DIMENSION n THAT IS RAISED TO THE POWER x ::

void inverseMatrix(struct matrix * M1, struct matrix * inverseM)
{

struct matrix U,w,V;    /* misc , SVD decomposition (U,w,V) and matrix product (Tmp) */

int i,j,k;                  /* counters */

/*
get memory for the matrices
*/
U = MemMat( M1->r, M1->c);
w = MemMat( 1, M1->c );
V = MemMat( M1->c, M1->c );

/*
svdcmp(double **A, int m, int n, double **u, double *w, double **v)
*/

svdcmp(M1->val, M1->r, M1->c, U.val, w.val[0], V.val);

/*
Check the w's for 0 values
*/
for(k = 0; k < inverseM->r; k++)
{
if( w.val[0][k] < 1e-6 )
{
fprintf(stderr, "warning w[%d]=%e, need extra check\n", k, w.val[0][k]);
}
}

/*
This is equivalent to
V . Diag(w)^-1 . Ut
but more efficient in computation time O(m n^2)
*/
for(i = 0; i < inverseM->r; i++ )
{
for(j = 0; j < inverseM->c; j++ )
{
inverseM->val[i][j]=0;

for(k = 0; k < inverseM->r; k++ )
{
inverseM->val[i][j] += V.val[i][k]*U.val[j][k]/w.val[0][k];
}
}
}

FreeMat(&U);
FreeMat(&V);
FreeMat(&w);
}

struct matrix LeastSquare( struct matrix *X, struct matrix *Y ){

struct matrix beta;          /* this will contains the estimates you aim at */

struct matrix Tmp, U,w,V;    /* misc , SVD decomposition (U,w,V) and matrix product (Tmp) */

int i,j,k;                  /* counters */

/*
get memory for the matrices
*/
beta = MemMat( X->c, 1);
Tmp = MemMat( X->c, X->r);
U = MemMat( X->r, X->c);
w = MemMat( 1, X->c );
V = MemMat( X->c, X->c );

/*
svdcmp(double **A, int m, int n, double **u, double *w, double **v)
*/

svdcmp(X->val,  X->r, X->c, U.val, w.val[0], V.val);

/*
Check the w's for 0 values
*/
for(k=0; k<Tmp.r; k++)
if( w.val[0][k] < 1e-6 )
fprintf(stderr, "warning w[%d]=%e, need extra check\n", k, w.val[0][k]);

/*
This is equivalent to
V . Diag(w)^-1 . Vt
but more efficient in computation time O(m n^2)
*/
for(i=0; i<Tmp.r; i++ )
for(j=0; j<Tmp.c; j++ )
{
Tmp.val[i][j]=0;

for(k=0; k<Tmp.r; k++ )
Tmp.val[i][j] += V.val[i][k]*U.val[j][k]/w.val[0][k];

}

/*
From there, estimate the beta's
*/
beta = MatrixProduct( &Tmp , Y, 0 );

FreeMat(&U);
FreeMat(&V);
FreeMat(&w);
FreeMat(&Tmp);

return beta;

}

```