/*
flow12.c
Copyright (C) 2003 Robert Geist, Karl Rasche, Robert Schalkoff, James Westall

This uses the modifications, described in lb6.tex, to the local
equilibrium distribution.  Pressure is now independent of velocity 
(u^2) and under user control as p = (3v^2 rho)/(6+R), where R is
a ratio of rest to non-rest density.

The interaction between rest and non-rest densities is 
controlled by the matrix term 'b' in the last row/col.
*/

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

#define RMG_GRAPHICS 1
#define DIRECTIONS 7
#define WIDTH 1024
#define HEIGHT 65

#define INJECTION_RATE 0.333333333

/*
 * In lb6.tex notation, rho = 6A + Ahat.  The ratio, R = Ahat/A,
 * controls pressure.  Write A = rho*A_wt and Ahat = rho*Ahat_wt,
 * and then R = (Ahat_wt/A_wt) and Ahat_wt = 1 - 6*A_wt.  Note
 * that A_wt is constrained to (0,1/6).
 *
 * Danger Wil Robinson! If A_wt is too high, pressure will be too high,
 * and the system will lose stability very quickly.  Diddle with this
 * at your own risk. ... 1.0/17.92 works, 1.0/17.91 is too large.
 */ 
#define A_wt (1.0/18.0)
#define Ahat_wt (1.0 - 6.0*A_wt)
#define R (Ahat_wt/A_wt)
#define REST_ALLOC (Ahat_wt/(6.0*A_wt))

#define NOMINAL_VISCOSITY 0.1
#define P_LAW_EXP -0.5

#define SQRT_3 1.732050807

#define lambda 1.0	/* lattice spacing */
#define tau (1.0/2.0)	/* time step ... affects viscosity */
#define v (lambda/tau)	/* unit velocity ... ditto */

#define FINAL_TIME (20.0*((double)(WIDTH))*tau)

struct ivector {
	int x;	/* col */
	int y;	/* row */
	}; 

/* 
mapping to rectangular coordinates is even/odd row dependent
			     <---- etc

   --- * --- x --- 0         <---- row 3
  \   / \   / \   / \
   \ /   \ /   \ /   \
    * --- x --- 0 --- x      <---- row 2
   / \   / \   / \   /
  /   \ /   \ /   \ /
   --- * --- x --- 0         <---- row 1
  \   / \   / \   / \ 
   \ /   \ /   \ /   \
    * --- x --- 0 --- x      <---- row 0

     * => 0'th column
     x => odd column
     0 => even column

plan here is to use ci[k][row%2]; the basic scheme is that
the rows are straight and the columns wiggle, since odd rows
are offset 1/2 to the right of even rows, so:
dir 0 is same row, +1 col., either parity
dir 1 is up one row, same col. if even row, next col. if odd row
dir 2 is up one row, prev. col. if even row, same col. if odd row
dir 3 is same row, -1 col., either parity
dir 4 is down one row, prev. col. if even row, same col. if odd row
dir 5 is down one row, same col. if even row, next col. if odd row
dir 6 is nowhere ... for rest density
*/

struct ivector ci[DIRECTIONS][2] = {
	1,0,1,0,   
	0,1,1,1,
	-1,1,0,1,
	-1,0,-1,0,
	-1,-1,0,-1,
	0,-1,1,-1,
	0,0,0,0
	};

struct vector {
	double x;	/* col */
	double y;	/* row */
	} vf[DIRECTIONS]; 

struct vector c[] = {
	1.0, 0.0,
	0.5, 0.5*SQRT_3,
	-0.5, 0.5*SQRT_3,
	-1.0, 0.0,
	-0.5, -0.5*SQRT_3,
	0.5, -0.5*SQRT_3,
	0.0, 0.0
	};

/* coord here will just be output to viz routine */
struct lattice_point {
	struct vector coord;
	double f[DIRECTIONS];
	double f0[DIRECTIONS];
	struct vector u;
	double rho;
	double edot;	/* second invariant of rate-of-deformation tensor */
	int class;	/* inside, boundary, or outside	*/
	double *dist[DIRECTIONS];	/* where to send flow */
	int edot_class;	/* marker for edot boundary processing */
	} lattice[HEIGHT][WIDTH];

struct lattice_point lost_source, lost_sink;

#define NULLDIR (double *)(0)

double omega[DIRECTIONS][DIRECTIONS];

void mark_geometry();
void pp_geometry();
void init_lattice();
void load_collision_matrix(double viscosity);
void update_rho_and_u();
void update_f(double t);
void update_f0();
void update_edot();
double genrand();
void dump();

main()
{
double t;

t = 0.0;

mark_geometry();
pp_geometry();
load_collision_matrix(NOMINAL_VISCOSITY);
init_lattice();
while(t<FINAL_TIME){
	update_rho_and_u();
	update_f0();
	update_f(t);
	t += tau;
#ifdef RMG_GRAPHICS
	/* this dumps output to be piped to lbs.c */
	if(((int)(t/tau))%20==0) dump();
#endif
	}
}
		
#define IN 0
#define BORDER 1
#define OUT 2
#define SOURCE 3
#define SINK 4

/* 
Mark each node in the rectangle by hand; eventually, a gui can map to this.
*/
void mark_geometry()
{
int i,j;

/* mark all interior regions first */
for(i=1;i<64;i++){
	for(j=1;j<512;j++){
		lattice[i][j].class = IN;
		}
	}

for(i=15;i<50;i++){
	for(j=512;j<1024;j++){
		lattice[i][j].class = IN;
		}
	}
for(i=51;i<65;i++){
	for(j=513;j<1024;j++){
		lattice[i][j].class = OUT;
		}
	}
for(i=0;i<14;i++){
	for(j=513;j<1024;j++){
		lattice[i][j].class = OUT;
		}
	}

/* now the borders */
for(j=0;j<513;j++){
	lattice[0][j].class = BORDER;
	lattice[64][j].class = BORDER;
	}
for(j=513;j<1024;j++){
	lattice[14][j].class = BORDER;
	lattice[50][j].class = BORDER;
	}
for(i=0;i<15;i++) lattice[i][512].class = BORDER;
for(i=50;i<65;i++) lattice[i][512].class = BORDER;


for(i=1;i<64;i++) lattice[i][0].class = SOURCE;
for(i=15;i<50;i++) lattice[i][1023].class = SINK;
}

/*
for an IN node, (i,j), direction k flow out goes to

(i+ci[k][i%2].y,j+ci[k][i%2].x)

in direction k, unless that node is a BORDER node, in which 
case it bounces back to (i,j) in direction (k+3)%(DIRECTIONS-1), 
or a SINK node, in which case the flow gets added to exiting density

there is no flow out from BORDER nodes or SINK nodes, since
their directional densities are maintained at 0

SOURCE nodes are a subset of column 0 of the lattice; here 
we have to check whether ci[k][i%2].x == -1, in which case
that flow is sent to lost injection
*/
void pp_geometry()
{
int i,j,k,ty,tx;
for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		switch(lattice[i][j].class){

			case OUT:
			case SINK:
			case BORDER:
				for(k=0;k<DIRECTIONS;k++){
					lattice[i][j].dist[k] = NULLDIR;
					}
				break;
			case IN:
			case SOURCE:
				for(k=0;k<DIRECTIONS;k++){
					ty = i+ci[k][i%2].y;	
					tx = j+ci[k][i%2].x;	
					if(tx<0){
						lattice[i][j].dist[k] = &lost_source.f[k];
						continue;
						}
					switch(lattice[ty][tx].class){
						case IN:
						case SOURCE:
							lattice[i][j].dist[k] = &lattice[ty][tx].f[k];
							break;
						
						case SINK:
							lattice[i][j].dist[k] = &lost_sink.f[k];
							break;

						case BORDER:
							lattice[i][j].dist[k] = &lattice[i][j].f[(k+3)%(DIRECTIONS-1)];
							break;
						case OUT:
							fprintf(stderr,"eek\n");
							break;

						}
					}
				break;
			}
		}
	}
		
}

void init_lattice()
{
/* 
load random densities, except at the borders
source nodes should have INJECTION_RATE in direction 0 only 
*/
int i,j,k;
double lsum;

for(k=0;k<DIRECTIONS;k++){
	vf[k].x = v*c[k].x;
	vf[k].y = v*c[k].y;
	}

for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		lattice[i][j].coord.x = ((double)(j)) + ((double)(i%2))*0.5;
		lattice[i][j].coord.y = ((double)(i))*SQRT_3/2.0;
		}
	}

for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		switch(lattice[i][j].class){
			case IN:
				lsum = 0.0;
				for(k=0;k<DIRECTIONS-1;k++){
					lattice[i][j].f[k] = ((double)(genrand()));
					lsum += lattice[i][j].f[k];
					lattice[i][j].f0[k] = 0.0;
					}
				/* now be sure they sum to the same value */
				for(k=0;k<DIRECTIONS-1;k++){
					lattice[i][j].f[k] /= lsum;
					/* 1.0 for now */
					}
				lattice[i][j].f[DIRECTIONS-1] = REST_ALLOC*1.0;
				lattice[i][j].f0[DIRECTIONS-1] = 0.0;
				break;
			case OUT:
			case BORDER:
			case SINK:
				for(k=0;k<DIRECTIONS;k++){
					lattice[i][j].f[k] = 0.0;
					lattice[i][j].f0[k] = 0.0;
					}
				break;
			case SOURCE:
				for(k=0;k<DIRECTIONS-1;k++){
					lattice[i][j].f[k] = 0.0;
					lattice[i][j].f0[k] = 0.0;
					}
				lattice[i][j].f[0] = INJECTION_RATE;
				lattice[i][j].f[DIRECTIONS-1] = INJECTION_RATE*REST_ALLOC;
				break;
			
			}
		}
	}
}

void update_rho_and_u()
{
int i,j,k;

for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		if(lattice[i][j].class==BORDER ||
		   lattice[i][j].class==SINK ||
		   lattice[i][j].class==OUT) continue;
		lattice[i][j].rho = 0.0;
		lattice[i][j].u.x = 0.0;
		lattice[i][j].u.y = 0.0;
		for(k=0;k<DIRECTIONS;k++){
			lattice[i][j].rho += lattice[i][j].f[k];
			lattice[i][j].u.x += lattice[i][j].f[k]*vf[k].x;
			lattice[i][j].u.y += lattice[i][j].f[k]*vf[k].y;
			}
		lattice[i][j].u.x /= lattice[i][j].rho;
		lattice[i][j].u.y /= lattice[i][j].rho;
		}
	}
}


void update_f(double t)
{
double currentf[HEIGHT][WIDTH][DIRECTIONS];
int i,j,k,n;
double new_density;
double viscosity;
double total_lost_source;
double total_lost_sink;
double local_sum;
double *outptr;
/*
record the current values; update must be synchronous;

We would like to fix this by using f[2][DIRECTIONS] and 
"new" and "old" indices to avoid copying ... but, then 
we'd need to double the number of dist[] pointers and 
cross point them to the correct f[][] locations, and, 
we'd still have the problem, right here, of initializing
the "new" storage area to 0.0 on each iteration; memset
would be faster than this copy, but ...
*/

for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		for(k=0;k<DIRECTIONS;k++){
			currentf[i][j][k] = lattice[i][j].f[k];
			lattice[i][j].f[k] = 0.0;
			}
		}
	}

/*
use current u() values to load up, for each point, the second
invariant of the rate of deformation tensor, edot
*/
update_edot();
/* 
The measured total output density (going out the right end of
the tube) and the lost injection density (going out the left end 
of the tube) should sum to (#source nodes)*INJECTION_RATE
*/
for(k=0;k<DIRECTIONS;k++){
	lost_source.f[k] = 0.0;
	lost_sink.f[k] = 0.0;
	}
for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		if(lattice[i][j].class==BORDER ||
		   lattice[i][j].class==SINK ||
		   lattice[i][j].class==OUT) continue;
		viscosity = NOMINAL_VISCOSITY*pow(lattice[i][j].edot,P_LAW_EXP);
		load_collision_matrix(viscosity);
		for(k=0;k<DIRECTIONS;k++){
			new_density = 0.0;
			for(n=0;n<DIRECTIONS;n++){
				new_density += omega[k][n]*
					(currentf[i][j][n]-lattice[i][j].f0[n]);
				}
			outptr = lattice[i][j].dist[k];
			*outptr += (currentf[i][j][k] + new_density);
			}
		}
	}

/* 
now reset SOURCE nodes to INJECTION_RATE + whatever bounced back in; 
sum it up and put it all in x direction; 
*/
total_lost_source = 0.0;
for(i=0;i<HEIGHT;i++){
	if(lattice[i][0].class!=SOURCE) continue;
	local_sum = 0.0;
	for(k=0;k<DIRECTIONS-1;k++){
		local_sum += lattice[i][0].f[k];
		lattice[i][0].f[k] = 0.0;
		}
	lattice[i][0].f[0] = local_sum+INJECTION_RATE;
	/* 
	 * we're doing a ham-handed reset of rest density in SOURCE, so 
	 * we should keep tabs on the net effect
	 */
	total_lost_source += lattice[i][0].f[DIRECTIONS-1];
	lattice[i][0].f[DIRECTIONS-1] = REST_ALLOC*INJECTION_RATE;
	}

/*  gather all exiting density so we can keep track of mass flow */
total_lost_sink = 0.0;
for(k=0;k<DIRECTIONS;k++){
	total_lost_sink += lost_sink.f[k];
	total_lost_source += lost_source.f[k];
	}
fprintf(stderr,"at time %f lost source: %f lost_sink: %f\n",t,total_lost_source,total_lost_sink);
if(total_lost_sink>1000000.0){
	fprintf(stderr,"lost stability\n");
	exit(1);
	}
}

double dot(struct vector u, struct vector vv)
{
return(u.x*vv.x + u.y*vv.y);
}

void update_f0()
{
/* 
 * new expression ... see lb6.tex, but it matches previous if A_wt = 1/12.0
 */
struct vector u;
double rho;
int i,j,k;
for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		u.x = lattice[i][j].u.x;
		u.y = lattice[i][j].u.y;
		rho = lattice[i][j].rho;
		for(k=0;k<DIRECTIONS-1;k++){
			lattice[i][j].f0[k] = A_wt*rho + 
					rho*dot(c[k],u)/(3.0*v) -
					rho*dot(u,u)/(6.0*v*v) +
					2.0*rho*(c[k].x*c[k].x*u.x*u.x +
					2.0*c[k].x*c[k].y*u.x*u.y +
					c[k].y*c[k].y*u.y*u.y)/(3.0*v*v);
			}
		lattice[i][j].f0[DIRECTIONS-1] = Ahat_wt*rho - rho*dot(u,u)/(v*v);
		}
	}
}


double
genrand()
{
static int flag;
if (flag==0){
   srandom(123456789);
   srand(1);
   flag=1;
   }
return(((double)(random()+1))/2147483649.);
}

void
dump()
{
int i,j;
struct lattice_point *lpp1, *lpp2, *rowbase;

lpp1 = &lattice[0][0];
lpp2 = rowbase = &lattice[1][0];

for(i=0;i<HEIGHT-1;i++,lpp1=rowbase,rowbase=lpp2){
	for(j=0;j<WIDTH;j++,lpp1++,lpp2++){
		printf("%f %f %f %f %f\n",lpp1->coord.x,lpp1->coord.y,
					lpp1->u.x,lpp1->u.y,lpp1->rho);
		printf("%f %f %f %f %f\n",lpp2->coord.x,lpp2->coord.y,
					lpp2->u.x,lpp2->u.y,lpp2->rho);
		}
	}
}

			
void load_collision_matrix(double viscosity)
{
double a_0, a_60, a_120, a_180, b, c, e1;

/* 
The row/col 6 entries are b b b b b b c.

The eigenvectors for 0 are now: (1,1,1,1,1,1,1), 
(c_0x,c_1x,c_2x,c_3x,c_4x,c_5x,0), and (c_0y,c_1y,c_2y,c_3y,c_4y,c_5y,0), 
with associated constraints:

a_0 + 2a_60 + 2a_120 + a_180 + b = 0
a_0 + a_60 - a_120 - a_180 = 0
6b + c = 0

There are three non-zero eigenvalues: 
e_1 = 6a_0 + 6a_60 + 2b; eigenvectors (1,0,-1,1,0,-1,0) and (1,-2,1,1,-2,1,0)
e_2 = -6a_0 - 12a_60 -3b; eigenvector (1,-1,1,-1,1,-1,0)
e_3 = -7b; eigenvector (1,1,1,1,1,1,-6)

The eigenvalue e_1 is determined by the target viscosity:
viscosity = ((-tau v^2)/4)(1/e_1 + 1/2);

The others we force to be -1 (so I+Omega iterations converge quickly).

This gives:
a_0 = (1/3)e_1 - 4/21;
a_60 = -(1/6)e_1 + 1/7;
a_120 = -b - 2a_0 - 3a_60;
a_180 =  b + 3a_0 + 4a_60;
b = 1/7
c = -6/7;
*/

e1 = -2.0*tau*v*v/(8.0*viscosity + tau*v*v);
a_0 = e1/3.0 - 4.0/21.0;
a_60 = -e1/6.0 + 1.0/7.0;
b = 1.0/7.0;
a_120 = -b - 2.0*a_0 - 3.0*a_60;
a_180 = b + 3.0*a_0 + 4.0*a_60;
c = -6.0/7.0;

/* we need to do this often, so unroll the loop */

omega[0][0] = a_0;
omega[0][1] = a_60;
omega[0][2] = a_120;
omega[0][3] = a_180;
omega[0][4] = a_120;
omega[0][5] = a_60;
omega[0][6] = b;

omega[1][0] = a_60;
omega[1][1] = a_0;
omega[1][2] = a_60;
omega[1][3] = a_120;
omega[1][4] = a_180;
omega[1][5] = a_120;
omega[1][6] = b;

omega[2][0] = a_120;
omega[2][1] = a_60;
omega[2][2] = a_0;
omega[2][3] = a_60;
omega[2][4] = a_120;
omega[2][5] = a_180;
omega[2][6] = b;

omega[3][0] = a_180;
omega[3][1] = a_120;
omega[3][2] = a_60;
omega[3][3] = a_0;
omega[3][4] = a_60;
omega[3][5] = a_120;
omega[3][6] = b;

omega[4][0] = a_120;
omega[4][1] = a_180;
omega[4][2] = a_120;
omega[4][3] = a_60;
omega[4][4] = a_0;
omega[4][5] = a_60;
omega[4][6] = b;

omega[5][0] = a_60;
omega[5][1] = a_120;
omega[5][2] = a_180;
omega[5][3] = a_120;
omega[5][4] = a_60;
omega[5][5] = a_0;
omega[5][6] = b;

omega[6][0] = b;
omega[6][1] = b;
omega[6][2] = b;
omega[6][3] = b;
omega[6][4] = b;
omega[6][5] = b;
omega[6][6] = c;
}

/* 
use current u() to estimate second invariant of rate-of-deformation tensor;
we need est. of partials of u.x and partials of u.y; now, a problem with
getting the partial of either with respect to y is that the next lattice 
value in the y direction with the same x coordinate is 2 rows away ... 

only IN nodes and SOURCE nodes need edots; for SOURCE nodes (subset
of column 0, by def.), we copy from column 1 after calculating those; 
the only remaining problem will be the nodes where one of the next 
lattice values in y direction is outside the rectangle or of class OUT ...

note bug: if (i,j) is missing lower y, i.e. (i-2,j), we assume (i+2,j) 
and (i+4,j) are ok and interpolate ... so channels must be wide enough
*/

#define EPS 0.0001
#define EDOT_OK 0
#define MISSING_LOW 1
#define MISSING_HIGH 2

void update_edot()
{
double edot_xx, edot_xy, edot_yx, edot_yy;
double p_xy, p_yx;
double slope;
int i,j;

for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		if(lattice[i][j].class!=IN) continue;
		if((i-2)<0){
			lattice[i][j].edot_class = MISSING_LOW;
			continue;
			}
		if(lattice[i-2][j].class==OUT){
			lattice[i][j].edot_class = MISSING_LOW;
			continue;
			}
		if((i+2)>HEIGHT-1){
			lattice[i][j].edot_class = MISSING_HIGH;
			continue;
			}
		if(lattice[i+2][j].class==OUT){
			lattice[i][j].edot_class = MISSING_HIGH;
			continue;
			}
		lattice[i][j].edot_class=EDOT_OK;
		edot_xx = (lattice[i][j+1].u.x - lattice[i][j-1].u.x)/
			  (lattice[i][j+1].coord.x - lattice[i][j-1].coord.x);
		edot_yy = (lattice[i+2][j].u.y - lattice[i-2][j].u.y)/
			  (lattice[i+2][j].coord.y - lattice[i-2][j].coord.y);
		p_yx =  (lattice[i][j+1].u.y - lattice[i][j-1].u.y)/
			(lattice[i][j+1].coord.x - lattice[i][j-1].coord.x);
		p_xy =  (lattice[i+2][j].u.x - lattice[i-2][j].u.x)/
		  	(lattice[i+2][j].coord.y - lattice[i-2][j].coord.y);
		edot_xy = edot_yx = (p_xy + p_yx)/2.0;
		lattice[i][j].edot = sqrt(edot_xx*edot_xx + 2.0*edot_xy*edot_yx
						+edot_yy*edot_yy);
		if(lattice[i][j].edot<EPS) lattice[i][j].edot = EPS;
		}
	}
/* 
now we have to do something about the missing low y and missing high y cases
should link these to avoid mostly non-hit loop
*/
for(i=0;i<HEIGHT;i++){
	for(j=0;j<WIDTH;j++){
		if(lattice[i][j].class!=IN) continue;
		if(lattice[i][j].edot_class==EDOT_OK) continue;

		if(lattice[i][j].edot_class==MISSING_LOW){
			slope = (lattice[i+4][j].edot - lattice[i+2][j].edot)/
				(lattice[i+4][j].coord.y - lattice[i+2][j].coord.y);
			lattice[i][j].edot = slope*(lattice[i][j].coord.y - 
						lattice[i+2][j].coord.y)+
						lattice[i+2][j].edot;
			}
		else {
			slope = (lattice[i-4][j].edot - lattice[i-2][j].edot)/
				(lattice[i-4][j].coord.y - lattice[i-2][j].coord.y);
			lattice[i][j].edot = slope*(lattice[i][j].coord.y - 
						lattice[i-2][j].coord.y)+
						lattice[i-2][j].edot;
			}
		if(lattice[i][j].edot<EPS) lattice[i][j].edot=EPS; 
		}
	}
/*
finally, fix the source nodes
*/
for(i=0;i<HEIGHT;i++){
	if(lattice[i][0].class==SOURCE){
		lattice[i][0].edot = lattice[i][1].edot;
		}
	}
}