#include "layout.h"

static graph*  data;
parameter*     para;
int            iteration;
long int       temperature;
vector         center;

int export_each_timestep = 0;

int Edes;
int EminSQR;
int Tmax;
int Tinit;
int  SHAKE;
float  GRAVITY;
int MAXATTRACT;
int rounds;
int endTemp;

float ALPHA_R;		//Rotation
float ALPHA_R_SIN;
float ALPHA_R_COS;
float ALPHA_O;		//Oscillation
float ALPHA_O_COS;
float SIGMA_R;		//Rotation Reaction Intensity
float SIGMA_O;		//Oscillation Reaction Intensity

void set_params (int attractFlag,int springAlgo)
{
  float factor;
  int maxVertexWidth; /* Die maximale Vertexbreite */
  
  maxVertexWidth = 1;
  factor = (float)sqrt(1.0 * edgeCount(data) / vertexCount(data));
  factor = MAX (1, factor);

  Edes = (int)(factor * maxVertexWidth);
  EminSQR = SQR (maxVertexWidth);
  Tmax = Edes;
  Tinit = Edes/2;
  SHAKE = Edes/2;
  GRAVITY = para->grav;
  MAXATTRACT = 64 * (int)SQR(Edes);
  
  ALPHA_O = para->ao;
  ALPHA_O_COS = (float)cos (ALPHA_O / 2);
  
  ALPHA_R = para->ar;
  ALPHA_R_SIN = (float)sin (M_PI/2 + ALPHA_R / 2);
  ALPHA_R_COS = (float)ABS (cos (M_PI/2 + ALPHA_R / 2));

  SIGMA_O = para->so;
  SIGMA_R = (float)(para->sr < 0 ? 1.0 / vertexCount(data) : para->sr);
  
  temperature = SQR(Tinit) * vertexCount(data);
  if (para->rd == -1)
    if (para->termAbs == -1)
      if (para->termPer == -1) {
        rounds = 5 * vertexCount(data);
        endTemp = (int)(temperature * 0.01);
      } else {
        rounds = 50 * vertexCount(data);
        endTemp = (int)(temperature * 0.01 * para->termPer);
    } else {
      rounds = 50 * vertexCount(data);
      endTemp = para->termAbs;
  } else {
    rounds = para->rd;
    endTemp = 0;
  }
  //printf ("\nParameter: Edes: %d GRAVITY: %f\n %f %f %f %f \n%d %d\n", 
  //        Edes, GRAVITY, ALPHA_O, ALPHA_R, SIGMA_O, SIGMA_R, rounds, endTemp);
  printf ("\nParameter: Edes: %d Gravity: %f\n Rounds: %d EndTemp: %d\n", 
          Edes, GRAVITY, rounds, endTemp);
}

void init ()
{
  int rnd;
  vertex vert;
  vertexinfo *vinf;
  int scale = (int)sqrt(vertexCount(data))/2 * Edes;

  srand (1);
  center.x = center.y = center.z = 0;
  for_all_vertices (data, vert) {
    vinf = mod_vertex (data, vert);
	//Bad (favors lower-order bits)
    rnd = rand () % (scale + 1) - scale / 2;
    vinf->pos.x = rnd;//0
    rnd = rand () % (scale + 1) - scale / 2;
    vinf->pos.y = rnd;//0
    rnd = rand () % (scale + 1) - scale / 2;
    vinf->pos.z = rnd;//0
    vinf->imp.x = 0;
    vinf->imp.y = 0;
    vinf->imp.z = 0;
    vinf->heat = (float)Tinit;
    vinf->dir1 = 0;
    vinf->dir2 = 0;
    vinf->dir3 = 0;
  }
  iteration = 0;
}

/*
void edgeUpdateZero(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
					if(data->mat2d[vert][i] < 0) weightFunc=1.0+((-1*data->mat2d[vert][i]/DENOM_THRESH)/2.0);
					else weightFunc=1.0+((data->mat2d[vert][i]/DENOM_THRESH)/2.0);
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));

			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				    if(data->mat2d[i][vert] < 0) weightFunc= 1.0+((-1*data->mat2d[i][vert]/DENOM_THRESH)/2.0);
				    else weightFunc= 1.0+((data->mat2d[i][vert]/DENOM_THRESH)/2.0);
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));
			}
		  }
  }
}
void edgeUpdateOne(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				weightFunc= SQR(Edes) * (1.0 + (data->mat2d[vert][i] * 1.0) / 2.0);
			  impuls->x -= delta.x * norm / (long)weightFunc;
			  impuls->y -= delta.y * norm / (long)weightFunc;
			  impuls->z -= delta.z * norm / (long)weightFunc;
			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				if(data->mat2d[i][vert] < 0) weightFunc=SQR(Edes) * ( data->mat2d[i][vert] * -1.0) / 2.0;
				else weightFunc= SQR(Edes) * ( data->mat2d[i][vert] * 1.0) / 2;
			  impuls->x -= delta.x * norm / (long)weightFunc;
			  impuls->y -= delta.y * norm / (long)weightFunc;
			  impuls->z -= delta.z * norm / (long)weightFunc;
			}
		  }
  }
}
void edgeUpdateTwo(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				if(data->mat2d[vert][i] < 0) weightFunc=SQR(Edes) * ((data->mat2d[vert][i]/DENOM_THRESH) * -1.0) / 2.0;
				else weightFunc=SQR(Edes) * ((data->mat2d[vert][i]/DENOM_THRESH) * 1.0) / 2.0;
			  impuls->x -= delta.x * norm * (1 - (long)weightFunc);
			  impuls->y -= delta.y * norm * (1 - (long)weightFunc);
			  impuls->z -= delta.z * norm * (1 - (long)weightFunc);
			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				if(data->mat2d[i][vert] < 0) weightFunc=SQR(Edes) * ((data->mat2d[i][vert]/DENOM_THRESH) * -1.0) / 2.0;
				else weightFunc=SQR(Edes) * ((data->mat2d[i][vert]/DENOM_THRESH) * 1.0) / 2.0;
			  impuls->x -= delta.x * norm * (1 - (long)weightFunc);
			  impuls->y -= delta.y * norm * (1 - (long)weightFunc);
			  impuls->z -= delta.z * norm * (1 - (long)weightFunc);
			}
		  }
  }
}
void edgeUpdateThree(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				if(data->mat2d[vert][i] < 0) weightFunc=SQR(Edes) * ((data->mat2d[vert][i]/DENOM_THRESH) * -1.0) / 2.0;
				else weightFunc=SQR(Edes) * ((data->mat2d[vert][i]/DENOM_THRESH) * 1.0) / 2.0;
			  impuls->x -= delta.x * norm * (long)weightFunc;
			  impuls->y -= delta.y * norm * (long)weightFunc;
			  impuls->z -= delta.z * norm * (long)weightFunc;
			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				if(data->mat2d[i][vert] < 0) weightFunc=SQR(Edes) * ((data->mat2d[i][vert]/DENOM_THRESH) * -1.0) / 2.0;
				else weightFunc=SQR(Edes) * ((data->mat2d[i][vert]/DENOM_THRESH) * 1.0) / 2.0;
			  impuls->x -= delta.x * norm * (long)weightFunc;
			  impuls->y -= delta.y * norm * (long)weightFunc;
			  impuls->z -= delta.z * norm * (long)weightFunc;
			}
		  }
  }
}
void edgeUpdateFour(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				if(data->mat2d[vert][i] < 0) weightFunc=SQR(Edes) * ((data->mat2d[vert][i]/DENOM_THRESH) * -1.0) / 2.0;
				else weightFunc=SQR(Edes) * ((data->mat2d[vert][i]/DENOM_THRESH) * 1.0) / 2.0;
			  impuls->x -= delta.x * norm * (SQR(Edes)-(long)weightFunc);
			  impuls->y -= delta.y * norm * (SQR(Edes)-(long)weightFunc);
			  impuls->z -= delta.z * norm * (SQR(Edes)-(long)weightFunc);
			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				if(data->mat2d[i][vert] < 0) weightFunc=SQR(Edes) * ((data->mat2d[i][vert]/DENOM_THRESH) * -1.0) / 2.0;
				else weightFunc=SQR(Edes) * ((data->mat2d[i][vert]/DENOM_THRESH) * 1.0) / 2.0;
			  impuls->x -= delta.x * norm * (SQR(Edes)-(long)weightFunc);
			  impuls->y -= delta.y * norm * (SQR(Edes)-(long)weightFunc);
			  impuls->z -= delta.z * norm * (SQR(Edes)-(long)weightFunc);
			}
		  }
  }
}*/

void edgeUpdateZero(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0

			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
					if(data->mat2d[vert][i] < 0) weightFunc=-1*data->mat2d[vert][i]/DENOM_THRESH;
					else weightFunc=data->mat2d[vert][i]/DENOM_THRESH;
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));

			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0

			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
					if(data->mat2d[i][vert] < 0) weightFunc=-1*data->mat2d[i][vert]/DENOM_THRESH;
					else weightFunc=data->mat2d[i][vert]/DENOM_THRESH;
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));
			}
		  }
  }
}
void edgeUpdateOne(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				    if(data->mat2d[vert][i] < 0) weightFunc= 1.0-(-1*data->mat2d[vert][i]/DENOM_THRESH);
				    else weightFunc= 1.0-(data->mat2d[vert][i]/DENOM_THRESH);
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));
			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
				    if(data->mat2d[i][vert] < 0) weightFunc= 1.0-(-1*data->mat2d[i][vert]/DENOM_THRESH);
				    else weightFunc= 1.0-(data->mat2d[i][vert]/DENOM_THRESH);
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));
			}
		  }
  }
}
void edgeUpdateTwo(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
					if(data->mat2d[vert][i] < 0) weightFunc=1.0/(-1.0*data->mat2d[vert][i]/DENOM_THRESH + .001);
					else weightFunc=1.0/(data->mat2d[vert][i]/DENOM_THRESH + .001);
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));
			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
					if(data->mat2d[i][vert] < 0) weightFunc=1.0/(-1.0*data->mat2d[i][vert]/DENOM_THRESH + .001);
					else weightFunc=1.0/(data->mat2d[i][vert]/DENOM_THRESH + .001);
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));
			}
		  }
  }
}
void edgeUpdateThree(vertex vert, int attractFlag, vector* impuls) {
	int i, norm; vertex other; vector delta;
	double weightFunc;
	vector coord = get_vertex(data,vert).pos;

	for(i=0;i<vert;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[vert][i] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
					if(data->mat2d[vert][i] < 0) weightFunc=-1.0/(data->mat2d[vert][i]/DENOM_THRESH + .001);
					else weightFunc=1.0/(data->mat2d[vert][i]/DENOM_THRESH + .001);
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));
			}
		  }
  }
  for(i=vert+1;i<data->numVerts;i++) {
	  if(vert!=data->numVerts)
		  if( data->mat2d[i][vert] != INAN ) {
			other = data->VERTEX[i+1];//Not [vert=0] or always 0
			if(attractFlag==1) {
				delta.x = get_vertex(data,other).pos.x - coord.x;
				delta.y = get_vertex(data,other).pos.y - coord.y;
				delta.z = get_vertex(data,other).pos.z - coord.z;
				norm = NORMSQR(delta);
				norm = MAX (norm, MAXATTRACT);
			}
			else {
				delta.x = coord.x - get_vertex(data,other).pos.x;
				delta.y = coord.y - get_vertex(data,other).pos.y;
				delta.z = coord.z - get_vertex(data,other).pos.z;
				norm = NORMSQR(delta);
				norm = MIN (norm, MAXATTRACT);
			}
			if (norm > EminSQR) {
					if(data->mat2d[i][vert] < 0) weightFunc=-1.0/(1.0 - data->mat2d[i][vert]/DENOM_THRESH + .001);
					else weightFunc=1.0/(1.0 - data->mat2d[i][vert]/DENOM_THRESH + .001);
				  impuls->x -= (long)(((double)delta.x * (double)norm * weightFunc)/SQR(Edes));
				  impuls->y -= (long)(((double)delta.y * (double)norm * weightFunc)/SQR(Edes));
				  impuls->z -= (long)(((double)delta.z * (double)norm * weightFunc)/SQR(Edes));
			}
		  }
  }
}

vector impulse (vertex vert, int attractFlag, int springAlgo)
{
  vertex other;
  int norm;
  vector impuls, delta;
  vector coord = get_vertex(data,vert).pos;

  impuls.x = rand () % (2 * SHAKE + 1) - SHAKE;
  impuls.y = rand () % (2 * SHAKE + 1) - SHAKE;
  impuls.z = rand () % (2 * SHAKE + 1) - SHAKE;

  impuls.x += (long)((center.x / vertexCount(data) - coord.x) * GRAVITY);
  impuls.y += (long)((center.y / vertexCount(data) - coord.y) * GRAVITY);
  impuls.z += (long)((center.z / vertexCount(data) - coord.z) * GRAVITY);

  for_all_vertices (data, other) {
    if (other != vert) {
	  delta.x = coord.x - get_vertex(data,other).pos.x;
	  delta.y = coord.y - get_vertex(data,other).pos.y;
	  delta.z = coord.z - get_vertex(data,other).pos.z;
      norm = NORMSQR(delta);
      if (norm != 0) {
        impuls.x += delta.x * SQR(Edes) / norm;
        impuls.y += delta.y * SQR(Edes) / norm;
        impuls.z += delta.z * SQR(Edes) / norm;
        if (norm < EminSQR) {
          impuls.x += delta.x * EminSQR / norm;
          impuls.y += delta.y * EminSQR / norm;
          impuls.z += delta.z * EminSQR / norm;
        }
      }
    }
  }

  if(springAlgo==0) edgeUpdateZero(vert, attractFlag, &impuls);
  else if(springAlgo==1) edgeUpdateOne(vert, attractFlag, &impuls);
  else if(springAlgo==2) edgeUpdateTwo(vert, attractFlag, &impuls);
  else if(springAlgo==3) edgeUpdateThree(vert, attractFlag, &impuls);
  //else if(springAlgo==4) edgeUpdateFour(vert, attractFlag, &impuls);
  else {printf("  Critical Error: Unsupported Spring Embedder algorithm\n"); exit(-1); }

  /*//Original GEM code
  edge_iterator(data, vert, kante) {//For each edge adjacent to vert
	//Get position of all connected edges
    other = this_vertex (data, kante);
    delta.x = coord.x - get_vertex(data,other).pos.x;
    delta.y = coord.y - get_vertex(data,other).pos.y;
    delta.z = coord.z - get_vertex(data,other).pos.z;

    norm = NORMSQR(delta);
    norm = MIN (norm, MAXATTRACT);
    if (norm > EminSQR) {
      int weightFunc = (int)(SQR(Edes) * (1 + (get_vertex(data,vert).weight * 1.0) / 2));
      impuls.x -= delta.x * norm / weightFunc;
      impuls.y -= delta.y * norm / weightFunc;
      impuls.z -= delta.z * norm / weightFunc;
    }
  }*/
  return impuls;
}

int temperatureUpdate (vertex vert, vector impuls)
{
  int temp;
  int norm1, norm2;
  float cosinus;
  vertexinfo *vinf;
  
  vinf = mod_vertex (data, vert);
  temp = (int)vinf->heat;
  if ((norm1 =  (int)NORM(impuls)) != 0) {
    impuls.x = impuls.x * temp / norm1;
    impuls.y = impuls.y * temp / norm1;
    impuls.z = impuls.z * temp / norm1;
    vinf->pos.x += impuls.x;
    vinf->pos.y += impuls.y;
    vinf->pos.z += impuls.z;
    center.x += impuls.x;
    center.y += impuls.y;
    center.z += impuls.z;
    if ((norm2 = NORMSQR(vinf->imp) != 0) && (NORMSQR (impuls) !=0)) {
      temperature -= SQR(temp);
      cosinus = (float)COS (vinf->imp, impuls);
      if (ABS(cosinus) >= ALPHA_O_COS) {
        temp += (int)(SIGMA_O * cosinus);
      }
      if (ABS(cosinus) <= ALPHA_R_COS) {
        vinf->dir1 += SIGMA_R * RotXY (vinf->imp, impuls);
        vinf->dir2 += SIGMA_R * RotYZ (vinf->imp, impuls);
        vinf->dir3 += SIGMA_R * RotXZ (vinf->imp, impuls);
      }
      temp = (int)MIN(Tmax, temp * pow ((1 - ABS(vinf->dir1)) *
                                        (1 - ABS(vinf->dir2)) *
                                        (1 - ABS(vinf->dir3)),1.f/3.f) );
      vinf->heat = (float)temp;
      temperature += SQR(temp);
    }
    vinf->imp.x = impuls.x;
    vinf->imp.y = impuls.y;
    vinf->imp.z = impuls.z;
  }
  return temperature;
}

int spring_iteration (int attractFlag, int springAlgo)
{
  vertex vert, aktVert;
  static int iteration;
  for_all_vertices (data, vert) {
	/* if (para->rnd == true) {
	      aktVert = selectVert (iteration);
	   } else {
	*/
    aktVert = vert;
	temperatureUpdate (aktVert, impulse (aktVert,attractFlag,springAlgo));
    iteration++;
  }
  return(1);
}

void set_export_each_timestep(int exportBool) {
	export_each_timestep=exportBool;
}

void layout_graph_main (graph* d, parameter* p, int delay, int iterations) {
  //Early convergence parameters (temperature changes less than .01% in 500 steps)
  int sameTempConverge = 500;
  long int startTemp;
  float percentConverge=0.0001f;
  int lastTemp = INT_MAX;
  int sameTempRound=0;

  int counter=1;
  data = d;
  para = p;
  set_params (p->attractFlag,p->springAlgo);
  init ();
  startTemp=temperature;
  
  if(iterations>0 && iterations<rounds) rounds=iterations;

  while (rounds-- && (temperature>endTemp) && sameTempRound<sameTempConverge ) {
    printf ("Round/Temp: %d %d\n", counter++, temperature);
    spring_iteration(p->attractFlag,p->springAlgo);
	if((counter%sameTempConverge)==0) write_layout("temp.gvl",d,p);
	if ((lastTemp-temperature)<startTemp*percentConverge) sameTempRound++;
	else {sameTempRound=0; lastTemp=temperature;}


	if(export_each_timestep!=0) {
		FILE* exportFP;
		char exportFN[100];
		float* exportLoc=NULL;
		int i, exportNumVerts;
		get_layout(d,p,&exportNumVerts,&exportLoc);
		sprintf(exportFN,"time_%d.txt",counter);
		exportFP=fopen(exportFN,"w");
		if(exportFP==NULL) {perror("Can't write export file"); exit(-1);}
		fprintf(exportFP,"%d\n",exportNumVerts);
		for(i=0;i<exportNumVerts;i++) {
			fprintf(exportFP,"%f %f %f\n",exportLoc[i*3],exportLoc[i*3+1],exportLoc[i*3+2]);
		}
		fclose(exportFP);
	}
	//Insert delay for visual simulation if desired
	if(delay!=0) {
		#ifdef _WIN32
			Sleep(delay);
		#else
			usleep(delay*1000);
		#endif
	}
  }
  printf("  Final temp %0.1f%%\n",100*temperature/(float)startTemp);
  /*
  if(sameTempRound>=sameTempConverge) printf("  SubOptimal Convergence Warning - Final temp %0.1f%%\n",100*temperature/(float)startTemp);
  else printf("Round=%d, Final Temp = %d (%f%%)\n",counter,temperature,100*temperature/(float)startTemp);
  */
}

void layout_graph_iterations (graph* d, parameter* p, int iterations)
{
	layout_graph_main (d,p,0,iterations);
}

void layout_graph_delay (graph* d, parameter* p, int delay)
{
	layout_graph_main (d,p,delay,RAND_MAX);
}

void layout_graph (graph* d, parameter* p)
{
	layout_graph_main (d,p,0,RAND_MAX);
}

graph* init_graph (string name, int n_vert, int n_edge, short int** mat2d, LabelList* edgeLabels)
{
	graph* result;
	if (!(n_vert > 0 && n_edge > 0)) {
		printf("Can't initalize a blank graph\n");
		exit (-1);
	}

	result = (graph*) malloc (sizeof (graph));
	result->name = strdup (name);
	
	result->numVerts=n_vert;
	result->numExistentEdges=n_edge;
	result->numEdges=n_vert*n_vert/2;

	result->mat2d=mat2d;
	result->edgeLabels=edgeLabels;

	result->vi     = (vertexinfo*) malloc ((1 + n_vert) * sizeof (vertexinfo));
	result->VERTEX = (vertex*) malloc ((2 * result->numEdges + 1) * sizeof (vertex));
	result->VERTEX+= result->numEdges;
  
	return (result);
}

parameter* init_param(float ao, float ar, float so, float sr, float grav, int rd, int termAbs, int termPer) {
	parameter* param;
	param = (parameter*)malloc(sizeof (parameter));
	param->ao=ao;
	param->ar=ar;
	param->so=so;
	param->sr=sr;
	param->grav=grav;
	param->rd=rd;
	param->termAbs=termAbs;
	param->termPer=termPer;
	param->springAlgo=0;
	return (param);
}

parameter* init_param_default(int attractFlag, int springAlgo) {
  parameter* param;
  param = (parameter*)malloc(sizeof (parameter));
  param->ao = 1.0471976f;
  param->ar = 1.0471976f;
  param->so = 0.1f;
  param->sr = -1.0f;
  //param->grav = 0.1f;
  param->rd = -1;
  param->termAbs = -1;
  param->termPer = -1;
  param->springAlgo = 0;
  return(param);
}

void get_layout (graph* data, parameter* p, int* numVerts, float** vertLocs) {
	int idx;
	float *locs;
	vertex vert;
	
	locs=(*vertLocs);
	if(locs!=NULL) free(locs);
	locs=(float*)malloc(data->numVerts*3*sizeof(float));

	idx=0;
	for_all_vertices (data, vert) {
		locs[idx*3]   = (float)get_vertex(data,vert).pos.x/DENOM_THRESH;
		locs[idx*3+1] = (float)get_vertex(data,vert).pos.y/DENOM_THRESH;
		locs[idx*3+2] = (float)get_vertex(data,vert).pos.z/DENOM_THRESH;
		idx++;
	}

	// Make data accessible
	(*vertLocs)=locs;
	(*numVerts)=data->numVerts;
}

void write_layout (string filename, graph* data, parameter* p) {
	vertex vert;
	FILE* fout;
	fout = fopen(filename,"w");
	if(fout==NULL) {
		printf("Error: can't open %s for writing.\n", filename);
	}
	else {
		//Input parameters (rma_pearsons.85.wel 0.85 1 1 0.1 0)
		fprintf(fout,"%s\t%f\t%d\t%d\t%f\t%d\n",data->name,p->thresh,p->absValFlag,p->attractFlag,p->grav,p->springAlgo);
		//Graph properties
		fprintf(fout,"%d\t%d\n",data->numVerts,data->numExistentEdges);
		for_all_vertices (data, vert) {
			fprintf(fout,"%s\t%d\t%d\t%d\n",ll_getLabel(data->edgeLabels,vert-1),get_vertex(data,vert).pos.x,get_vertex(data,vert).pos.y,get_vertex(data,vert).pos.z);
		}
	}
	fclose(fout);
}

void write_graph(char* filename, graph* data, string format) {
	int v1,v2;
	FILE* fout;
	fout = fopen(filename,"w");
	if(fout==NULL) {
		printf("Error: can't open %s for writing.\n", filename);
	}
	else {
		//Inefficient with if statements needlessly in loops, but most clear
		if(strcmp(format,"row_major")==0) {
			printf("Writing file...\n");
			fprintf(fout,"%d\n",data->numVerts);
			for(v1=0;v1<data->numVerts;v1++) {
				for(v2=0;v2<data->numVerts;v2++) {
					if(v1<v2) {
						if(data->mat2d[v2][v1]==INAN) {
							fprintf(fout,"%E\n",0.0);
						} else {
							fprintf(fout,"%E\n",data->mat2d[v2][v1]/DENOM_THRESH);
						}
					}
					else if(v1>v2) {
						if(data->mat2d[v1][v2]==INAN) {
							fprintf(fout,"%E\n",0.0);
						} else {
							fprintf(fout,"%E\n",data->mat2d[v1][v2]/DENOM_THRESH);
						}
					}
					else fprintf(fout,"%E\n",1.0); //on diagonal
				}
			}
			printf("  Wrote %s\n",filename);
		}
      else if (strcmp(format,"veg")==0) {
			printf("Writing file...\n");
         //Number and indices of selected vertices
			fprintf(fout,"%d\n",data->numDisplayVerts);
			for(v1=0;v1<data->numDisplayVerts;v1++) {
            fprintf(fout,"%d\t",data->vertList[v1]);
         }
         fprintf(fout,"\n\n");
         //Number and indices of selected edges
			fprintf(fout,"%d\n",data->numDisplayEdges);
			for(v1=0;v1<data->numDisplayEdges;v1++) {
            fprintf(fout,"%d\t",data->edgeList[v1]);
         }
         fprintf(fout,"\n\n");
         //Print out the adjacency matrix LTM
			fprintf(fout,"%d %d",data->numVerts,data->numEdges);
			for(v1=0;v1<data->numVerts;v1++) {
				for(v2=0;v2<v1;v2++) {
               fprintf(fout,"%f\t",data->mat2d[v1][v2]/DENOM_THRESH);
            }
            fprintf(fout,"\n");
         }
			printf("  Wrote %s\n",filename);
      }
		else {
			printf("Unrecognized format: %s\n",format);
		}
		fclose(fout);
	}
}

void read_graph(char* filename, graph* data, string format) {
	int v1,v2;
   int numVerts,numEdges;
   float edgeWeight;
	FILE* fin;
	fin = fopen(filename,"r");
	if(fin==NULL) {
		printf("Error: can't open %s for reading, disregarding.\n", filename);
	}
	else {
		//Inefficient with if statements needlessly in loops, but most clear
		if( strcmp(format,"veg")==0 ) {
			printf("Reading file...\n");
         //Number and indices of selected vertices
			fscanf(fin,"%d\n",&(data->numDisplayVerts));
			for(v1=0;v1<data->numDisplayVerts;v1++) {
            fscanf(fin,"%d\t",&(data->vertList[v1]));
         }
         fscanf(fin,"\n\n");
         //Number and indices of selected edges
			fscanf(fin,"%d\n",&(data->numDisplayEdges));
			for(v1=0;v1<data->numDisplayEdges;v1++) {
            fscanf(fin,"%d\t",&(data->edgeList[v1]));
         }
         fscanf(fin,"\n\n");
         //Print out the adjacency matrix LTM
			fscanf(fin,"%d %d",&(numVerts),&(numEdges));
         if(numVerts!=data->numVerts || numEdges!=data->numEdges) {
            printf("  Incompatible graph size: only display data used.\n");
         }
         else {
			   for(v1=0;v1<data->numVerts;v1++) {
				   for(v2=0;v2<v1;v2++) {
                  fscanf(fin,"%f\t",&edgeWeight);
                  data->mat2d[v1][v2] = (short int)(edgeWeight*DENOM_THRESH);
               }
               fscanf(fin,"\n");
            }
			   printf("  read %s\n",filename);
         }
      }
		else {
			printf("Unrecognized format: %s\n",format);
		}
		fclose(fin);
	}
}