/*
Description: This accepts a file that is the output produced from EvalAdjMats.c and finds the coordinates of all those adjacency matrices (that presumably all have solution).  This doesn't have a maximum number of iterations - so if it comes across an adjacency matrix without a physical solution, it will be stuck in an infinite loop.  If you think you will encounter such adjacency matrices - you should put in a maximum number of iterations.

The difference between 'FindCoordsOfSolutions2' and 'FindCoordsOfSolutions' is that this only prints the adjacency matrix, along with its header, the relative distances, the coordinates, and the 2nd moment, in that order.  Wheras the other file prints everything in the input file, with the coords, the 'relative distance conformatiom', and the 2nd moment directly after the adjacency matrix.

The difference between this and 'FindCoordsOfSolutions3' is that this one prints the coordinates and 2nd moment after the adjacency matrices and relative distance solutions - it does this by having the local functions finding the coordinates and actually update an array containing the coordinates (the 2nd moment is returned from the function, as it always has been).
*/

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

#define MAXLEN 100

/*Declare local functions*/
int find_coords(int [][MAXLEN], int, double *, double *);

int newton_iters(int [][MAXLEN], double *, int);

void num_grad(int [][MAXLEN], double [], double, int, double (*)[MAXLEN], int);

void dist_func(int [][MAXLEN], double [], double *, int);

void ludcmp(double (*)[MAXLEN], int, int, int *, double (*)[MAXLEN], double (*)[MAXLEN], int (*)[MAXLEN]);

void breakupLU(double (*)[MAXLEN], int, int, double (*)[MAXLEN], double (*)[MAXLEN]);

void get_perm_mat(int *, int, int (*)[MAXLEN]);

void inv(double (*)[MAXLEN], int, double (*)[MAXLEN]);

void forward_sub(double [][MAXLEN], int, double [], double *);

void back_sub(double [][MAXLEN], int, double [], double *);

void mat_mult(double [][MAXLEN], const int, const int, double [][MAXLEN], const int, const int, double (*)[MAXLEN]);

void rel_dists_func(double [], double *, int);

double second_moment(double [], int);

main() {
	
	/*Declare and initialize local variables*/
	int i, j, c, flag, row_count, col_count, first, iter_count, max_col, count, count2, count3, mat_count, min_mat, converge, document;
	int a[MAXLEN][MAXLEN];
	double moment2, min_moment;
	double coords[100]; //just making it some large number
	flag = 1;
	row_count = 0;
	col_count = 0;
	iter_count = 0;
	mat_count = 0;
	converge = 0;
	document = 0;
	count3 = 0;
	moment2 = 10000;
	min_moment = moment2;
	srand (time(NULL)); //seed the random number generator using the current time
	
	/*Read input graphs and relative distance solutions - solve for coordinates and print everything out.*/
	while ((c = getchar()) != EOF) {
		//printf("\ncount = %d, flag = %d\n",count,flag);
		putchar(c);
		++count;
		++count2;
		if (c == 'M') { //this is the start of a new adjacency matrix (specifically the header for a new adjacency matrix)
			flag = 1;
			count = 0;
		}
		else if (c == ':') {
			if (count == 6)
				flag = 0;
		}
		else if (flag == 0) { //save the current adjacency matrix read in the input file to the temporary adjacency matrix, a, which is an array.
			if (c == '1' || c == '0') {
				if (c == 49)
					a[row_count][col_count] = 1;
				if (c == 48)
					a[row_count][col_count] = 0;
				++col_count;
				iter_count = 1;
			}
			if (c == '\n') {
				if (iter_count == 1) {
					++row_count;
					max_col = col_count;
					col_count = 0; //re-initialize
				}
			}
		}
		if (c == 'R')
			count2 = 0;
		if (row_count == max_col) { //if we've finished copying the entire adjacency matrix, we can print it out or evaluate it.
			++mat_count;
			fprintf(stderr,"\nmatrix %d\n",mat_count);
			/*printf("\nadj mat has completed being copied\n");
			//Print the adjacency matrix out, if desired
			for (i = 0; i < max_col; ++i) {
				for (j = 0; j < max_col; ++j) {
					if (j < max_col - 1)
						printf("%d",a[i][j]);
					else
						printf("%d\n",a[i][j]);
				}
			}*/
			converge = find_coords(a, max_col, coords, &moment2);
			if ((converge == 1) && (moment2 < min_moment)) {
				min_moment = moment2;
				min_mat = mat_count;
			}
			row_count = 0; //re-initialize
			col_count = 0;
			iter_count = 0;
			flag = 1;
		}
		if ((c == ')') && (count2 == 1))
			document = 1;
		if ((document == 1) && (c == '\n'))
			++count3;
		if (count3 == max_col*(max_col-1)/2 + 1) { //print the coordinates and 2nd moment after having printed the solutions to the adjacency matrix
			if (converge == 1) {
				printf("\nCoordinates:\n");
				for (i = 0; i < 4; ++i)
					printf("0.0\n");
				printf("%6.15f\n",coords[max_col-2]);
				printf("0.0\n");
				printf("%6.15f\n%6.15f\n",coords[0],coords[max_col-2+1]);
				printf("0.0\n");
				for (i = 3; i < max_col; ++i)
					printf("%6.15f\n%6.15f\n%6.15f\n",coords[i-2],coords[max_col-2+i-1],coords[2*max_col-3+i-3]);
				printf("\n2nd moment = %6.15f\n",moment2);
			}
			count3 = 0;
			document = 0;
		}
	}
	
	printf("\n\nThe minimum moment is %6.15f, and corresponds to packing # %d.",min_moment,min_mat);
	
}

/*Local Functions*/

/*Description: Use Newton iterations to find the coordinates of an adjacency matrix*/
int find_coords(int a[][MAXLEN], int n, double *coords, double *mom2) {

	/*Declare and initialize local variables*/
	int i, j, num_vars, max_iters, converge;
	double moment2;
	if (n == 1)
		num_vars = 0;
	else if (n == 2)
		num_vars = 1;
	else
		num_vars = 3*n-6;
	converge = 0;
	j = 0;
	double rel_dists[n*(n-1)/2];
	
	while (converge == 0) {
		++j;
		for (i = 0; i < num_vars; ++i) {
			//coords[i] = ((double) rand() / (RAND_MAX+1.0))*4*n+1 - 2*n; //Creates a random floating point # btwn -2n and 2n.  RAND_MAX is the maximum value of the pseudo-random number generator, and is defined in <stdlib.h>
			coords[i] = ((double) rand() / (RAND_MAX+1.0))*8*n+1 - 4*n; //Creates a random floating point # btwn -4n and 4n.  RAND_MAX is the maximum value of the pseudo-random number generator, and is defined in <stdlib.h>
		}
		converge = newton_iters(a, coords, n);
		if (converge == 1) {
			rel_dists_func(coords, rel_dists, n);
			/*printf("\nrelative distances:\n");
			for (i = 0; i < n*(n-1)/2; ++i)
				printf("r[%d] = %6.12f\n",i,rel_dists[i]);*/
			for (i = 0; i < n*(n-1)/2; ++i) {
				if ((isnan(rel_dists[i]) == 1) || (rel_dists[i] < .99999)) {
					//printf("\ni = %d, I was called!\n",i);
					converge = 0;
					break;
				}
			}
		}
	}
	
	/*printf("\nrelative distances confirmation:\n");
	for (i = 0; i < n*(n-1)/2; ++i)
		printf("r[%d] = %6.12f\n",i,rel_dists[i]);*/
	
	if (converge == 1) {
		moment2 = second_moment(coords, n);
		//printf("\n2nd moment = %6.15f\n",moment2);
		*mom2 = moment2;
	}
	
	return converge;

}

/*Description: Perform newton iterations to find the zeros of a system of n equations.*/
int newton_iters(int a[][MAXLEN], double *guess, int n) {
	
	/*Declare and initialize local variables*/
	int i, j, max_iters, num_vars, tol_met, converge;
	double tol, step;
	if (n == 1)
		num_vars = 0;
	else if (n == 2)
		num_vars = 1;
	else
		num_vars = 3*n-6;
	max_iters = 20;
	converge = 1;
	step = pow(10,-6);
	tol = pow(10,-7);
	double dists[num_vars], residual[num_vars], grad[num_vars][MAXLEN], jac_times_res[num_vars][MAXLEN], y[num_vars][MAXLEN], inv_jac[num_vars][MAXLEN];
	
	for (i = 0; i < max_iters; ++i) {
		dist_func(a, guess, dists, n);
		tol_met = 1; //re-initialize
		for (j = 0; j < num_vars; ++j) {
			residual[j] = dists[j];
			if (fabs(residual[j]) > tol)
				tol_met = 0;
		}
		if (tol_met == 1)
			break;
		num_grad(a, guess, step, num_vars, grad, n);
		inv(grad, num_vars, inv_jac);
		for (j = 0; j < num_vars; ++j)
			y[j][0] = residual[j];
		mat_mult(inv_jac, num_vars, num_vars, y, num_vars, 1, jac_times_res);
		for (j = 0; j < num_vars; ++j)
			guess[j] -= jac_times_res[j][0];
	}
	
	if (i == max_iters) { //if we've reached the maximum number of iterations, then check whether the newest guess has converged to a solution
		dist_func(a, guess, dists, n);
		tol_met = 1; //re-initialize
		for (j = 0; j < num_vars; ++j) {
			residual[j] = dists[j];
			if (fabs(residual[j]) > tol)
				tol_met = 0;
		}
		if (tol_met == 0)
			converge = 0;
	}
	
	/*printf("\nresidual:\n");
	for (i = 0; i < num_vars; ++i)
		printf("%6.12f\n",residual[i]);*/
	
	return converge;

}

/*Description: Find the numerical gradient of a system of n equations.*/
void num_grad(int a[][MAXLEN], double vars[], double step, int num_vars, double (*grad)[MAXLEN], int n) {
	
	/*Declare and initialize local variables*/
	int i, j;
	double var_plus_step[num_vars], var_minus_step[num_vars], dists[num_vars], plus_dist[num_vars], minus_dist[num_vars];
	
	for (i = 0; i < num_vars; ++i) { //take the derivative with respect to each variable
		for (j = 0; j < num_vars; ++j) { //perturb the variable
			var_plus_step[j] = vars[j];
			var_minus_step[j] = vars[j];
		}
		var_plus_step[i] += step;
		var_minus_step[i] -= step;
		dist_func(a, var_plus_step, dists, n);
		for (j = 0; j < num_vars; ++j)
			plus_dist[j] = dists[j];
		dist_func(a, var_minus_step, dists, n);
		for (j = 0; j < num_vars; ++j)
			minus_dist[j] = dists[j];
		for (j = 0; j < num_vars; ++j)
			grad[j][i] = (plus_dist[j] - minus_dist[j])/(2*step);
	}
	
}

/*
Description: This function creates a set of unit distance equations, given an n x n adjacency matrix.

Note that without loss of generality, we set particle 1 (i.e. index 0) to lie at the origin (i.e. x=y=z=0), particle 2 (index 1) to be confined to the y axis (i.e. x=z=0), and particle 3 (index 2) to be confined to the xy-plane (i.e. z=0).

Also note that the coordinates are stored in a 3n-6 x 1 vector -- where entries 0 to n-3 correspond to the x coordinates of particles 3 to n (or 2 to n-1, if 0 based), entries n-2 to 2n-4 correspond to the y coordinates of particles 2 to n (or 1 to n-1, if use 0 based particle numbers), and entries 2n-3 to the end correspond to the z coordinates of particles 4 to n (or 3 to n-1, if use 0 based particle numbers).  Therefore, if use 0 based particle labeling, for particle i, the index of its x coordinate will be i-2, the index of its y coordinate will be given by n-2+i-1, and the index of its z coordinate will be given by 2n-3+i-3 (if use 1 based particle labeling, then the indices will be given by i-3, n-2+i-2, and 2n-3+i-4, respectively).
*/
void dist_func(int a[][MAXLEN], double coords[], double *dists, int n) {
	
	/*Declare and initialize local variables*/
	int i, j, k, count, found;
	count = 0;
	found = 0;
	
	for (i = 0; i < n; ++i) { //search through adjacency matrix to find which particles touch eachother - if particles touch, calculate the residual from the unit distance
		for (j = i+1; j < n; ++j) {
			if (a[i][j] == 1) {
				if (i == 0) {
					if (j == 1)
						dists[count] = pow(coords[n-2],2) - 1;
					else if (j == 2)
						dists[count] = pow(coords[0],2) + pow(coords[n-2+1],2) - 1;
					else
						dists[count] = pow(coords[j-2],2) + pow(coords[n-2+j-1],2) + pow(coords[2*n-3+j-3],2) - 1;
				}
				else if (i == 1) {
					if (j == 2)
						dists[count] = pow(-coords[0],2) + pow(coords[n-2] - coords[n-2+1],2) - 1;
					else
						dists[count] = pow(-coords[j-2],2) + pow(coords[n-2] - coords[n-2+j-1],2) + pow(-coords[2*n-3+j-3],2) - 1;
				}
				else if (i == 2)
					dists[count] = pow(coords[0] - coords[j-2],2) + pow(coords[n-2+1] - coords[n-2+j-1],2) + pow(-coords[2*n-3+j-3],2) - 1;
				else
					dists[count] = pow(coords[i-2] - coords[j-2],2) + pow(coords[n-2+i-1] - coords[n-2+j-1],2) + pow(coords[2*n-3+i-3] - coords[2*n-3+j-3],2) - 1;
				++count;
			}
		}
	}
	
}

/*
Description:

Inputs: 
a is the m x n matrix that will be decomposed into LU form.
m is the number of rows a has.
n is the number of columns a has.

Output:
*/
void ludcmp(double (*a)[MAXLEN], int m, int n, int *pivot, double (*L)[MAXLEN], double (*U)[MAXLEN], int (*perm)[MAXLEN]) {
	
	/*Declare and initialize local variables*/
	int i, j, k, piv;
	double temp, maxval;
	
	for (i = 0; i < m; ++i)
		pivot[i] = i; //initialize row-pivot indices
		
	for (k = 0; k < m; ++k) { //loop over each column
		piv = k;
		if (k > n) //this is needed for non-square matrices
			break;
		maxval = fabs(a[k][k]);
		for (i = k + 1; i < m; ++i) { //scan the lower triangular part of this column, and record the row with the largest value
			if ((temp = fabs(a[i][k])) > maxval) {
				maxval = temp;
				piv = i;
			}
		}
		if (piv != k) { //swap rows if max value is not in row k
			for (i = 0; i < n; ++i) {
				temp = a[piv][i];
				a[piv][i] = a[k][i];
				a[k][i] = temp;
			}
			temp = pivot[piv]; //swap pivot row indices
			pivot[piv] = pivot[k];
			pivot[k] = temp;
		}
		if (a[k][k] == 0)
			fprintf(stderr,"\nInput matrix is singular. k = %d.\n",k);
		for (i = k + 1; i < m; ++i) //Do column reduction now
			a[i][k] *= 1.0/a[k][k]; //divide lower triangular part of column by max
		for (j = k + 1; j < n; ++j) { //subtract multiple of column from remaining columns
			for (i = k + 1; i < m; ++i)
				a[i][j] -= a[i][k]*a[k][j];
		}
	}
	
	breakupLU(a, m, n, L, U);
	get_perm_mat(pivot, n, perm);

}

/*Description: break up the matrix that has the LU form of a in one matrix, to 2 matrices, L, and U (as in ch 2.3 of numerical recipes in C).*/
void breakupLU(double (*a)[MAXLEN], int m, int n, double (*L)[MAXLEN], double (*U)[MAXLEN]) {

	/*Declare and initialize local variables*/
	int i, j;
	for (i = 0; i < m; ++i) {
		for (j = 0; j < n; ++j) {
			L[i][j] = 0;
			U[i][j] = 0;
		}
	}

	for (i = 0; i < m; ++i) {
		for (j = 0; j < n; ++j) {
			if (i > j) //fill up the upper diagonal matrix
				L[i][j] = a[i][j];
			else if (i == j) {
				L[i][j] = 1;
				U[i][j] = a[i][j];
			}
			else //if i < j - fill up the lower diagonal matrix
				U[i][j] = a[i][j];
		}
	}

}

/*Description: Turn the pivot vector into a permutation matrix.*/
void get_perm_mat(int *pivot, int n, int (*perm)[MAXLEN]) {

	/*Declare and initialize local variables*/
	int i, j;
	for (i = 0; i < n; ++i) {
		for (j = 0; j < n; ++j) {
			perm[i][j] = 0;
		}
	}
	
	for (i = 0; i < n; ++i)
		perm[i][pivot[i]] = 1;

}

/*Description: Computes the inverse of the n x n matrix a.  The inverse of matrix a is written to z.*/
void inv(double (*a)[MAXLEN], int n, double (*z)[MAXLEN]) { //this assumes a square matrix

	/*Declare and initialize local variables*/
	int i, j, k, det;
	int pivot[MAXLEN], perm[MAXLEN][MAXLEN];
	double y[n][n], col[MAXLEN], permd[MAXLEN][MAXLEN], L[MAXLEN][MAXLEN], U[MAXLEN][MAXLEN], temp[MAXLEN][MAXLEN], temp2[MAXLEN];
	for (i = 0; i < n; ++i) {
		for (j = 0; j < n; ++j)
			y[i][j] = 0;
	}
	
	if (n == 1)
		z[0][0] = 1/a[0][0];
	else if (n == 2) {
		det = a[0][0]*a[1][1] - a[1][0]*a[0][1];
		z[0][0] = a[1][1]/det;
		z[0][1] = -a[0][1]/det;
		z[1][0] = -a[1][0]/det;
		z[1][1] = a[0][0]/det;
	}
	else {
		ludcmp(a,n,n,pivot,L,U,perm);
		for (j = 0; j < n; ++j) {
			for (i = 0; i < n; ++i) //the following 2 lines create a vector of all 0's, except for a 1 in row j
				col[i] = 0;
			col[j] = 1;
			for (i = 0; i < n; ++i) { //write L to a temporary array, so that L doens't get changed in the call to forward_sub
				temp2[i] = col[i]; //write col to a temporary array also, as for some reason it wasn't being fed in correctl when I was feeding in col directly, this seems to have fixed it...
				for (k = 0; k < n; ++k)
					temp[i][k] = L[i][k];
			}
			forward_sub(temp,n,temp2,col);
			for (i = 0; i < n; ++i) { //write U to a temporary array, so that U doens't get changed in the call to back_sub
				temp2[i] = col[i];
				for (k = 0; k < n; ++k)
					temp[i][k] = U[i][k];
			}
			back_sub(temp,n,temp2,col);
			for (i = 0; i < n; ++i)
				y[i][j] = col[i];
		}
		for (i = 0; i < n; ++i) { //convert the perm array, which is an array of ints, to an array of doubles, as that is the expected input for mat_mult
			for (j = 0; j < n; ++j)
				permd[i][j] = (double)perm[i][j];
		}
		for (i = 0; i < n; ++i) { //this is also not feeding in correctly for some reason, if I just put in y, so I'm writing it to a temp array here...
			for (k = 0; k < n; ++k)
				temp[i][k] = y[i][k];
		}
		mat_mult(temp,n,n,permd,n,n,z);
	}

}

/*Description: perform a forward substitution - of the type in ch 2.3 of numerical recipes in C.  The vector x is updated by this function, thus this is what the function returns.*/
void forward_sub(double L[][MAXLEN], int n, double b[], double *x) {
	
	/*Declare and initialize local variables*/
	int i, j;
	double sum;
	for (i = 0; i < n; ++i)
		x[i] = 0;
	
	for (i = 0; i < n; ++i) {
		sum = b[i];
		for (j = 0; j < i; ++j)
			sum -= L[i][j]*x[j];
		x[i] = sum/L[i][i];
	}
	
}

/*Description: perform a backward substitution - of the type in ch 2.3 of numerical recipes in C.  The vector x is updated by this function, thus this is what the function returns.*/
void back_sub(double U[][MAXLEN], int n, double b[], double *x) {
	
	/*Declare and initialize local variables*/
	int i, j;
	double sum;
	for (i = 0; i < n; ++i)
		x[i] = 0;
		
	for (i = n-1; i >= 0; --i) {
		sum = b[i];
		for (j = i+1; j < n; ++j)
			sum -= U[i][j]*x[j];
		x[i] = sum/U[i][i];
	}
	
}

/*
Descriptoin: perform the matrix multiplication x*y.

Inputs:
x is the first n1 x m1 matrix.
n1 is the number of rows in x.
m1 is the number of columns in x.
y is the 2nd matrix, it's dimensions are n2 x m2.
n2 is the number of rows in y.
m2 is the number of columns in y.
z is the resulting product of the 2 matrices.
*/
void mat_mult(double x[][MAXLEN], const int n1, const int m1, double y[][MAXLEN], const int n2, const int m2, double (*z)[MAXLEN]) {
	
	/*Declare and initialize local variables*/
	int i, j, k, q, n;
	
	if (m1 != n2)
		fprintf(stderr,"\nError in matrix multiplication: dimensions of matrices don't agree.\n");
	
	for (i = 0; i < n1; ++i) { //loop through rows of x - this determines how many rows z will have
		for (j = 0; j < m2; ++j) { //loop through columns of y - this determines how many columns z will have
			z[i][j] = 0; //initialize
			for (k = 0; k < m1; ++k) //Perform the matrix multiplication that will yield each element of z, i.e. each z[i][j] - first loop through the columns of row i of matrix x, then loop through the rows of column j of matrix y, and mutliply and sum to get the element z[i][j]
				z[i][j] += x[i][k]*y[k][j];
		}
	}
	
}

/* Description: This function returns the relative distances for a set of 3n-6 coordinates (fed in the format of the previous function).*/
void rel_dists_func(double coords[], double *rel_dists, int n) {
	
	/*Declare and initialize local variables*/
	int i, j, count;
	count = 0;
	
	for (i = 0; i < n; ++i) { //search through adjacency matrix to find which particles touch eachother - if particles touch, calculate the residual from the unit distance
		for (j = i+1; j < n; ++j) {
			if (i == 0) {
				if (j == 1)
					rel_dists[count] = sqrt(pow(coords[n-2],2));
				else if (j == 2)
					rel_dists[count] = sqrt(pow(coords[0],2) + pow(coords[n-2+1],2));
				else
					rel_dists[count] = sqrt(pow(coords[j-2],2) + pow(coords[n-2+j-1],2) + pow(coords[2*n-3+j-3],2));
			}
			else if (i == 1) {
				if (j == 2)
					rel_dists[count] = sqrt(pow(-coords[0],2) + pow(coords[n-2] - coords[n-2+1],2));
				else
					rel_dists[count] = sqrt(pow(-coords[j-2],2) + pow(coords[n-2] - coords[n-2+j-1],2) + pow(-coords[2*n-3+j-3],2));
			}
			else if (i == 2)
				rel_dists[count] = sqrt(pow(coords[0] - coords[j-2],2) + pow(coords[n-2+1] - coords[n-2+j-1],2) + pow(-coords[2*n-3+j-3],2));
			else
				rel_dists[count] = sqrt(pow(coords[i-2] - coords[j-2],2) + pow(coords[n-2+i-1] - coords[n-2+j-1],2) + pow(coords[2*n-3+i-3] - coords[2*n-3+j-3],2));
			++count;
		}
	}
	
}

/* Description: This function calculates the 2nd moment of a conformation of n particles, given a 3n-6 coordinate vector (same coordinate format as with previous functions).*/
double second_moment(double coords[], int n) {

	/*Declare and initialize local variables*/
	int i;
	double c_x, c_y, c_z, M;
	c_x = 0;
	c_y = 0;
	c_z = 0;
	M = 0;
	
	/*Calculate the x, y, and z locations of the centroid (i.e. the average of all x, y, and z positions).*/
	for (i = 0; i < n-2; ++i)
		c_x += coords[i]/n;
	for (i = n-2; i < 2*n-3; ++i)
		c_y += coords[i]/n;
	for (i = 2*n-3; i < 3*n-6; ++i)
		c_z += coords[i]/n;
	
	/*Calculate the 2nd momend, which is the sum of each particle's distance from the centroid.*/
	M += pow(-c_x,2) + pow(-c_y,2) + pow(-c_z,2); //the first particle's distance from the centroid (the first particle is defined to be at the origin).
	M += pow(-c_x,2) + pow(coords[n-2] - c_y,2) + pow(-c_z,2); //the 2nd particle's distance from the centroid (the 2nd particle resides on the y axis, i.e. x_2 = z_2 = 0).
	if (n > 2) {
		M += pow(coords[0] - c_x,2) + pow(coords[n-2+1] - c_y,2) + pow(-c_z,2); //the 3rd particle's distance from the centroid (the 3rd particle resides on the xy-plane, i.e. z_3 = 0).
		for (i = 3; i < n; ++i) { //for particle 4 and higher, x, y, and z coordinates are free to move, and so we just use the standard formula (as this is a 0 based system, particle 4 corresponds to index 3)
			M += pow(coords[i-2] - c_x,2) + pow(coords[n-2+i-1] - c_y,2) + pow(coords[2*n-3+i-3] - c_z,2);
		}
	}
	
	return M;

}