#include	<stdio.h>
#include	<sys/file.h>
#include	<math.h>

/*
	grademodel takes an input file (normally named "run".grad,
	where "run" is the code name for the FD run).  From this,
	it constructs three files, pvelocity, svelocity, and density.
	The first line of the input file is the background medium
	specified as pvelocity, svelocity, density.

	In the input file, you may specify homogeneous polygons:
		nvert pvel svel den ipat
			x0     z0
			x1     z1
			 .      .
			 .      .
			xnvert znvert
			x0     z0
	where xn,zn are the x,z coordinates of the vertices; ipat
	is a pattern designator for other programs which draw the
	model; and nvert is the number of vertices (2 < nvert < 50).

	You may also specify polygons with gradients:
		-nvert dum dum dum ipat
			x0     z0     p0     s0     d0
			x1     z1     p0     s0     d0
			 .      .      .      .      .
			 .      .      .      .      .
			xnvert znvert pnvert snvert dnvert
			x0     z0     p0     s0     d0
	xn, zn, ipat are as above, nvert must be preceded by a '-' (this
	is used as a flag in the program); dum indicates a dummy float
	which must be present; and pn, sn, dn are the pvelocity, svelocity
	and density at the vertex.  (3 <= nvert <= 4; if more vertices
	are required, break region into 3 and 4 vertex polys).

	Required parameters:
		model="run".grad
		pvelocity="run".pvel
		svelocity="run".svel
		density="run".den
		nx=nx
		nz=nz
	Optional parameter:
		shift=1 (default is 0):
			this parameter tells grademodel to shift
			coordinates by half a grid point before
			calculating interpolated media parameters,
			allowing exact coorespondence to the offset
			grid scheme used to fill the polygons.
*/

double ap, bp, cp, dp,
       as, bs, cs, ds,
       ad, bd, cd, dd;

#define Pfunc(X,Z) ( dp * X * Z + cp * Z + bp * X + ap )
#define Sfunc(X,Z) ( ds * X * Z + cs * Z + bs * X + as )
#define Dfunc(X,Z) ( dd * X * Z + cd * Z + bd * X + ad )

struct vertex
   {
	int xv, zv;
	struct vertex *f;
	struct vertex *b;
   }	base[200];

struct gradvertex
   {
	int xv, zv;
	float pv, sv, dv;
	struct gradvertex *f;
	struct gradvertex *b;
   }	gradbase[200];

int crosses[2*200];

float *a, *b, *c;
int nx;
int nz;
char model[64];
char pvel[64];
char svel[64];
char density[64];
int shift=0;

main(ac,av)
int ac; char **av;
   {
	struct vertex *head, *p;
	struct gradvertex *ghead, *gp;
	int af, bf, cf;
	FILE *mf, *fopen();
	float aback,bback,cback,aval,bval,cval;
	int x, z;
	float p1, s1, d1;
	float *pa, *pb, *pc;
	int npts, xstr, xend, i;
	int zvmin, zvmax, ncross;
	int n;

	setpar(ac,av);
	mstpar("nx","d",&nx);
	mstpar("nz","d",&nz);
	getpar("shift","d",&shift);
	getpar("pvelocity","s",pvel);
	getpar("svelocity","s",svel);
	getpar("density","s",density);
	mstpar("model","s",model);
	endpar();

	a=(float *) malloc(nx*nz*4);
	b=(float *) malloc(nx*nz*4);
	c=(float *) malloc(nx*nz*4);
	if(a==NULL||b==NULL||c==NULL){
		fprintf(stderr,"can't allocate memory!\n");
		exit(-1);
		}
	
	if( (af= open(pvel,O_WRONLY|O_TRUNC|O_CREAT,0664)) <= 1)
	   {
		fprintf(stderr,"cannot creat pvel=%s\n",pvel);
		exit(-1);
	   }
	
	if( (bf= open(svel,O_WRONLY|O_TRUNC|O_CREAT,0664)) <= 1)
	   {
		fprintf(stderr,"cannot creat svel=%s\n",svel);
		exit(-1);
	   }
	
	if( (cf= open(density,O_WRONLY|O_TRUNC|O_CREAT,0664)) <= 1)
	   {
		fprintf(stderr,"cannot creat density=%s\n",density);
		exit(-1);
	   }
	if( (mf= fopen(model,"r")) == NULL)
	   {
		fprintf(stderr,"cannot open model=%s\n",model);
		exit(-1);
	   }

	n=fscanf(mf,"%f %f %f",&aback,&bback,&cback);
	for(i=0; i< (nx*nz); i++)
	   {
		a[i]= aback;
		b[i]= bback;
		c[i]= cback;
	   }

	while( (n=fscanf(mf,"%d %f %f %f %*d",&npts,&aval,&bval,&cval)) != -1 )
	   {
	    if (npts < 0) {	/* gradient poly */
		npts = -npts;
		fprintf(stderr,"grad poly: %d vertices\n",npts);
		if(npts < 3)
		   {
			fprintf(stderr,"polynomial with < 3 vertices\n");
			exit(-1);
		   }
		if(npts > 4)
		   {
			fprintf(stderr,"gradient polynomial with > 4 vertices\n");
			fprintf(stderr,"not supported, please break up into 3 and 4 vertex polys\n");
			exit(-1);
		   }

		/* Set up a double linked ring of vertices. The pointer
		 * to the ring (head) is set to the node with the maximum
		 * x-value so that intersect() will not eliminate 'head'
		 * while casting off vertices.
		 */
		zvmin= 2000000; zvmax= -2000000;
		ghead= gradbase;
		for(i=0; i<npts; i++)
		   {
			gp= &gradbase[i];
			gp->f= gp+1; gp->b= gp-1;
			fscanf(mf,"%d %d %f %f %f",&x,&z,&p1,&s1,&d1);
			gp->xv= x; gp->zv= z;
			gp->pv= p1; gp->sv= s1; gp->dv= d1;
			if(gp->zv > zvmax) { zvmax= gp->zv; ghead= gp; }
			if(gp->zv < zvmin)   zvmin= gp->zv;
		   }
		gp->f= gradbase; gradbase->b= gp; /* close the ring */
		fscanf(mf,"%d %d %f %f %f",&x,&z,&p1,&s1,&d1);
		/* check for closure */
		if(gradbase->xv != x || gradbase->zv !=  z ||
		   gradbase->pv != p1 || gradbase->sv != s1 ||
		   gradbase->dv != d1)
		   {
			fprintf(stderr,"grad poly does not close\n");
			fprintf(stderr,"first vertex: %d %d %f %f %f\n",
				gradbase->xv,gradbase->zv,
				gradbase->pv,gradbase->sv,gradbase->dv);
			fprintf(stderr,"last  vertex: %d %d %f %f %f\n",
				x,z,p1,s1,d1);
			exit(-1);
		   }
		interpol(gradbase, npts);
		if(zvmax > (nz-1)) zvmax= (nz-1);
		if(zvmin < 0 ) zvmin= 0;
		gp= ghead;
		do gp->zv = 2* gp->zv + 1; while( (gp=gp->f) != ghead);
		for(z= zvmin; z <= zvmax; z++)
		   {
			ncross= gintersect(2*z,crosses,ghead);
			sort(crosses,ncross);
			pa= a + z*nx;
			pb= b + z*nx;
			pc= c + z*nx;
			for(i=0; i< ncross; i += 2)
			   {
				xstr= crosses[i];
				if (xstr<0) xstr=0;
				xend= crosses[i+1];
				if (xend>=nx) xend = nx-1;
				for(x=xstr; x<=xend; x++)
				   {
					pa[x] = Pfunc(x,z);
					pb[x] = Sfunc(x,z);
					pc[x] = Dfunc(x,z);
				   }
			   }
		   }
	    }
	    else {		/* homogeneous poly */
		fprintf(stderr,"uniform poly: %d vertices\n",npts);
		if(npts < 3)
		   {
			fprintf(stderr,"polynomial with < 3 vertices");
			exit(-1);
		   }

		/* Set up a double linked ring of vertices. The pointer
		 * to the ring (head) is set to the node with the maximum
		 * x-value so that intersect() will not eliminate 'head'
		 * while casting off vertices.
		 */
		zvmin= 2000000; zvmax= -2000000;
		head= base;
		for(i=0; i<npts; i++)
		   {
			p= &base[i];
			p->f= p+1; p->b= p-1;
			fscanf(mf,"%d %d",&x,&z);
			p->xv= x; p->zv= z;
			if(p->zv > zvmax) { zvmax= p->zv; head= p; }
			if(p->zv < zvmin)   zvmin= p->zv;
		   }
		p->f= base; base->b= p; /* close the ring */
		fscanf(mf,"%d %d",&x,&z);
		/* check for closure */
		if(base->xv != x || base->zv !=  z)
		   {
			fprintf(stderr,"polygon does not close");
			fprintf(stderr,"first vertex: %d %d\n",
				base->xv,base->zv);
			fprintf(stderr,"last  vertex: %d %d\n", x,z);
			exit(-1);
		   }
		if(zvmax > (nz-1)) zvmax= (nz-1);
		if(zvmin < 0 ) zvmin= 0;
		p= head;
		do p->zv = 2* p->zv + 1; while( (p=p->f) != head);
		for(z= zvmin; z <= zvmax; z++)
		   {
			ncross= intersect(2*z,crosses,head);
			sort(crosses,ncross);
			pa= a + z*nx;
			pb= b + z*nx;
			pc= c + z*nx;
			for(i=0; i< ncross; i += 2)
			   {
				xstr= crosses[i];
				if (xstr<0) xstr=0;
				xend= crosses[i+1];
				if (xend>=nx) xend = nx-1;
				for(x=xstr; x<=xend; x++)
				   {
					pa[x] = aval;
					pb[x] = bval;
					pc[x] = cval;
				   }
			   }
		   }
		}
	   }
	write(af,a,nx*nz*4);
	write(bf,b,nx*nz*4);
	write(cf,c,nx*nz*4);
   }

intersect(z,crosses,head)
register int z; register int *crosses; struct vertex *head;
   {
	register int ncross, x; register struct vertex *p;

	ncross= 0;
	p= head;
	do
	   {
		if(p->zv > z && p->b->zv > z) continue;
		if(p->zv < z && p->b->zv < z)
		   {
			if(p->f->zv > z) continue;
			/* eliminate vertex */
			p->b->f= p->f;
			p->f->b= p->b;
			continue;
		   }
		x= solve(z,p->zv,p->xv,p->b->zv,p->b->xv);
		crosses[ncross++]= x;
	   }	while( (p=p->f) != head );
	return(ncross);
   }

gintersect(z,crosses,head)
register int z; register int *crosses; struct gradvertex *head;
   {
	register int ncross, x; register struct gradvertex *p;

	ncross= 0;
	p= head;
	do
	   {
		if(p->zv > z && p->b->zv > z) continue;
		if(p->zv < z && p->b->zv < z)
		   {
			if(p->f->zv > z) continue;
			/* eliminate vertex */
			p->b->f= p->f;
			p->f->b= p->b;
			continue;
		   }
		x= solve(z,p->zv,p->xv,p->b->zv,p->b->xv);
		crosses[ncross++]= x;
	   }	while( (p=p->f) != head );
	return(ncross);
   }

sort(vec,n)
register int *vec; register int n;
   {
	/* sort the elements of vec into ascending order. */
	register int *above, *below, *last;
	register int temp;
	last = vec + (n-1);
	for(above=vec; above<last; above++)
	   {
		for(below=above+1; below<=last; below++)
		   {
			if(*above > *below)
			   {
				temp=   *above;
				*above= *below;
				*below= temp;
			   }
		   }
	   }
   }
solve(pnot,p1,q1,p2,q2)
register int pnot,p1,q1,p2,q2;
   {
	/* floating point version */
	double invslope;
	register int qnot;
	if(pnot==p1) return(q1);
	if(pnot==p2) return(q2);
	if(q1==q2) return(q1);
	invslope= (q1-q2)/( (double) (p1-p2));
	qnot= (pnot-p1)*invslope + (double) q1 + 0.5;
	return(qnot);
   }

interpol(verts,nvert)
/*
	fits a polynomial surface to specified values of
	p-velocity, s-velocity and density (in turn) for
	three (plane) or four specified points.  Uses
	gaussian elimination - partial pivoting subroutines.
*/
struct gradvertex *verts;
int nvert;
{
	double **a, b[4];
	int *rows;
	double **init_pivot();
	int solve_pivot();
	void pivot();
	void backsub();
	void reaugment();

	switch (nvert) {

	case 3:
		/*
		   Form of equation:
		       | a00 a01 a02 |   | ap |   | a03 |
		       | a10 a11 a12 | * | bp | = | a13 |
		       | a20 a21 a22 |   | cp |   | a23 |
		   Equation derived from
		       ap + bp * xv + cp * zv + dp * xv * zv = pv,
		   one equation for each vertex.  (For just three
		   vertices, dp = 0.)  an0 = 1.  an3 are the media
		   parameters specified at each grid point.
		*/

		a = init_pivot(nvert,&rows);
		a[0][0] = 1; a[0][1] = verts[0].xv;
		 a[0][2] = verts[0].zv + shift * 0.5;
		a[1][0] = 1; a[1][1] = verts[1].xv;
		 a[1][2] = verts[1].zv + shift * 0.5;
		a[2][0] = 1; a[2][1] = verts[2].xv;
		 a[2][2] = verts[2].zv + shift * 0.5;
		/* do p-velocity first */
		a[0][3] = verts[0].pv;
		a[1][3] = verts[1].pv;
		a[2][3] = verts[2].pv;
		if (solve_pivot(a, rows, nvert) < 0) {
			fprintf(stderr,"Singular matrix, check input vertices!\n");
			exit(-1);
		}
		backsub(a, nvert);
		ap = a[0][3];
		bp = a[1][3];
		cp = a[2][3];
		dp = 0.0;
		/* now do s-velocity using reaugmentation of a */
		b[0] = verts[0].sv;
		b[1] = verts[1].sv;
		b[2] = verts[2].sv;
		reaugment(a, b, rows, nvert);
		backsub(a, nvert);
		as = a[0][3];
		bs = a[1][3];
		cs = a[2][3];
		ds = 0.0;
		/* finally, do density */
		b[0] = verts[0].dv;
		b[1] = verts[1].dv;
		b[2] = verts[2].dv;
		reaugment(a, b, rows, nvert);
		backsub(a, nvert);
		ad = a[0][3];
		bd = a[1][3];
		cd = a[2][3];
		dd = 0.0;
		break;
	
	case 4:
		/*
		   Form of equation:
		       | a00 a01 a02 a03 |   | ap |   | a04 |
		       | a10 a11 a12 a13 | * | bp | = | a14 |
		       | a20 a21 a22 a23 |   | cp |   | a24 |
		       | a30 a31 a32 a33 |   | cp |   | a24 |
		   Equation derived from
		       ap + bp * xv + cp * zv + dp * xv * zv = pv,
		   one equation for each vertex.  (For just three
		   vertices, dp = 0.)  an0 = 1.  an4 are the media
		   parameters specified at each grid point.
		*/

		a = init_pivot(nvert,&rows);
		a[0][0] = 1; a[0][1] = verts[0].xv;
		 a[0][2] = verts[0].zv + shift * 0.5;
		 a[0][3] = verts[0].xv * a[0][2];
		a[1][0] = 1; a[1][1] = verts[1].xv;
		 a[1][2] = verts[1].zv + shift * 0.5;
		 a[1][3] = verts[1].xv * a[1][2];
		a[2][0] = 1; a[2][1] = verts[2].xv;
		 a[2][2] = verts[2].zv + shift * 0.5;
		 a[2][3] = verts[2].xv * a[2][2];
		a[3][0] = 1; a[3][1] = verts[3].xv;
		 a[3][2] = verts[3].zv + shift * 0.5;
		 a[3][3] = verts[3].xv * a[3][2];
		/* do p-velocity first */
		a[0][4] = verts[0].pv;
		a[1][4] = verts[1].pv;
		a[2][4] = verts[2].pv;
		a[3][4] = verts[3].pv;
		if (solve_pivot(a, rows, nvert) < 0) {
			fprintf(stderr,"Singular matrix, check input vertices!\n");
			exit(-1);
		}
		backsub(a, nvert);
		ap = a[0][4];
		bp = a[1][4];
		cp = a[2][4];
		dp = a[3][4];
		/* now do s-velocity using reaugmentation of a */
		b[0] = verts[0].sv;
		b[1] = verts[1].sv;
		b[2] = verts[2].sv;
		b[3] = verts[3].sv;
		reaugment(a, b, rows, nvert);
		backsub(a, nvert);
		as = a[0][4];
		bs = a[1][4];
		cs = a[2][4];
		ds = a[3][4];
		/* finally, do density */
		b[0] = verts[0].dv;
		b[1] = verts[1].dv;
		b[2] = verts[2].dv;
		b[3] = verts[3].dv;
		reaugment(a, b, rows, nvert);
		backsub(a, nvert);
		ad = a[0][4];
		bd = a[1][4];
		cd = a[2][4];
		dd = a[3][4];
		break;
	
	default:
		fprintf(stderr,"Error in interpol:  %d vertices unsupported\n",
			nvert);
		exit(-1);
	}
}