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

#include "sysdep.h"
#include "linear.h"
#include "output.h"
#include "mainloop.h"
#include "interval.h"

char *ram;
static int *perm,*col,signdet,luflag=0;
static double **lumat,*c,det;
static complex **c_lumat,*c_c,c_det;
static int rank;

static long nmark;

#define superalign(n) ((((n)-1)/ALIGNMENT+1)*ALIGNMENT)
#define increase(r,n) nmark=superalign(n); if (!freeramfrom((r),(nmark))) outofram(); (r)+=nmark;

/***************** real linear systems *******************/

static void lu (double *a, int n, int m)
/***** lu
	lu decomposition of a
*****/
{	int i,j,k,mm,j0,kh,j1;
	double *d,piv,temp,*temp1;

	if (!luflag)
	{
		/* get place for result c and move a to c */
		c=(double *)ram;
		increase(ram,(long)n*m*sizeof(double));
		memmove((char *)c,(char *)a,(long)n*m*sizeof(double));
	}
	else c=a;

	/* inititalize lumat */
	lumat=(double **)ram;
	increase(ram,(long)n*sizeof(double *));
	d=c;
	for (i=0; i<n; i++) { lumat[i]=d; d+=m; }

	/* get place for perm */
	perm=(int *)ram;
	increase(ram,(long)n*sizeof(int));

	/* get place for col */
	col=(int *)ram;
	increase(ram,(long)m*sizeof(int));

	/* gauss algorithm */
	for (k=0; k<n; k++) perm[k]=k;
	signdet=1; rank=0; det=1.0; k=0;
	for (kh=0; kh<m; kh++)
	{   piv=fabs(lumat[k][kh]);
		if (!luflag)
		{	for (j=kh+1; j<m; j++) piv+=fabs(lumat[k][j]);
			if (piv==0)
			{	output("Determinant zero!\n");
				error=131; return;
			}
			piv=fabs(lumat[k][kh])/piv;
		}
		j0=k;
		for (j=k+1; j<n; j++)
		{	temp=fabs(lumat[j][kh]);
			if (!luflag)
			{	for (j1=kh+1; j1<m; j1++) temp+=fabs(lumat[j][j1]);
				if (temp==0)
				{	output("Determinant zero!\n");
					error=131; return;
				}
				temp=fabs(lumat[j][kh])/temp;
			}
			if (piv<temp)
			{	piv=temp; j0=j;
			}
		}
		if (luflag && piv<epsilon)
		{	signdet=0;
			col[kh]=0;
		}
		else
		{	col[kh]=1; rank++;
			det*=lumat[j0][kh];
			if (!luflag && det==0)
			{	output("Determinant zero!\n");
				error=131; return;
			}
			if (j0!=k)
			{	signdet=-signdet;
				mm=perm[j0]; perm[j0]=perm[k]; perm[k]=mm;
				temp1=lumat[j0]; lumat[j0]=lumat[k]; lumat[k]=temp1;
			}
			for (j=k+1; j<n; j++)
				if (lumat[j][kh] != 0.0)
				{	lumat[j][kh] /= lumat[k][kh];
					for (temp=lumat[j][kh], mm=kh+1; mm<m; mm++)
						lumat[j][mm]-=temp*lumat[k][mm];
				}
			k++;
			if (k>=n) { kh++; break; }
        }
	}
	if (k<n || kh<m)
	{	signdet=0;
		if (!luflag)
		{	error=131; output("Determinant zero!\n");
			return;
		}
	}
	for (j=kh; j<m; j++) col[j]=0;
	det=det*signdet;
}

void solvesim (double *a, int n, double *rs, int m, double *res)
/**** solvesim
	solve simultanuously a linear system.
****/
{	double **x,**b,*h,sum;
	int i,k,l,j;
	ram=newram; luflag=0;
	lu(a,n,n); if (error) return;

	/* initialize x and b */
	x=(double **)ram;
	increase(ram,(long)n*sizeof(double *));
	h=res; for (i=0; i<n; i++) { x[i]=h; h+=m; }
	b=(double **)ram;
	increase(ram,(long)n*sizeof(double *));
	h=rs; for (i=0; i<n; i++) { b[i]=h; h+=m; }

	for (l=0; l<m; l++)
	{	x[0][l]=b[perm[0]][l];
		for (k=1; k<n; k++)
		{	x[k][l]=b[perm[k]][l];
			for (j=0; j<k; j++)
				x[k][l] -= lumat[k][j]*x[j][l];
		}
		x[n-1][l] /= lumat[n-1][n-1];
		for (k=n-2; k>=0; k--)
		{	for (sum=0.0, j=k+1; j<n; j++)
				sum+=lumat[k][j]*x[j][l];
			x[k][l] = (x[k][l]-sum)/lumat[k][k];
		}
	}
}

static void make_lu (double *a, int n, int m,
	int **rows, int **cols, int *rankp,
	double *detp)
{	ram=newram;
	luflag=1; lu(a,n,m); newram=ram;
	if (error) return;
	*rows=perm; *cols=col; *rankp=rank; *detp=det;
}

static void lu_solve (double *a, int n, double *rs, int m, double *res)
{	double **x,**b,*h,sum,*d;
	int i,k,l,j;
	ram=newram;
	
	/* initialize x and b */
	x=(double **)ram;
	increase(ram,(long)n*sizeof(double *));
	h=res; for (i=0; i<n; i++) { x[i]=h; h+=m; }

	b=(double **)ram;
	increase(ram,(long)n*sizeof(double *));
	h=rs; for (i=0; i<n; i++) { b[i]=h; h+=m; }
	
	/* inititalize lumat */
	lumat=(double **)ram;
	increase(ram,(long)n*sizeof(double *));
	d=a;
	for (i=0; i<n; i++) { lumat[i]=d; d+=n; }
	
	for (l=0; l<m; l++)
	{	x[0][l]=b[0][l];
		for (k=1; k<n; k++)
		{	x[k][l]=b[k][l];
			for (j=0; j<k; j++)
				x[k][l] -= lumat[k][j]*x[j][l];
		}
		x[n-1][l] /= lumat[n-1][n-1];
		for (k=n-2; k>=0; k--)
		{	for (sum=0.0, j=k+1; j<n; j++)
				sum+=lumat[k][j]*x[j][l];
			x[k][l] = (x[k][l]-sum)/lumat[k][k];
		}
	}
}

/******************* complex linear systems **************/

void c_add (complex x, complex y, complex z)
{	z[0]=x[0]+y[0];
	z[1]=x[1]+y[1];
}

void c_sub (complex x, complex y, complex z)
{	z[0]=x[0]-y[0];
	z[1]=x[1]-y[1];
}

void c_mult (complex x, complex y, complex z)
{	double h;
	h=x[0]*y[0]-x[1]*y[1];
	z[1]=x[0]*y[1]+x[1]*y[0];
	z[0]=h;
}

void c_div (complex x, complex y, complex z)
{	double r,h;
	r=y[0]*y[0]+y[1]*y[1];
	h=(x[0]*y[0]+x[1]*y[1])/r;
	z[1]=(x[1]*y[0]-x[0]*y[1])/r;
	z[0]=h;
}

double c_abs (complex x)
{	return sqrt(x[0]*x[0]+x[1]*x[1]);
}

void c_copy (complex x, complex y)
{	x[0]=y[0];
	x[1]=y[1];
}

static void c_lu (double *a, int n, int m)
/***** clu
	lu decomposition of a
*****/
{	int i,j,k,mm,j0,kh,j1;
	double piv,temp;
	complex t,*h,*temp1;

	if (!luflag)
	{	/* get place for result c and move a to c */
		c_c=(complex *)ram;
		increase(ram,(long)n*m*sizeof(complex));
		memmove((char *)c_c,(char *)a,(long)n*m*sizeof(complex));
	}
	else c_c=(complex *)a;

	/* inititalize c_lumat */
	c_lumat=(complex **)ram;
	increase(ram,(long)n*sizeof(complex *));
	h=c_c;
	for (i=0; i<n; i++) { c_lumat[i]=h; h+=m; }

	/* get place for perm */
	perm=(int *)ram;
	increase(ram,(long)n*sizeof(int));

	/* get place for col */
	col=(int *)ram;
	increase(ram,(long)m*sizeof(int));

	for (k=0; k<n; k++) perm[k]=k;
	signdet=1; rank=0; c_det[0]=1.0; c_det[1]=0.0; k=0;
	for (kh=0; kh<m; kh++)
	{	piv=c_abs(c_lumat[k][kh]);
		if (!luflag)
		{	for (j=kh+1; j<m; j++)
			{	piv+=c_abs(c_lumat[k][j]);
			}
			if (piv==0)
			{	output("Determinant zero!\n");
				error=131; return;
			}
			piv=c_abs(c_lumat[k][kh])/piv;
		}
		j0=k;
		for (j=k+1; j<n; j++)
		{	temp=c_abs(c_lumat[j][kh]);
			if (!luflag)
			{	for (j1=kh+1; j1<m; j1++) temp+=c_abs(c_lumat[j][j1]);
				if (temp==0)
				{	output("Determinant zero!\n");
					error=131; return;
				}
				temp=c_abs(c_lumat[j][kh])/temp;
			}
			if (piv<temp)
			{	piv=temp; j0=j;
			}
		}
		if (luflag && piv<epsilon)
		{	signdet=0;
			if (luflag)
			{	col[kh]=0;
				continue;
			}
		}
		else
		{	col[kh]=1; rank++;
			c_mult(c_det,c_lumat[j0][kh],c_det);
			if (!luflag && c_det[0]==0 && c_det[1]==0)
			{	output("Determinant zero!\n");
				error=131; return;
			}
		}
		if (j0!=k)
		{	signdet=-signdet;
			mm=perm[j0]; perm[j0]=perm[k]; perm[k]=mm;
			temp1=c_lumat[j0]; c_lumat[j0]=c_lumat[k];
				c_lumat[k]=temp1;
		}
		for (j=k+1; j<n; j++)
			if (c_lumat[j][kh][0] != 0.0 || c_lumat[j][kh][1]!=0.0)
			{	c_div(c_lumat[j][kh],c_lumat[k][kh],c_lumat[j][kh]);
				for (mm=kh+1; mm<m; mm++)
				{	c_mult(c_lumat[j][kh],c_lumat[k][mm],t);
					c_sub(c_lumat[j][mm],t,c_lumat[j][mm]);
				}
			}
		k++;
		if (k>=n) { kh++; break; }
	}
	if (k<n || kh<m)
	{	signdet=0;
		if (!luflag)
		{	error=131; output("Determinant zero!\n");
			return;
		}
	}
	for (j=kh; j<m; j++) col[j]=0;
	c_det[0]=c_det[0]*signdet; c_det[1]=c_det[1]*signdet;
}

void c_solvesim (double *a, int n, double *rs, int m, double *res)
/**** solvesim
	solve simultanuously a linear system.
****/
{	complex **x,**b,*h;
	complex sum,t;
	int i,k,l,j;
	ram=newram;
	luflag=0; c_lu(a,n,n); if (error) return;
	
	/* initialize x and b */
	x=(complex **)ram;
	increase(ram,(long)n*sizeof(complex *));
	h=(complex *)res; for (i=0; i<n; i++) { x[i]=h; h+=m; }

	b=(complex **)ram;
	increase(ram,(long)n*sizeof(complex *));
	h=(complex *)rs; for (i=0; i<n; i++) { b[i]=h; h+=m; }
	
	for (l=0; l<m; l++)
	{	c_copy(x[0][l],b[perm[0]][l]);
		for (k=1; k<n; k++)
		{	c_copy(x[k][l],b[perm[k]][l]);
			for (j=0; j<k; j++)
			{	c_mult(c_lumat[k][j],x[j][l],t);
				c_sub(x[k][l],t,x[k][l]);
			}
		}
		c_div(x[n-1][l],c_lumat[n-1][n-1],x[n-1][l]);
		for (k=n-2; k>=0; k--)
		{	sum[0]=0; sum[1]=0.0;
			for (j=k+1; j<n; j++)
			{	c_mult(c_lumat[k][j],x[j][l],t);
				c_add(sum,t,sum);
			}
			c_sub(x[k][l],sum,t);
			c_div(t,c_lumat[k][k],x[k][l]);
		}
	}
}

static void cmake_lu (double *a, int n, int m,
	int **rows, int **cols, int *rankp,
	double *detp, double *detip)
{	ram=newram;
	luflag=1; c_lu(a,n,m); newram=ram;
	if (error) return;
	*rows=perm; *cols=col; *rankp=rank; 
	*detp=c_det[0]; *detip=c_det[1];
}

static void clu_solve (double *a, int n, double *rs, int m, double *res)
/**** solvesim
	solve simultanuously a linear system.
****/
{	complex **x,**b,*h;
	complex sum,t;
	int i,k,l,j;
	ram=newram;
	
	/* initialize x and b */
	x=(complex **)ram;
	increase(ram,(long)n*sizeof(complex *));
	h=(complex *)res; for (i=0; i<n; i++) { x[i]=h; h+=m; }
	
	b=(complex **)ram;
	increase(ram,(long)n*sizeof(complex *));
	h=(complex *)rs; for (i=0; i<n; i++) { b[i]=h; h+=m; }
	
	/* inititalize c_lumat */
	c_lumat=(complex **)ram;
	increase(ram,(long)n*sizeof(complex *));
	h=(complex *)a;
	for (i=0; i<n; i++) { c_lumat[i]=h; h+=n; }
	
	for (l=0; l<m; l++)
	{	c_copy(x[0][l],b[0][l]);
		for (k=1; k<n; k++)
		{	c_copy(x[k][l],b[k][l]);
			for (j=0; j<k; j++)
			{	c_mult(c_lumat[k][j],x[j][l],t);
				c_sub(x[k][l],t,x[k][l]);
			}
		}
		c_div(x[n-1][l],c_lumat[n-1][n-1],x[n-1][l]);
		for (k=n-2; k>=0; k--)
		{	sum[0]=0; sum[1]=0.0;
			for (j=k+1; j<n; j++)
			{	c_mult(c_lumat[k][j],x[j][l],t);
				c_add(sum,t,sum);
			}
			c_sub(x[k][l],sum,t);
			c_div(t,c_lumat[k][k],x[k][l]);
		}
	}
}

void msolve (header *hd)
{	header *st=hd,*hd1,*result;
	double *m,*m1;
	int r,c,r1,c1;
	hd1=nextof(st);
	equal_params_2(&hd,&hd1);
	if (error) return;
	if (hd->type==s_matrix || hd->type==s_real)
	{	getmatrix(hd,&r,&c,&m);
		if (hd1->type==s_cmatrix)
		{	make_complex(st);
			msolve(st); return;	
		}
		if (hd1->type!=s_matrix && hd1->type!=s_real)
			wrong_arg_in("\\");
		getmatrix(hd1,&r1,&c1,&m1);
		if (c!=r || c<1 || r!=r1) wrong_arg_in("\\");
		result=new_matrix(r,c1,""); if (error) return;
		solvesim(m,r,m1,c1,matrixof(result));
		if (error) return;
		moveresult(st,result);
	}
	else if (hd->type==s_cmatrix || hd->type==s_complex)
	{	getmatrix(hd,&r,&c,&m);
		if (hd1->type==s_matrix || hd1->type==s_real)
		{	make_complex(next_param(st));
			msolve(st); return;
		}
		if (hd1->type!=s_cmatrix && hd1->type!=s_complex)
			wrong_arg_in("\\");
		getmatrix(hd1,&r1,&c1,&m1);
		if (c!=r || c<1 || r!=r1) wrong_arg_in("\\");
		result=new_cmatrix(r,c1,""); if (error) return;
		c_solvesim(m,r,m1,c1,matrixof(result));
		if (error) return;
		moveresult(st,result);
	}
	else if (hd->type==s_imatrix || hd->type==s_interval)
	{	getmatrix(hd,&r,&c,&m);
		if (hd1->type==s_matrix || hd1->type==s_real)
		{	make_interval(next_param(st));
			msolve(st); return;
		}
		if (hd1->type!=s_imatrix && hd1->type!=s_interval)
			wrong_arg_in("\\");
		getmatrix(hd1,&r1,&c1,&m1);
		if (c!=r || c<1 || r!=r1) wrong_arg_in("\\");
		result=new_imatrix(r,c1,""); if (error) return;
		i_solvesim(m,r,m1,c1,matrixof(result));
		if (error) return;
		moveresult(st,result);
	}
	else wrong_arg_in("\\");
}

void mlu (header *hd)
{	header *st=hd,*result,*res1,*res2,*res3;
	double *m,*mr,*m1,*m2,det,deti;
	int r,c,*rows,*cols,rank,i;
	hd=getvalue(hd); if (error) return;
	if (hd->type==s_matrix || hd->type==s_real)
	{	getmatrix(hd,&r,&c,&m);
		if (r<1) wrong_arg_in("lu");
		result=new_matrix(r,c,""); if (error) return;
		mr=matrixof(result);
		memmove((char *)mr,(char *)m,(long)r*c*sizeof(double));
		make_lu(mr,r,c,&rows,&cols,&rank,&det); if (error) return;
		res1=new_matrix(1,rank,""); if (error) return;
		res2=new_matrix(1,c,""); if (error) return;
		res3=new_real(det,""); if (error) return;
		m1=matrixof(res1);
		for (i=0; i<rank; i++)
		{	*m1++=*rows+1;
			rows++;
		}
		m2=matrixof(res2);
		for (i=0; i<c; i++)
		{	*m2++=*cols++;
		}
		moveresult(st,getvalue(result)); st=nextof(st);
		moveresult(st,getvalue(res1)); st=nextof(st);
		moveresult(st,getvalue(res2)); st=nextof(st);
		moveresult(st,getvalue(res3));
	}
	else if (hd->type==s_cmatrix || hd->type==s_complex)
	{	getmatrix(hd,&r,&c,&m);
		if (r<1) wrong_arg_in("lu");
		result=new_cmatrix(r,c,""); if (error) return;
		mr=matrixof(result);
        memmove((char *)mr,(char *)m,(long)r*c*(long)2*sizeof(double));
		cmake_lu(mr,r,c,&rows,&cols,&rank,&det,&deti); 
			if (error) return;
		res1=new_matrix(1,rank,""); if (error) return;
		res2=new_matrix(1,c,""); if (error) return;
		res3=new_complex(det,deti,""); if (error) return;
		m1=matrixof(res1);
		for (i=0; i<rank; i++)
		{	*m1++=*rows+1;
			rows++;
		}
		m2=matrixof(res2);
		for (i=0; i<c; i++)
		{	*m2++=*cols++;
		}
		moveresult(st,getvalue(result)); st=nextof(st);
		moveresult(st,getvalue(res1)); st=nextof(st);
		moveresult(st,getvalue(res2)); st=nextof(st);
		moveresult(st,getvalue(res3));
	}
	else wrong_arg_in("lu");
}

void mlusolve (header *hd)
{	header *st=hd,*hd1,*result;
	double *m,*m1;
	int r,c,r1,c1;
	hd=getvalue(hd);
	hd1=next_param(st);
	if (hd1) hd1=getvalue(hd1);
	if (error) return;
	if (hd->type==s_matrix || hd->type==s_real)
	{	getmatrix(hd,&r,&c,&m);
		if (hd1->type==s_cmatrix)
		{	make_complex(st);
			mlusolve(st); return;
		}
		if (hd1->type!=s_matrix && hd1->type!=s_real)
			wrong_arg_in("lu");
		getmatrix(hd1,&r1,&c1,&m1);
		if (c!=r || c<1 || r!=r1) wrong_arg_in("lu");
		result=new_matrix(r,c1,""); if (error) return;
		lu_solve(m,r,m1,c1,matrixof(result));
		if (error) return;
		moveresult(st,result);
	}
	else if (hd->type==s_cmatrix || hd->type==s_complex)
	{	getmatrix(hd,&r,&c,&m);
		if (hd1->type==s_matrix || hd1->type==s_real)
		{	make_complex(next_param(st));
			mlusolve(st); return;
		}
		if (hd1->type!=s_cmatrix && hd1->type!=s_complex)
			wrong_arg_in("lu");
		getmatrix(hd1,&r1,&c1,&m1);
		if (c!=r || c<1 || r!=r1) wrong_arg_in("lu");
		result=new_cmatrix(r,c1,""); if (error) return;
		clu_solve(m,r,m1,c1,matrixof(result));
		if (error) return;
		moveresult(st,result);
	}
	else wrong_arg_in("lu");
}


/**************** tridiagonalization *********************/

static double **mg;

static void tridiag ( double *a, int n, int **rows)
/***** tridiag
	tridiag. a with n rows and columns.
	r[] contains the new indices of the rows.
*****/
{	char *ram=newram,rh;
	double **m,maxi,*mh,lambda,h;
	int i,j,ipiv,ik,jk,k,*r;
	
	/* make a pointer array to the rows of m : */
	m=(double **)ram; increase(ram,n*sizeof(double *));
	for (i=0; i<n; i++) { m[i]=a; a+=n; }
	r=(int *)ram; increase(ram,n*sizeof(double *)); *ram=0;
	for (i=0; i<n; i++) r[i]=i;
	
	/* start algorithm : */
	for (j=0; j<n-2; j++) /* need only go the (n-2)-th column */
	{	/* determine pivot */
		jk=r[j]; maxi=fabs(m[j+1][jk]); ipiv=j+1;
		for (i=j+2; i<n; i++)
		{	h=fabs(m[i][jk]);
			if (h>maxi) { maxi=h; ipiv=i; }
		}
		if (maxi<epsilon) continue;
		/* exchange with pivot : */
		if (ipiv!=j+1)
		{	mh=m[j+1]; m[j+1]=m[ipiv]; m[ipiv]=mh;
			rh=r[j+1]; r[j+1]=r[ipiv]; r[ipiv]=rh;
		}
		/* zero elements */
		for (i=j+2; i<n; i++)
		{	jk=r[j]; m[i][jk]=lambda=-m[i][jk]/m[j+1][jk];
			for (k=j+1; k<n; k++) 
			{	ik=r[k]; m[i][ik]+=lambda*m[j+1][ik];
			}
			/* same for columns */
			jk=r[j+1]; ik=r[i];
			for (k=0; k<n; k++) m[k][jk]-=lambda*m[k][ik];
		}
	}
	*rows=r; mg=m;
}

static complex **cmg;

static void ctridiag ( double *ca, int n, int **rows)
/***** tridiag
	tridiag. a with n rows and columns.
	r[] contains the new indices of the rows.
*****/
{	char *ram=newram,rh;
	complex **m,*mh,lambda,*a=(complex *)ca,help;
	double maxi,h;
	int i,j,ipiv,ik,jk,k,*r;
	
	/* make a pointer array to the rows of m : */
	m=(complex **)ram; increase(ram,n*sizeof(double *));
	for (i=0; i<n; i++) { m[i]=a; a+=n; }
	r=(int *)ram; increase(ram,n*sizeof(complex *)); *ram=0;
	for (i=0; i<n; i++) r[i]=i;
	
	/* start algorithm : */
	for (j=0; j<n-2; j++) /* need only go the (n-2)-th column */
	{	/* determine pivot */
		jk=r[j]; maxi=c_abs(m[j+1][jk]); ipiv=j+1;
		for (i=j+2; i<n; i++)
		{	h=c_abs(m[i][jk]);
			if (h>maxi) { maxi=h; ipiv=i; }
		}
		if (maxi<epsilon) continue;
		/* exchange with pivot : */
		if (ipiv!=j+1)
		{	mh=m[j+1]; m[j+1]=m[ipiv]; m[ipiv]=mh;
			rh=r[j+1]; r[j+1]=r[ipiv]; r[ipiv]=rh;
		}
		/* zero elements */
		for (i=j+2; i<n; i++)
		{	jk=r[j];
			c_div(m[i][jk],m[j+1][jk],lambda);
			lambda[0]=-lambda[0]; lambda[1]=-lambda[1];
			c_copy(m[i][jk],lambda);
			for (k=j+1; k<n; k++)
			{	ik=r[k];
				c_mult(lambda,m[j+1][ik],help);
				c_add(m[i][ik],help,m[i][ik]);
			}
			/* same for columns */
			jk=r[j+1]; ik=r[i];
			for (k=0; k<n; k++)
			{	c_mult(lambda,m[k][ik],help);
				c_sub(m[k][jk],help,m[k][jk]);
			}
		}
	}
	*rows=r; cmg=m;
}

void mtridiag (header *hd)
{	header *result,*st=hd,*result1;
	double *m,*mr;
	int r,c,*rows,i;
	hd=getvalue(hd); if (error) return;
	if (hd->type==s_matrix)
	{	getmatrix(hd,&c,&r,&m);
		if (c!=r || c==0) wrong_arg();
		result=new_matrix(c,c,""); if (error) return;
		result1=new_matrix(1,c,""); if (error) return;
		mr=matrixof(result);
		memmove(mr,m,(long)c*c*sizeof(double));
		tridiag(mr,c,&rows);
		mr=matrixof(result1);
		for (i=0; i<c; i++) *mr++=rows[i]+1;
	}
	else if (hd->type==s_cmatrix)
	{	getmatrix(hd,&c,&r,&m);
		if (c!=r || c==0) wrong_arg();
		result=new_cmatrix(c,c,""); if (error) return;
		result1=new_matrix(1,c,""); if (error) return;
		mr=matrixof(result);
        memmove(mr,m,(long)c*c*(long)2*sizeof(double));
		ctridiag(mr,c,&rows);
		mr=matrixof(result1);
		for (i=0; i<c; i++) *mr++=rows[i]+1;
	}
	else wrong_arg();
	moveresult(st,result);
	moveresult((header *)newram,result1);
}

static void charpoly (double *m1, int n, double *p)
/***** charpoly
	compute the chracteristic polynomial of m.
*****/
{	int i,j,k,*r;
	double **m,h1,h2;
	tridiag(m1,n,&r); m=mg; /* unusual global variable handling */
	/* compute the p_n rekursively : */
	m[0][r[0]]=-m[0][r[0]]; /* first one is x-a(0,0). */
	for (j=1; j<n; j++)
	{	m[0][r[j]]=-m[0][r[j]];
		for (k=1; k<=j; k++)
		{	h1=-m[k][r[j]]; h2=m[k][r[k-1]]; 
			for (i=0; i<k; i++) 
				m[i][r[j]]=m[i][r[j]]*h2+m[i][r[k-1]]*h1;
			m[k][r[j]]=h1;
		}
		for (i=0; i<j; i++) m[i+1][r[j]]+=m[i][r[j-1]];
	}
	for (i=0; i<n; i++) p[i]=m[i][r[n-1]];
	p[n]=1.0;
}

static void ccharpoly (double *m1, int n, double *p)
/***** charpoly
	compute the chracteristic polynomial of m.
*****/
{	int *r,i,j,k;
	complex **m,h1,h2,g1,g2,*pc=(complex *)p;
	ctridiag(m1,n,&r); m=cmg; /* unusual global variable handling */
	/* compute the p_n rekursively : */
	m[0][r[0]][0]=-m[0][r[0]][0];
	m[0][r[0]][1]=-m[0][r[0]][1]; /* first one is x-a(0,0). */
	for (j=1; j<n; j++)
	{	m[0][r[j]][0]=-m[0][r[j]][0];
		m[0][r[j]][1]=-m[0][r[j]][1];
		for (k=1; k<=j; k++)
		{	h1[0]=-m[k][r[j]][0]; h1[1]=-m[k][r[j]][1]; 
			c_copy(h2,m[k][r[k-1]]); 
			for (i=0; i<k; i++)
			{	c_mult(h2,m[i][r[j]],g1);
				c_mult(h1,m[i][r[k-1]],g2);
				c_add(g1,g2,m[i][r[j]]);
			}
			c_copy(m[k][r[j]],h1);
		}
		for (i=0; i<j; i++)
		{	c_add(m[i+1][r[j]],m[i][r[j-1]],m[i+1][r[j]]);
		}
	}
	for (i=0; i<n; i++) c_copy(pc[i],m[i][r[n-1]]);
	pc[n][0]=1.0; pc[n][1]=0.0;
}

void mcharpoly (header *hd)
{	header *result,*st=hd,*result1;
	double *m,*mr;
	int r,c;
	hd=getvalue(hd); if (error) return;
	if (hd->type==s_matrix)
	{	getmatrix(hd,&c,&r,&m);
		if (c!=r || c==0) wrong_arg();
		result=new_matrix(c,c,""); if (error) return;
		result1=new_matrix(1,c+1,""); if (error) return;
		mr=matrixof(result);
		memmove(mr,m,(long)c*c*sizeof(double));
		charpoly(mr,c,matrixof(result1));
	}
	else if (hd->type==s_cmatrix)
	{	getmatrix(hd,&c,&r,&m);
		if (c!=r || c==0) wrong_arg();
		result=new_cmatrix(c,c,""); if (error) return;
		result1=new_cmatrix(1,c+1,""); if (error) return;
		mr=matrixof(result);
        memmove(mr,m,(long)c*c*(long)2*sizeof(double));
		ccharpoly(mr,c,matrixof(result1));
	}
	else wrong_arg();
	moveresult(st,result1);
}

/***************** jacobi-givens eigenvalues **************/

static double rotate (double *m, int j, int k, int n)
{
	double theta,t,s,c,tau,h,pivot;
	int l;
	pivot=*mat(m,n,j,k);
	if (fabs(pivot)<epsilon) return 0;
	theta=(*mat(m,n,j,j)-*mat(m,n,k,k))/(2*pivot);
	t=1/(fabs(theta)+sqrt(1+theta*theta));
	if (theta<0) t=-t;
	c=1/sqrt(1+t*t); s=t*c;
	tau=s/(1+c);
	for (l=0; l<n; l++)
	{
		if (l==j || l==k) continue;
		h=*mat(m,n,l,j);
		*mat(m,n,j,l)=*mat(m,n,l,j)=h+s*(*mat(m,n,l,k)-tau*h);
		*mat(m,n,l,k)-=s*(h+tau* *mat(m,n,l,k));
		*mat(m,n,k,l)=*mat(m,n,l,k);
	}
	*mat(m,n,j,j)+=t*pivot;
	*mat(m,n,k,k)-=t*pivot;
	*mat(m,n,j,k)=*mat(m,n,k,j)=0;
	return fabs(pivot);
}

void mjacobi (header *hd)
{
	header *st=hd,*result,*hd1;
	double *m,max,neumax,*mr;
	int r,c,i,j;
	hd=getvalue(hd); if (error) return;
	if (hd->type!=s_matrix) wrong_arg_in("jacobi");
	getmatrix(hd,&r,&c,&m);
	if (r!=c) wrong_arg_in("jacobi");
	if (r<2) { moveresult(st,hd); return; }
	hd1=new_matrix(r,r,""); if (error) return;
	m=matrixof(hd1);
	memmove(m,matrixof(hd),(long)r*r*sizeof(double));
	while(1)
	{
		max=0.0;
		for (i=0; i<r-1; i++)
			for (j=i+1; j<r; j++)
				if ((neumax=rotate(m,i,j,r))>max)
					max=neumax;
		if (max<epsilon) break;
	}
	result=new_matrix(1,r,""); if (error) return;
	mr=matrixof(result);
	for (i=0; i<r; i++) *mr++=*mat(m,r,i,i);
	moveresult(st,result);
}

/*************** simplex-method *********************/

static int simplex (int m, int n, int * iv,
	double **a, double *b, double *c, double *x)
/*
Minimize c'x under all x with a*x<=b, x>=0.
iv[0]..iv[n-1] contain the variable indices of the rows,
iv[n]..iv[n+m-1] contain the variable indices of the columns.
a,b and c will be changed here.
Results: -1 unfeasable, 1 unbounded, 0 solution found
This procedure uses Bland's anticycling rule.
*/
{	int i,i0,iv0,j,j0,jv0;
	double mi,h;
	/* Balance the lines */
	for (i=0; i<m; i++)
	{	h=fabs(b[i]);
		for (j=0; j<n; j++) h+=fabs(a[i][j]);
		if (h>epsilon)
		{	b[i]/=h;
			for (j=0; j<n; j++) a[i][j]/=h;
		}
	}
	/* Search a feasable starting point */
	while (1)
	{
		/* Search for a row with negative b[i],
		take least index on tie */
		iv0=n+m; i0=m;
		for (i=0; i<m; i++)
			if (b[i]<-epsilon && iv[n+i]<iv0) { i0=i; iv0=iv[n+i]; }
		if (i0>=m) break;
		/* Search for a negative lambda */
		for (j=0; j<n; j++)
			if (a[i0][j]<-epsilon) break;
		if (j>=n) return -1;
		/* Search for a possible column,
		take least variable index on tie */
		jv0=iv[j]; j0=j; j++;
		for (; j<n; j++)
		{	if (a[i0][j]<-epsilon)
			{	if (iv[j]<jv0)
				{	j0=j; jv0=iv[j];
				}
			}
		}
		/* Exchange column j0 and i0 */
		a[i0][j0]=h=1/a[i0][j0];
		for (j=0; j<n; j++)
			if (j!=j0) a[i0][j]*=h;
		b[i0]*=h;
		for (i=0; i<m; i++)
			if (i!=i0)
			{	h=a[i][j0];
				for (j=0; j<n; j++)
					if (j!=j0) a[i][j]-=h*a[i0][j];
				b[i]-=h*b[i0];
			}
		h=c[j0];
		for (j=0; j<n; j++)
			if (j!=j0) c[j]-=h*a[i0][j];
		h=a[i0][j0];
		for (i=0; i<m; i++)
			if (i!=i0) a[i][j0]*=-h;
		c[j0]*=-h;
		i=iv[j0]; iv[j0]=iv[n+i0]; iv[n+i0]=i;
		if (test_key()==escape) { error=301; return 2; }
	}
	/* Search optimum */
	while (1)
	{
		/* Search for a profitable column,
		take least index on tie */
		jv0=n+m; j0=n;
		for (j=0; j<n; j++)
			if (c[j]<-epsilon && iv[j]<jv0) { j0=j; jv0=iv[j]; }
		if (j0>=n) break;
		/* Search for a positive lambda */
		for (i=0; i<m; i++)
			if (a[i][j0]>epsilon) break;
		if (i>=m)
		{	for (j=0; j<n; j++) x[j]=0;
			for (i=0; i<m; i++)
			{   if (iv[n+i]<n) x[iv[n+i]]=b[i];
			}
			return 1;
		}
		/* Search for a possible row,
		take least variable index on tie */
		iv0=iv[n+i]; i0=i; mi=b[i]/a[i][j0]; i++;
		for (; i<m; i++)
		{	if (a[i][j0]>epsilon)
			{	h=b[i]/a[i][j0];
				if (h<mi-epsilon || (h<=mi+epsilon && iv[n+i]<iv0))
				{	mi=h; i0=i; iv0=iv[n+i];
				}
			}
		}
		/* Exchange column j0 and i0 */
		a[i0][j0]=h=1/a[i0][j0];
		for (j=0; j<n; j++)
			if (j!=j0) a[i0][j]*=h;
		b[i0]*=h;
		for (i=0; i<m; i++)
			if (i!=i0)
			{	h=a[i][j0];
				for (j=0; j<n; j++)
					if (j!=j0) a[i][j]-=h*a[i0][j];
				b[i]-=h*b[i0];
			}
		h=c[j0];
		for (j=0; j<n; j++)
			if (j!=j0) c[j]-=h*a[i0][j];
		h=a[i0][j0];
		for (i=0; i<m; i++)
			if (i!=i0) a[i][j0]*=-h;
		c[j0]*=-h;
		i=iv[j0]; iv[j0]=iv[n+i0]; iv[n+i0]=i;
		if (test_key()==escape) { error=301; return 2; }
	}
	/* Compute the solution x */
	for (j=0; j<n; j++) x[j]=0;
	for (i=0; i<m; i++)
	{   if (iv[n+i]<n) x[iv[n+i]]=b[i];
	}
	return 0;
}

void msimplex (header *hd)
{	header *result,*hda=hd,*hdb,*hdc;
	int i,n,m,res;
	double *x,*a,*b,*c,**aa;
	int *iv;
	char *ram;
	hdb=next_param(hda);
	hdc=next_param(hdb);
	hda=getvalue(hda); if (error) return;
	hdb=getvalue(hdb); if (error) return;
	hdc=getvalue(hdc); if (error) return;
	if (hda->type!=s_matrix || hdb->type!=s_matrix ||
		hdc->type!=s_matrix || dimsof(hdb)->c!=1 ||
		dimsof(hdc)->r!=1)
			wrong_arg_in("simplex");
	getmatrix(hda,&m,&n,&x);
	if (m!=dimsof(hdb)->r || n!=dimsof(hdc)->c)
		wrong_arg_in("simplex");
	result=new_matrix(n,1,""); if (error) return;
	x=matrixof(result);
	for (i=0; i<n; i++) *x++=0.0;
	ram=newram;
	a=(double *)ram; increase(ram,(n*m)*sizeof(double));
	memcpy(a,matrixof(hda),(n*m)*sizeof(double));
	b=(double *)ram; increase(ram,m*sizeof(double));
	memcpy(b,matrixof(hdb),m*sizeof(double));
	c=(double *)ram; increase(ram,n*sizeof(double));
	memcpy(c,matrixof(hdc),n*sizeof(double));
	aa=(double **)ram; increase(ram,m*sizeof(double *));
	for (i=0; i<m; i++) aa[i]=a+i*n;
	iv=(int *)ram;
	for (i=0; i<n+m; i++) iv[i]=i;
	res=simplex(m,n,iv,aa,b,c,matrixof(result));
	moveresult(hd,result);
	new_real(res,"");
}

/************** Singular Value Decomposition ************/

#define SIGN(a,b) ((b) >= 0.0 ? 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 sqrarg;
#define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)

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.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
}

static void svdcmp(double **a, int m, int n, double w[], double **v)
{
	int flag,i,its,j,jj,k,l=0,nm=0;
	double anorm,c,f,g,h,s,scale,x,y,z,*rv1;

	rv1=(double *)malloc((n+1)*sizeof(double));
	g=scale=anorm=0.0;
	for (i=1;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(a[k][i]);
			if (scale) {
				for (k=i;k<=m;k++) {
					a[k][i] /= scale;
					s += a[k][i]*a[k][i];
				}
				f=a[i][i];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][i]=f-g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
					f=s/h;
					for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
				}
				for (k=i;k<=m;k++) a[k][i] *= scale;
			}
		}
		w[i]=scale *g;
		g=s=scale=0.0;
		if (i <= m && i != n) {
			for (k=l;k<=n;k++) scale += fabs(a[i][k]);
			if (scale) {
				for (k=l;k<=n;k++) {
					a[i][k] /= scale;
					s += a[i][k]*a[i][k];
				}
				f=a[i][l];
				g = -SIGN(sqrt(s),f);
				h=f*g-s;
				a[i][l]=f-g;
				for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
				for (j=l;j<=m;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
					for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
				}
				for (k=l;k<=n;k++) a[i][k] *= scale;
			}
		}
		anorm=FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
	}
	for (i=n;i>=1;i--) {
		if (i < n) {
			if (g) {
				for (j=l;j<=n;j++)
					v[j][i]=(a[i][j]/a[i][l])/g;
				for (j=l;j<=n;j++) {
					for (s=0.0,k=l;k<=n;k++) s += a[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,n);i>=1;i--) {
		l=i+1;
		g=w[i];
		for (j=l;j<=n;j++) a[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 += a[k][i]*a[k][j];
				f=(s/a[i][i])*g;
				for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
			}
			for (j=i;j<=m;j++) a[j][i] *= g;
		} else for (j=i;j<=m;j++) a[j][i]=0.0;
		++a[i][i];
	}
	for (k=n;k>=1;k--) {
		for (its=1;its<=30;its++) {
			flag=1;
			for (l=k;l>=1;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=0.0;
				s=1.0;
				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.0/h;
					c=g*h;
					s = -f*h;
					for (j=1;j<=m;j++) {
						y=a[j][nm];
						z=a[j][i];
						a[j][nm]=y*c+z*s;
						a[j][i]=z*c-y*s;
					}
				}
			}
			z=w[k];
			if (l == k) {
				if (z < 0.0) {
					w[k] = -z;
					for (j=1;j<=n;j++) v[j][k] = -v[j][k];
				}
				break;
			}
			if (its == 30)
			{	output("No convergence in svd\n");
				error=1; return;
			}
			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=1;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=1;jj<=m;jj++) {
					y=a[jj][j];
					z=a[jj][i];
					a[jj][j]=y*c+z*s;
					a[jj][i]=z*c-y*s;
				}
			}
			rv1[l]=0.0;
			rv1[k]=f;
			w[k]=x;
		}
	}
	free(rv1);
}

void msvd (header *hd)
{	header *st=hd,*hda,*hdw,*hdv;
	int m,n,i;
	double *mm;
	double **ma,**mv;
	hd=getvalue(hd); if (error) return;
	if (hd->type!=s_matrix) wrong_arg_in("svd");
	getmatrix(hd,&m,&n,&mm);
	if (m<2 || n<2) wrong_arg_in("svd");
	hda=new_matrix(m,n,""); if (error) return;
	hdw=new_matrix(1,n,""); if (error) return;
	hdv=new_matrix(n,n,""); if (error) return;
	memmove((char *)matrixof(hda),(char *)mm,
		n*sizeof(double)*m);
	ma=(double **)malloc(m*sizeof(double *));
	for (i=0; i<m; i++) ma[i]=matrixof(hda)+i*n-1;
	mv=(double **)malloc(n*sizeof(double *));
	for (i=0; i<n; i++) mv[i]=matrixof(hdv)+i*n-1;
	svdcmp(ma-1,m,n,matrixof(hdw)-1,mv-1);
	free(ma); free(mv);
	if (error) return;
	moveresult1(st,hda);
}

/**************** Toeplitz systems ******************/

static void toeplitz (double r[], double x[], double y[], int n)
{
	int j,k,m,m1,m2;
	double pp,pt1,pt2,qq,qt1,qt2,sd,sgd,sgn,shn,sxn;
	double *g,*h,*g1=0,*h1=0;

	if (fabs(r[n]) < epsilon)
	{	output("Toeplitz singular error\n");
		error=1; goto stop;
	}
	g1=(double *)malloc(n*sizeof(double)); g=g1-1;
	h1=(double *)malloc(n*sizeof(double)); h=h1-1;
	x[1]=y[1]/r[n];
	if (n == 1) goto stop;
	g[1]=r[n-1]/r[n];
	h[1]=r[n+1]/r[n];
	for (m=1;m<=n;m++) {
		m1=m+1;
		sxn = -y[m1];
		sd = -r[n];
		for (j=1;j<=m;j++) {
			sxn += r[n+m1-j]*x[j];
			sd += r[n+m1-j]*g[m-j+1];
		}
		if (fabs(sd) < epsilon)
		{	output("Toeplitz singular error\n");
			error=1; goto stop;
		}
		x[m1]=sxn/sd;
		for (j=1;j<=m;j++) x[j] -= x[m1]*g[m-j+1];
		if (m1 == n) goto stop;
		sgn = -r[n-m1];
		shn = -r[n+m1];
		sgd = -r[n];
		for (j=1;j<=m;j++) {
			sgn += r[n+j-m1]*g[j];
			shn += r[n+m1-j]*h[j];
			sgd += r[n+j-m1]*h[m-j+1];
		}
		if (fabs(sd)<epsilon || fabs(sgd)<epsilon)
		{	output("Toeplitz singular error\n");
			error=1; goto stop;
		}
		g[m1]=sgn/sgd;
		h[m1]=shn/sd;
		k=m;
		m2=(m+1) >> 1;
		pp=g[m1];
		qq=h[m1];
		for (j=1;j<=m2;j++) {
			pt1=g[j];
			pt2=g[k];
			qt1=h[j];
			qt2=h[k];
			g[j]=pt1-pp*qt2;
			g[k]=pt2-pp*qt1;
			h[j]=qt1-qq*pt2;
			h[k--]=qt2-qq*pt1;
		}
	}
	output("Toeplitz failed.\n");
	error=1;
	stop : free(g1); free(h1);
}

void msolvetoeplitz (header *hd)
{	header *st=hd,*hdb,*result;
	int n,i;
	double *m,*r;
	hdb=nextof(hd);
	hd=getvalue(hd); if (error) return;
	hdb=getvalue(hdb); if (error) return;
	if (hd->type!=s_matrix || dimsof(hd)->r!=1 ||
		hdb->type!=s_matrix || dimsof(hdb)->c!=1)
		wrong_arg_in("toeplitzsolve");
	n=dimsof(hdb)->r;
	if (n<2 || dimsof(hd)->c!=2*n-1) wrong_arg_in("toeplitzsolve");
	result=new_matrix(n,1,""); if (error) return;
	r=(double *)malloc((2*n-1)*sizeof(double));
	m=matrixof(hd);
	for (i=0; i<2*n-1; i++) r[i]=m[2*n-2-i];
	toeplitz(r-1,matrixof(result)-1,
		matrixof(hdb)-1,n);
	free(r);
	if (error) return;
	moveresult(st,result);
}

void mtoeplitz (header *hd)
{	header *st=hd,*result;
	int i,n;
    double *m,*r;
	hd=getvalue(hd); if (error) return;
	if (hd->type!=s_matrix || dimsof(hd)->r!=1)
		wrong_arg_in("toeplitz");
	n=dimsof(hd)->c;
	if (n<2 || n%2!=1) wrong_arg_in("toeplitz");
	n=n/2+1;
	result=new_matrix(n,n,""); if (error) return;
	m=matrixof(result);
	r=matrixof(hd);
	for (i=0; i<n; i++)
	{	memmove((char *)(m+i*n),(char *)(r+n-1-i),
			n*sizeof(double));
	}
	moveresult(st,result);
}