/* 	3d SOR solver
	by Matthew Gray, <mkgray@mit.edu> */

#include "main.h"
#include "sor.h"
#include "xgraphics.h"

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

fpt     *Data;
char    *realData;
int     *Boundary;
int	*Objects;
int c, i, j, k,ct;
double alpha;
fpt ch;
int viewslice;
char useX=0;
char dump=0;
int nobj;

extern NPTS;

void clear_data_and_boundaries();
void read_boundaries_file();
void set_boundaries(int o1, int o2);
fpt run_simulation(int f, int s);
void gaussj(float a[7][7], int n, float b[7][2], int m);

void
main(int argc, char **argv){
	extern char *optarg;
	extern int optind;
	int o1, o2, o3, o4;
	fpt cap, thermalnoise, tmp;
	fpt capmat[7][7];
	fpt solveme[6][6], solvemeo[6][6];
	fpt rhs[6][2];
	int node, ct;
	int FREQ = 250000;

	/* Process the command line options */

	NPTS = 50;
	while((c = getopt(argc, argv, "a:v:n:f:xd")) != EOF)
	  switch (c) {
	  case 'a':
	    alpha = (double) strtod(optarg, NULL);
	    break;
	  case 'v':
	    viewslice = (int) atoi(optarg);
	    break;
	  case 'n':
	    NPTS = (int) atoi(optarg);
	    break;
	  case 'd':
	    dump = 1;
	    break;
	  case 'f':
	    FREQ = (int) atoi(optarg);
	    break;
	  case 'x':
	    useX =  1;
	    break;
	  }
	fprintf(stderr, "Alpha is %f.\n", alpha);


	/* Allocate memory for field solver*/
	fprintf(stderr, "Allocating memory for a %d cubic grid\n", NPTS);
	Data = malloc(NPTS*NPTS*NPTS*sizeof(fpt));
	realData = malloc(NPTS*NPTS*sizeof(char));
	Boundary = malloc(NPTS*NPTS*NPTS*sizeof(int));
	Objects = malloc(NPTS*NPTS*NPTS*sizeof(int));

	if(useX)
		initialize_graphics();
	fprintf(stderr, "Initialized graphics\n");
	clear_data_and_boundaries();	
	read_boundaries_file();

	/* First, do the field solution for each pair of objects */

	for(o1=1; o1 <=nobj; o1++){
		for(o2=(o1+1);o2<=nobj;o2++) {
			if(o1 == o2)
				continue;
			fprintf(stderr, "Starting simulation between objects %d and %d.\n", o1, o2);
			set_boundaries(o1, o2);
			cap = run_simulation(o1, o2);
			capmat[o1][o2]=cap;
		}
	}

	/* Ok, now we do the circuit half of the problem */

	/* First, clear the matrix */
	for(o1=1;o1<7;o1++){
	  for(o2=1;o2<7;o2++){
	    solvemeo[o1][o2] = 0.0;
	    rhs[o1][1] = 0.0;
	    if(o1>=o2){
	      fprintf(stdout, "------ ");
	    }
	    else{
	      fprintf(stdout, "%6.4f ", capmat[o1][o2]*1000000000);
	    }
	  }
	  fprintf(stdout, "\n");
	}

	/* Now, set the total current to each node to 0 */
	for(o1=1;o1<6;o1++){
	  for(o2=1;o2<6;o2++){
	    if(o1>o2){
	      solvemeo[o1][o2] = FREQ*capmat[o2][o1];
	    }
	    else if (o2>o1){
	      solvemeo[o1][o2] = FREQ*capmat[o1][o2];
	    }
	  }
	}

	for(o1=1;o1<6;o1++){
	  solvemeo[o1][o1] =  0;
	  for(o2=1;o2<7;o2++){
	    if(o1 != o2){
	      if(o1>o2){
		solvemeo[o1][o1] -= FREQ*capmat[o2][o1];
	      }
	      else{
		solvemeo[o1][o1] -= FREQ*capmat[o1][o2];
	      }
	    }
	  }
	}

	for(o1=1;o1<6;o1++){
	  for(o2=1;o2<6;o2++){
	    solveme[o1][o2] = solvemeo[o1][o2];
	  }
	}

	/* Add the transmitter current */
	rhs[4][1] = .00100;
	rhs[3][1] = -.00100;

	gaussj(solveme, 5, rhs, 1);


	fprintf(stderr, "----------------------------------------\n");
	for(o1=1;o1<6;o1++){
	  for(o2=1;o2<6;o2++){
	    fprintf(stdout, "%4.4f ", solveme[o1][o2]);
	  }
	  fprintf(stdout, "    %4.10f\n", rhs[o1][1]);
	}

	fprintf(stdout, "Receiver voltage per transmission volt: %f\n",
		rhs[5][1]/(rhs[3][1]-rhs[4][1]));

	/* Noise propagation*/
	thermalnoise=0.0;
	for(o3=1;o3<6;o3++){
	  for(o4=1;o4<6;o4++){
	    if(o4>o3){
	      for(o1=1;o1<6;o1++){
		rhs[o1][1] = 0.0;
		for(o2=1;o2<6;o2++){
		  solveme[o1][o2] = solvemeo[o1][o2];
		}
	      }
	      rhs[o4][1] = .01;
	      rhs[o3][1] = -.01;
	      gaussj(solveme, 5, rhs, 1);
	      tmp = ((4.14e-21)/capmat[o3][o4])*rhs[5][1]/(rhs[o3][1]-rhs[o4][1]);
	      thermalnoise += (tmp*tmp);
	    }
	  }
	}
	fprintf(stdout, "Thermal noise in V^2: %.12f x 10^-9\n", thermalnoise*1000000000);

	for(o1=1;o1<6;o1++){
	  for(o2=1;o2<6;o2++){
	    solveme[o1][o2] = solvemeo[o1][o2];
	  }
	}

	/* Add the transmitter current */
	rhs[5][1] = 1.0;

	gaussj(solveme, 5, rhs, 1);

	fprintf(stdout, "Impedance at amplifier is %f\n", rhs[5][1]);

	exit(0);
}

fpt run_simulation(int firstob, int secondob) {
	int ct;
	fpt stab, cap;
	static lastsim = 0;

	stab = 1.0;
	ct = 0;
	if(firstob != lastsim){
	  lastsim = firstob;
	while((10000*stab/(NPTS*NPTS*NPTS) > 1) || (ct <=3)){
	  ct++;
	  stab = relax(alpha, Data, Boundary);
	  if(useX){
	    i = viewslice;
	    for(j = 0; j < NPTS; j++)
	      for(k=0;k<NPTS;k++) {
		Dd(realData, j, k) = (int) F(Data, i, j, k);
	      }
	    
	    update_image((char **) realData);
	  }
	  fprintf(stderr, ".");
	  if(ct%25 == 0) {
	    fprintf(stderr, "%d (%f)\n", ct, stab);
	  }
	}
	fprintf(stderr, "\n");
	if(dump){
	  fprintf(stdout, "Dump of potential map between %d and %d\n",
		  firstob, secondob);
	  for(i = 0; i < NPTS; i++) {
	    for(j=0;j<NPTS;j++) {
	      for(k=0;k<NPTS;k++){
		fprintf(stdout, "%d ", (int) F(Data, i, j, k));
	      }
	      fprintf(stdout, "\n");
	    }
	    fprintf(stdout, "\n");
	  }
	}
	}

	ch = charge(Data, Objects, secondob);
	fprintf(stderr, "Charge of object 2 is %f\n", ch);

	cap = fabs(8.854e-12*(ch/CHARGE_UP));
	ch = charge(Data, Objects, firstob);
	fprintf(stderr, "Charge of object 1 is %f\n", ch);

	fprintf(stderr, "Intraobject capacitance is %f\n", cap);

	return cap;
}


void
clear_data_and_boundaries() {
  int i,j,k;
	
  for (i = 0; i < NPTS; ++i)
    for (j = 0; j < NPTS; ++j) {
      for (k = 0; k < NPTS; ++k) {
	F(Data, i, j, k) = 0;
	F(Boundary, i, j, k) = -1;
	F(Objects, i, j, k) = -1;
      }
    }
}

void
set_boundaries(int obj1, int obj2){
  int i,j,k;
	
  for (i = 0; i < NPTS; ++i)
    for (j = 0; j < NPTS; ++j) {
      for (k = 0; k < NPTS; ++k) {
	if(F(Objects, i, j, k) == obj1)
		F(Boundary, i, j, k) = CHARGE_UP;	
	else if(F(Objects, i, j, k) > 0)
		F(Boundary, i, j, k) = 0;
	else 
		F(Boundary, i, j, k) = -1;
	}
	}
}

void
read_boundaries_file() {
/*
	Ok.  The boundaries file gives the locations of the equipotentials
	for the body(s), the ground, the txmit body plate, the txmit ground
	plate, the recv body plate, and the recv ground plate
	The format of the file is as follows:

	xcoord ycoord zcoord part

	where, xcoord, ycoord, and zcoord are integer coordinates
	and part is one of:

	1	Body
	2	Ground
	3	txmit body
	4	txmit ground
	5	recv body
	6	recv ground
*/

	char readme[100];
	char *sp;
	char *sp2;
	char *sp3;
	char *sp4;
	int i, j, k;
	int val, obj;
	int tmpobj=0;
	nobj = 0;

   while(fgets(readme, 100, stdin)){
	sp = (char *) strchr(readme, (int) ' ');
	*sp = (char) '\0';
	sp2= (char *) strchr((char *) sp+1, (int) ' ');
	*sp2 = '\0';
	sp3= (char *) strchr((char *) sp2+1, (int) ' ');
	*sp3 = '\0';
	i = atoi(readme);
	j = atoi(sp+1);
	k = atoi(sp2+1);
	obj = atoi(sp3+1);
	F(Objects, i, j, k) = obj;
	nobj = (nobj > obj) ? nobj:obj;
   }
}