/* Copyright 2002 Ben Blum, Christian Shelton
 *
 * This file is part of GameTracer.
 *
 * GameTracer 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.
 *
 * GameTracer 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.
 *
 * You should have received a copy of the GNU General Public License
 * along with GameTracer; if not, write to the Free Software Foundation, 
 * Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include "cmatrix.h"
#include "math.h"
#include "float.h"
cvector::~cvector() { delete []x; }
// adopted from NRiC, pg 45

cmatrix::~cmatrix()
 { delete []x; }

int cvector::num_vec_cons = 0;
cmatrix cmatrix::inv(bool &worked) const {
	if (m!=n) {
		cerr << "invalid cmatrix inverse" << endl;
		exit(1);
	}
	cmatrix temp(n,n);
	int *ix = new int[n];
	
	if (!LUdecomp(temp,ix)) {
		worked = false;
		delete []ix;
		return cmatrix(n,n,0,false);
	}
	worked = true;

	cmatrix ret(n,n);
	int i,j;
	double *col = new double[n];
	for(j=0;j<n;j++) {
		for(i=0;i<n;i++) col[i] = 0;
		col[j] = 1;
		temp.LUbacksub(ix,col);
		for(i=0;i<n;i++) ret.x[i*n+j] = col[i];
	}
	delete []col;
	delete []ix;
	return ret;
}

// adopted from NRiC, pg 43
int cmatrix::LUdecomp(cmatrix &LU, int *ix) const {
	if (m!=n||LU.m!=LU.n||LU.n!=n) {
		cerr << "invalid cmatrix in LUdecomp" << endl;
		exit(1);
	}
	int d=1,i,j,k;
	LU = *this;
	double *vv = new double[n];
	double dum;

	k = 0;
	for(i=0;i<n;i++) {
		vv[i] = fabs(x[k]); k++;
		for(j=1;j<n;j++,k++) if(vv[i]<(dum=fabs(x[k]))) vv[i]=dum;
		if (vv[i]==(double)0.0) {
			delete []vv;
			return 0;
		}
		vv[i] = 1/vv[i];
	}
	double sum,big;
	int imax;
	for(j=0;j<n;j++) {
		for(i=0;i<j;i++) {
			sum = LU.x[i*n+j];
			for(k=0;k<i;k++) sum -= LU.x[i*n+k]*LU.x[k*n+j];
			LU.x[i*n+j] = sum;
		}
		big = 0;
		for(i=j;i<n;i++) {
			sum = LU.x[i*n+j];
			for(k=0;k<j;k++) sum -= LU.x[i*n+k]*LU.x[k*n+j];
			LU.x[i*n+j] = sum;
			if ((dum=vv[i]*fabs(sum))>=big) {
				big = dum;
				imax = i;
			}
		}
		if (j!=imax) {
			for(k=0;k<n;k++) {
				dum = LU.x[imax*n+k];
				LU.x[imax*n+k] = LU.x[j*n+k];
				LU.x[j*n+k] = dum;
			}
			d = -d;
			vv[imax] = vv[j];
		}
		ix[j] = imax;
		if (LU.x[j*n+j] == 0) {
			LU.x[j*n+j] = (double)1.0e-20;
		}
		if (j!=n-1) {
			dum = 1/LU.x[j*n+j];
			for(i=j+1;i<n;i++) LU.x[i*n+j] *= dum;
		}
	}
	delete []vv;
	return d;
}

void cmatrix::LUbacksub(int *ix, double *b) const {
	if (n!=m) {
		cerr << "invalid cmatrix in LUbacksub" << endl;
		exit(1);
	}
	int ip,ii=-1,i;
	double sum;

	for(i=0;i<n;i++) {
		ip = ix[i];
		sum = b[ip];
		b[ip] = b[i];
		if (ii!=-1)
			for(int j=ii;j<=i-1;j++) sum -= x[i*n+j]*b[j];
		else if (sum!=0) ii=i;
		b[i] = sum;
	}
	for(i=n-1;i>=0;i--) {
		sum = b[i];
		for(int j=i+1;j<=n-1;j++) sum -= x[i*n+j]*b[j];
		b[i] = sum/x[i*n+i];
	}
}

bool cmatrix::solve(cvector &b, cvector &ret) {
	if (m!=n) {
		cerr << "invalid cmatrix in solve" << endl;
		exit(1);
	}
	for(int i=0;i<n;i++) ret[i] = b[i];
	int *ix = new int[n];
	cmatrix a(n,n);
	
	if (!LUdecomp(a,ix)) {
		delete []ix;
		return false;
	}
	a.LUbacksub(ix,ret.values());
	delete []ix;
	return true;
}
double *cmatrix::solve(const double *b, bool &worked) const {
	if (m!=n) {
		cerr << "invalid cmatrix in solve" << endl;
		exit(1);
	}
	double *ret = new double[n];
	for(int i=0;i<n;i++) ret[i] = b[i];
	int *ix = new int[n];
	cmatrix a(n,n);
	
	if (!LUdecomp(a,ix)) {
		worked = false;
		delete []ix;
		return ret;
	}
	worked=true;
	a.LUbacksub(ix,ret);
	delete []ix;
	return ret;
}

double cmatrix::pythag(double a, double b) {
	double absa,absb;
	absa = fabs(a);
	absb = fabs(b);
	if (absa>absb) {
		double sqr = absb/absa;
		return absa*sqrt(1.0+sqr*sqr);
	} else {
		if (absb==0.0) return 0;
		double sqr = absa/absb;	
		return absb*sqrt(1.0+sqr*sqr);
	}
}

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

// adopted from pg 67 of NRiC
void cmatrix::svd(cmatrix &u, cmatrix &v, double *w) {

	u = *this;
	if (v.n!=n || v.m!=n) {
		delete []v.x;
		v.n = n; v.m = n;
		v.s = n*n;
		v.x = new double[v.s];
	}

	int flag,i,its,j,jj,k,l,nm;
	double anorm,c,f,g,h,s,scale,x,y,z,*rv1;

	rv1 = new double[n];
	g=scale=anorm=(double)0;
	for(i=0;i<n;i++) {
		l = i+1;
		rv1[i] = scale*g;
		g=s=scale=(double)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=(double)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=(double)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;
			}
		}
		double temp = fabs(w[i])+fabs(rv1[i]);
		anorm = anorm>temp ? anorm : temp;
	}
	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=(double)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]=(double)0;
		}
		v[i][i] = (double)1;
		g=rv1[i];
		l=i;
	}
	for(i=m>n?n-1:m-1;i>=0;i--) {
		l=i+1;
		g=w[i];
		for(j=l;j<n;j++) u[i][j] = (double)0;
		if (g) {
			g = 1/g;
			for(j=l;j<n;j++) {
				for(s=(double)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] = (double)0;
		++u[i][i];
	}
	for(k=n-1;k>=0;k--) {
		for(its=1;its<=30;its++) {
			flag = 1;
			for(l=k;l>=0;l--) {
				nm = l-1;
				if ((double)(fabs(rv1[l])+anorm)==anorm) {
					flag = 0;
					break;
				}
				if ((double)(fabs(w[nm])+anorm)==anorm) break;
			}
			if (flag) {
				c = (double)0;
				s = (double)1;
				for(i=l;i<=k;i++) {
					f = s*rv1[i];
					rv1[i] = c*rv1[i];
					if ((double)(fabs(f)+anorm)==anorm) break;
					g = w[i];
					h = pythag(f,g);
					w[i] = h;
					h = 1/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==30) {
				// some other method (like an error return
				// value should be put here)
				cerr << "no convergence after 30 svdcmp "
					"iterations" << endl;
				exit(1);
			}
			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*h*y);
			g = pythag(f,1.0);
			f = ((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
			c=s=1;
			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/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;
			rv1[k] = f;
			w[k] = x;
		}
	}
	delete []rv1;
}

double cmatrix::adjoint() {
  int i, j, i0, j0, maxi, lastj = -1;
  double max, pivot;
  int r[m];
  int r2[m];
  int c[m];
  double D = 1.0;
  double retval[m][m];
  for(i = 0; i < m; i++)
    for(j = 0; j < m; j++)
      retval[i][j] = x[i*n+j];

  for(i= 0; i < m; i++) {
    r[i] = -1;
  }
  for(j = 0; j < m; j++) {
    if(D == 0.0)
      return DBL_MAX;
    max = -1.0;
    maxi = -1;
    for(i = 0; i < m; i++) {
      if(r[i] < 0 && fabs(retval[i][j]) > max) {
	max = fabs(retval[i][j]);
	maxi = i;
      }
    }
    if(j != lastj && max == 0.0) {
      if(lastj >= 0)
        return DBL_MAX;
      lastj = j;
      if(j != m-1)
	continue;
    }
    if(maxi == -1) {
      cout << "oops";
      return DBL_MAX;
    }

    i = maxi;
    pivot = retval[i][j];
    for(i0 = 0; i0 < m; i0++) {
      if(i0 != i) {
	for(j0 = 0; j0 < m; j0++) {
	  if(j0 != j) {
	    retval[i0][j0] *= pivot;
	    retval[i0][j0] -= retval[i0][j] * retval[i][j0];
	    retval[i0][j0] /= D;
	  }
	}
      }
    }
    for(i0 = 0; i0 < m; i0++) {
      retval[i0][j] = -retval[i0][j];
    }
    retval[i][j] = D;
    D = pivot;
    r[i] = j;
    c[j] = i;
    if(j == lastj)
      break;
    if(j == m-1 && lastj >= 0)
      j = lastj - 1;
  }
  //  cout << retval << endl << endl;
  int s=0;
  i = 0;
  memcpy(r2, r, m * sizeof(int));
  while(i < m-1) {
    j = r2[i];
    if(i != j) {
      s++;
      r2[i] = r2[j];
      r2[j] = j;
    } else
      i++;
  }
  for(i = 0; i < m; i++)
    for(j = 0; j < m; j++)
      x[i*n+j] = retval[c[i]][r[j]];
  if(s%2 == 1) {
    negate();
    D = -D;
  }
  // cout << *this << endl << endl;
  return D;
}

double cmatrix::testAdjoint()
//returns the characteristic polynomial and adjoint cmatrix
{
  cmatrix c(n,n,1.0,1), p(n,n);
  int i = 0, j;
  double det,b;

  for (;;) {
    i++;
    if (i == n) break;
    p = *this * c;
    c = p;
    b = - c.trace() / i;
    for (j = 0; j < n; j++) c[j][j] += b;
  }
  p = m * c;
  det = - p.trace() / n;
  b = 1 - 2*((n - 1) % 2);
  *this = c * b;
  return det;
}

double cmatrix::trace() {
  assert(n == m);
  double sum = 0.0;
  for(int i = 0; i < n; i++) {
    sum += x[i*n+i];
  }
  return sum;
}