 /******************
 * Simple Math     *
 *                 *
 * Marcelo Gattass *
 * Dec01,2005      *
 ******************/

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

/* Utility **********************************************************/
static double sqrarg;
#define SQR(a) ((sqrarg=(a)) == 0.0f ? 0.0f : sqrarg*sqrarg)

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

static double maxarg1,maxarg2;
#define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
        (maxarg1) : (maxarg2))

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

static double pythag(double a, double b)
{
	double absa,absb;
	absa=fabs(a);
	absb=fabs(b);
	if (absa > absb) return (absa*sqrt(1.0+SQR(absb/absa)));
	else return (absb == 0.0f ? 0.0f : (absb*sqrt(1.0+SQR(absa/absb))));
}

/* Exported functions ***********************************************/
int smSVD(double*a, int m, int n, double* u, double* d, double* v, double* tmp)
{
   int flag,i,its,j,jj,k,l,nm;
   double anorm,c,f,g,h,s,scale,x,y,z;

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

   g=scale=anorm=0.0;
   for (i=0;i<n;i++) {
      l=i+2;
      tmp[i]=scale*g;
      g=s=scale=0.0;
      if (i < m) {
         for (k=i;k<m;k++) scale +=  fabs(u[k*n+i]);
         if (scale != 0.0) {
            for (k=i;k<m;k++) {
               u[k*n+i] /= scale;
               s += u[k*n+i]*u[k*n+i];
            }
            f=u[i*n+i];
            g = -SIGN(sqrt(s),f);
            h=f*g-s;
            u[i*n+i]=f-g;
            for (j=l-1;j<n;j++) {
               for (s=0.0,k=i;k<m;k++) s += u[k*n+i]*u[k*n+j];
               f=s/h;
               for (k=i;k<m;k++) u[k*n+j] += f*u[k*n+i];
            }
            for (k=i;k<m;k++) u[k*n+i] *= scale;
         }
      }
      d[i]=scale *g;
      g=s=scale=0.0;
      if (i+1 <= m && i+1 != n) {
         for (k=l-1;k<n;k++) scale += fabs(u[i*n+k]);
         if (scale!=0.0) {
            for (k=l-1;k<n;k++) {
               u[i*n+k] /= scale;
               s += u[i*n+k]*u[i*n+k];
            }
            f=u[i*n+l-1];
            g = -SIGN(sqrt(s),f);
            h=f*g-s;
            u[i*n+l-1]=f-g;
            for (k=l-1;k<n;k++) tmp[k]=u[i*n+k]/h;
            for (j=l-1;j<m;j++) {
               for (s=0.0,k=l-1;k<n;k++) s += u[j*n+k]*u[i*n+k];
               for (k=l-1;k<n;k++) u[j*n+k] += s*tmp[k];
            }
           for (k=l-1;k<n;k++) u[i*n+k] *= scale;
         }
      }
      anorm=FMAX(anorm,(fabs(d[i])+fabs(tmp[i])));
   }
   for (i=n-1;i>=0;i--) {
      if (i < n-1) {
         if (g!=0.0) {
           for (j=l;j<n;j++)
               v[j*n+i]=(u[i*n+j]/u[i*n+l])/g;
            for (j=l;j<n;j++) {
               for (s=0.0,k=l;k<n;k++) s += u[i*n+k]*v[k*n+j];
               for (k=l;k<n;k++) v[k*n+j] += s*v[k*n+i];
            }
         }
         for (j=l;j<n;j++) v[i*n+j]=v[j*n+i]=0.0;
      }
      v[i*n+i]=1.0;
      g=tmp[i];
      l=i;
   }
   for (i=IMIN(m,n)-1;i>=0;i--) {
      l=i+1;
      g=d[i];
      for (j=l;j<n;j++) u[i*n+j]=0.0;
      if (g != 0.0) {
         g=1.0/g;
         for (j=l;j<n;j++) {
            for (s=0.0,k=l;k<m;k++) s += u[k*n+i]*u[k*n+j];
            f=(s/u[i*n+i])*g;
            for (k=i;k<m;k++) u[k*n+j] += f*u[k*n+i];
         }
         for (j=i;j<m;j++) u[j*n+i] *= g;
      } else for (j=i;j<m;j++) u[j*n+i]=0.0;
      ++u[i*n+i];
   }
   for (k=n-1;k>=0;k--) {
      for (its=0;its<30;its++) {
         flag=1;
         for (l=k;l>=0;l--) {
            nm=l-1;
            if ((fabs(tmp[l])+anorm) == anorm) {
               flag=0;
               break;
            }
            if ((fabs(d[nm])+anorm) == anorm) break;
         }
         if (flag) {
            c=0.0;
            s=1.0;
            for (i=l;i<k+1;i++) {
               f=s*tmp[i];
               tmp[i]=c*tmp[i];
               if ((fabs(f)+anorm) == anorm) break;
               g=d[i];
               h=pythag(f,g);
               d[i]=h;
               h=1.0/h;
               c=g*h;
               s = -f*h;
               for (j=0;j<m;j++) {
                  y=u[j*n+nm];
                  z=u[j*n+i];
                  u[j*n+nm]=y*c+z*s;
                  u[j*n+i]=z*c-y*s;
               }
            }
         }
         z=d[k];
         if (l == k) {
            if (z < 0.0) {
               d[k] = -z;
               for (j=0;j<n;j++) v[j*n+k] = -v[j*n+k];
            }
            break;
         }
         if (its == 29) return 0;
         x=d[l];
         nm=k-1;
         y=d[nm];
         g=tmp[nm];
         h=tmp[k];
         f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0f*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=tmp[i];
            y=d[i];
            h=s*g;
            g=c*g;
            z=pythag(f,h);
            tmp[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*n+j];
               z=v[jj*n+i];
               v[jj*n+j]=x*c+z*s;
               v[jj*n+i]=z*c-x*s;
            }
            z=pythag(f,h);
            d[j]=z;
            if (z) {
               z=1.0f/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*n+j];
               z=u[jj*n+i];
               u[jj*n+j]=y*c+z*s;
               u[jj*n+i]=z*c-y*s;
            }
         }
         tmp[l]=0.0;
         tmp[k]=f;
         d[k]=x;
      }
   }
   return 1;
}

/* 
 * Add the tensor product of the vector {v} (i.e., {v}{v}T) 
 * to the matrix [A].  
 * [A]+={v}{v}T
 *
 */
void smAddMatVecTensor(double* a, double* v, int n)
{
	int i,j;
   for (i=0;i<n;i++) {
      for (j=0;j<n;j++) { 
			a[i*n+j]+=v[i]*v[j];
      }
   }
}

/* {x}=[A]{b}    Dimensions: [A]=mxn, {b}=m and {x}=n. */
void smAb(double* a, double* b, int m, int n, double* x)
{
   int i,j;
   for (i=0;i<m;i++) {
      x[i]=0.0;
      for (j=0;j<n;j++)
         x[i]+=a[i*n+j]*b[j];
   }
}

/* {x}=[A]T{b}    Dimensions: [A]=mxn, {b}=m and {x}=n. */
void smAtb(double* a, double* b, int m, int n, double* x)
{
   int i,j;
   for (i=0;i<n;i++) {
      x[i]=0.0;
      for (j=0;j<m;j++)
         x[i]+=a[j*n+i]*b[j];
   }
}


/* [X]=[A][B]    Dimensions: [A]=mxp, [B]=pxn and [X]=mxn. */
void smAB(double* a, double* b, int m, int p, int n, double* x)
{
   int i,j,k;
   for (i=0;i<m;i++) {
      for (j=0;j<n;j++) {
         x[i*n+j]=0.0;
         for (k=0;k<p;k++)
                x[i*n+j]+=a[i*p+k]*b[k*n+j];
      }
   }
}

/* [X]=[A][B]T    Dimensions: [A]=mxp, [B]=nxp and [X]=mxn. */
void smABt(double* a, double* b, int m, int p, int n, double* x)
{
   int i,j,k;
   for (i=0;i<m;i++) {
      for (j=0;j<n;j++) {
         x[i*n+j]=0.0;
         for (k=0;k<p;k++)
                x[i*n+j]+=a[i*p+k]*b[j*p+k];
      }
   }
}

/*  [X]=[A]T[B]    Dimensions: [A]=mxp, [B]=mxn and [X]=pxn */
void smAtB(double* a, double* b, int m, int p, int n, double* x)
{
   int i,j,k;
   for (i=0;i<p;i++) {
      for (j=0;j<n;j++) {
         x[i*n+j]=0.0;
         for (k=0;k<m;k++)
                x[i*n+j]+=a[k*p+i]*b[k*n+j];
      }
   }
}

/*  [X]=[A]+[B]    Dimensions: [A]=mxn, [B]=mxn and [X]=mxn */
void smAddMat(double* a, double* b, int m, int n, double* x)
{
   int i,j;
   for (i=0;i<m;i++)
      for (j=0;j<n;j++)
         x[i*n+j]=a[i*n+j]+b[i*n+j];
}

/*  [X]=[A]-[B]    Dimensions: [A]=mxn, [B]=mxn and [X]=mxn */
void smSubMat(double* a, double* b, int m, int n, double* x)
{
   int i,j;
   for (i=0;i<m;i++)
      for (j=0;j<n;j++)
         x[i*n+j]=a[i*n+j]-b[i*n+j];
}


/*  [X]=s[A]    Dimensions: [A]=mxn and [X]=mxn */
void smScaMat(double* a, double s,int m, int n, double* x)
{
   int i,j;
   for (i=0;i<m;i++)
      for (j=0;j<n;j++)
         x[i*n+j]=s*a[i*n+j];
}


/*  [X]=[A]T    Dimensions: [A]=mxn and [X]=nxm */
void smAt(double* a, int m, int n, double* x)
{
   int i,j;
   for (i=0;i<m;i++)
      for (j=0;j<n;j++)
         x[j*m+i]=a[i*n+j];
}

/*  [X]=[A]    Dimensions: [A]=mxn and [X]=mxn */
void smMatCopy(double* a, int m, int n, double* x)
{
   int i,j;
   for (i=0;i<m;i++) 
      for (j=0;j<n;j++) 
         x[i*n+j]=a[i*n+j];
}

/*  {x}={v}+{u}  Dimensions: {v}=m, {u}=m and {x}=m */
void smAddVec(double* u, double* v, int m, double* x)
{
   int i;
   for (i=0;i<m;i++) x[i]=u[i]+v[i];
}

/*  {x}={v}-{u}  Dimensions: {v}=m, {u}=m and {x}=m */
void smSubVec(double* u, double* v, int m, double* x)
{
   int i;
   for (i=0;i<m;i++) x[i]=u[i]-v[i];
}

/*  {x}=s{u}  Dimensions: {u}=m and {x}=m */
void smScaVec(double* u, int m,  double s, double* x)
{
   int i;
   for (i=0;i<m;i++) x[i]=s*u[i];
}

/*  s={v}.{u}  Dimensions: {v}=m and  {u}=m */
double smDotProd(double* u, double* v, int m)
{
   double tmp=0;
   int i;
   for (i=0;i<m;i++) 
      tmp+=u[i]*v[i];
   return tmp;
}

/* Print matrix */
void smShowMat(char * title, double *a, int m, int n)
{
	int i,j;
	printf("\n%s",title);
	for (i=0;i<m;i++){
		for (j=0;j<n;j++){
            if ((j%5)==0) printf("\n");
			printf("%12.6lf ", a[i*n+j]);
		}
	}
}

/* Print vector */
void smShowVec(char * title, double *v, int n)
{
	int j;
	printf("\n%s",title);
	for (j=0;j<n;j++){
            if ((j%5)==0) printf("\n");
			printf("%12.6lf ", v[j]);
		}
}
