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

void quint(a,b,c,d,e,f,n)
/*
	Subroutine QUINT solves a pentadiagonal system by overwriting
	the input vectors.  The vectors a through e are the five diagonals
	of A (in Ax = f), in order from second subdiagonal to second
	superdiagonal.  The vectors b, c and d are overwritten in
	forming the upper tridiagonal system.  The solution vector x
	is written into the input vector f.  The nth element of each
	input vector is the value of matrix element of that diagonal
	on the nth row of A.
*/
float *a, *b, *c, *d, *e, *f;
int n;
{
	float fac;
	int k;

	for( k=1; k<n-1; k++) {
		fac  = b[k] / c[k-1];
		c[k  ] -= fac * d[k-1];
		d[k  ] -= fac * e[k-1];
		f[k  ] -= fac * f[k-1];
		fac  = a[k+1] / c[k-1];
		b[k+1] -= fac * d[k-1];
		c[k+1] -= fac * e[k-1];
		f[k+1] -= fac * f[k-1];
	}
	fac  = b[n-1] / c[n-2];
	c[n-1] -= fac * d[n-2];
	f[n-1] -= fac * f[n-2];
	f[n-1] /= c[n-1];
	f[n-2] -= d[n-2] * f[n-2];
	f[n-2] /= c[n-2];
	for( k=n-3; k>=0; k--) {
		f[k] -= e[k] * f[k+2] + d[k] * f[k+1];
		f[k] /= c[k];
	}
}

float *a, *b, *c, *d, *e, *f;
struct disp { float u, w; };

void frob(p1,p2,nx,g,u,w)
struct disp *p1, *p2;
float *g, *u, *w;
int nx;
{
	int i;

	zap(b,nx); zap(d,nx);

	/* compute A in Ax = f */

	/* absorbing b.c. at left and right */
	a[0] = 0.;
	a[1] = 0.;
	c[0] = -1-u[0];
	c[1] = -1-u[1];
	d[0] =  1-u[1];
	d[1] =  1-u[2];
	e[0] = 0.;
	e[1] = 0.;
	a[nx-2] = 0.;
	a[nx-1] = 0.;
	b[nx-2] = -1+u[3];
	b[nx-1] = -1+u[4];
	c[nx-2] =  1+u[4];
	c[nx-1] =  1+u[5];
	e[nx-2] = 0.;
	e[nx-1] = 0.;

	for(i=2;i<nx-2;i++){
		a[i] = -0.0625 * g[i-1];
		e[i] = -0.0625 * g[i+1];
		c[i] = 1. + 0.0625 * (g[i-1] + g[i+1]);
	}

	/* compute f in Ax = f */
	f[0]=(1-u[0])*p2[0].u-(1+u[1])*p2[1].u;
	f[1]=(1-u[1])*p2[1].u-(1+u[2])*p2[2].u;
	f[nx-2]=(1+u[3])*p2[nx-3].u-(1-u[4])*p2[nx-2].u;
	f[nx-1]=(1+u[4])*p2[nx-2].u-(1-u[5])*p2[nx-1].u;

	for(i=2;i<nx-2;i++){
		f[i] = (1. - 0.0625 * (g[i-1] + g[i+1])) * p1[i+nx].u;
		f[i] += 0.0625 * (g[i-1]*p1[i+nx-2].u + g[i+1]*p1[i+nx+2].u);
		f[i] += 0.5 * (p1[nx+i+1].w - p1[nx+i-1].w);
	}

	/* solve for x in Ax = f */
	quint(a,b,c,d,e,f,nx);

	/* substitute in top row of u */
	for(i=0;i<nx;i++) p1[i].u = f[i];

	/* repeat for w */
	zap(b,nx); zap(d,nx);

	a[0] = 0.;
	a[1] = 0.;
	c[0] = -1-w[0];
	c[1] = -1-w[1];
	d[0] =  1-w[1];
	d[1] =  1-w[2];
	e[0] = 0.;
	e[1] = 0.;
	a[nx-2] = 0.;
	a[nx-1] = 0.;
	b[nx-2] = -1+w[3];
	b[nx-1] = -1+w[4];
	c[nx-2] =  1+w[4];
	c[nx-1] =  1+w[5];
	e[nx-2] = 0.;
	e[nx-1] = 0.;

	for(i=2;i<nx-2;i++){
		a[i] = e[i] = -0.0625 * g[i];
		c[i] = 1. + 0.125 * g[i];
	}

	f[0]=(1-w[0])*p2[0].w-(1+w[1])*p2[1].w;
	f[1]=(1-w[1])*p2[1].w-(1+w[2])*p2[2].w;
	f[nx-2]=(1+w[3])*p2[nx-3].w-(1-w[4])*p2[nx-2].w;
	f[nx-1]=(1+w[4])*p2[nx-2].w-(1-w[5])*p2[nx-1].w;

	for(i=2;i<nx-2;i++){
		f[i] = (1. - 0.125 * g[i]) * p1[i+nx].w;
		f[i] += 0.0625 * g[i] * (p1[i+nx-2].w + p1[i+nx+2].w);
		f[i] += 0.5 * g[i] * (p1[nx+i+1].u - p1[nx+i-1].u);
	}

	quint(a,b,c,d,e,f,nx);

	for(i=0;i<nx;i++) p1[i].w = f[i];
}

#define 	ta(x)		na[pt[x]+0]
#define 	tb(x)		na[pt[x]+1]
#define 	tc(x)		na[pt[x]+2]

void fsfill(g,u,w,pt,na,nx)
float *g, *w, *u, *na;
int *pt;
int nx;
{
	int i;

	a = (float *) malloc(4*nx);
	b = (float *) malloc(4*nx);
	c = (float *) malloc(4*nx);
	d = (float *) malloc(4*nx);
	e = (float *) malloc(4*nx);
	f = (float *) malloc(4*nx);

	for(i=0;i<nx;i++)
		g[i]= tb(i)/(tb(i)+2.*ta(i));
	u[0]=sqrt((tb(0)+2.*ta(0))/tc(0));
	w[0]=sqrt(ta(0)/tc(0));
	u[1]=sqrt((tb(1)+2.*ta(1))/tc(1));
	w[1]=sqrt(ta(1)/tc(1));
	u[2]=sqrt((tb(2)+2.*ta(2))/tc(2));
	w[2]=sqrt(ta(2)/tc(2));
	u[3]=sqrt((tb(nx-3)+2.*ta(nx-3))/tc(nx-3));
	w[3]=sqrt(ta(nx-3)/tc(nx-3));
	u[4]=sqrt((tb(nx-2)+2.*ta(nx-2))/tc(nx-2));
	w[4]=sqrt(ta(nx-2)/tc(nx-2));
	u[5]=sqrt((tb(nx-1)+2.*ta(nx-1))/tc(nx-1));
	w[5]=sqrt(ta(nx-1)/tc(nx-1));
}