int count1 = 0, count2 = 0, count3 = 0, count_tmp = 0;

double Average_Displacement;

double Current_con_factor = 1.0;

double E_grad_dir = 0;

double alpha = 0;
const double alpha_init = 10;
double rho = .9;
double c = .1;

int t_count = 0;

double midline_wall = (Vx[Nv-1] - Vx[0])/2.0;
double Apical_area_initial = Cell_Apical[0];
//cout << "Vx[Nv-1] = " << Vx[Nv-1] << " Vx[0] = " << Vx[0] << " and midline is " << midline_wall << endl;

for (i=0; i<Nv; i++) {
	Vx_nxt[i] = Vx[i];
	Vy_nxt[i] = Vy[i];
	Vz_nxt[i] = Vz[i];
}

/*
 #include "load_init.h"

for (i=0;i<Nv;i++){
	cout << "V_before(" << i << ") = [" << Vx_nxt[i] << ", " << Vy_nxt[i] << ", " << Vz_nxt[i] << "]" << endl;
	cout << "V_after(" << i << ") = [" << Vx[i] << ", " << Vy[i] << ", " << Vz[i] << "]" << endl << endl;
	}
 */

int* V_side = new int[Nv]; 

for (i=0; i<Nv; i++){
	
	if (Vx[i] < midline_wall) {
		V_side[i] = -1;
	}
	else if (Vx[i] > midline_wall) {
		V_side[i] = 1;
	}
	else {
		V_side[i] = 0;
	}
	//cout << "Vx = " << Vx[i] << " V_side = " << V_side[i] << endl;
}



	t = 0; t_count = 0;


// Declare flags;
int flag_start = 0, flag_end = 0, flag_exit = 0;

// Declare iostream files for flags and intermediate variables
ifstream file_flag_read; ofstream file_flag_write;
ifstream file_inter_read; ofstream file_inter_write;

flag_start = 0;
file_flag_write.open("flag2_start.m", ios::out);
file_flag_write << flag_start << endl;
file_flag_write.close();

while (1) {

//*******************************************************************
// Slave proceduce 1:
// Export intermediate verticies 6 to slave 3
// Update Current_con_factor
// Then set the start flag back to 0
	cout << "waiting for procedure 1 to start ..." << endl;

while (flag_start == 0) {
	file_flag_read.open("flag2_start.m", ios::in);
	file_flag_read >> flag_start;
	file_flag_read.close();
	sleep(.1);
	//cout << "waiting for flag2_start ..." << endl;
	}
	
	file_flag_read.open("Exit_flag.m", ios::in);
	file_flag_read >> flag_exit;
	file_flag_read.close();
	
	cout << "Procedure 1 started" << endl;
	
	if (flag_exit == 1) break;
	
	flag_start = 0;
	file_flag_write.open("flag2_start.m", ios::out);
	file_flag_write << flag_start << endl;
	file_flag_write.close();
	
	// Update Current_con_factor
	file_inter_read.open("con_factor.m", ios::in);
	file_inter_read >> Current_con_factor;
	file_inter_read.close();
	
	cout << "Current con factor is now " << Current_con_factor << endl;
	
	for (j=0; j<(Ny-1); j++) {
		for (i=0; i<(Nx-1); i++) {
			id_tmp = Cell_Id(i, j, Nx);
			if ((i>= Constrict_start) && (i<= Constrict_end) && (j >= y_start) && (j <= y_stop)) {
				A_0_cell[id_tmp] = Current_con_factor*Apical_area_initial;
			}
			
		}
	}
	
	// Export intermediate verticies 6 for slave 3	
	file_inter_write.open("V_inter_6.m", ios::out);
	for (i=0; i<Nv; i++){
		if (V_slave_id[i] != 6) continue;
		file_inter_write << Vx[i];
		file_inter_write << Vy[i];
		file_inter_write << Vz[i];
	}
	file_inter_write.close();
	
	// Set end flag to 1
	flag_end = 1;
	file_flag_write.open("flag2_end.m", ios::out);
	file_flag_write << flag_end << endl;
	file_flag_write.close();
//*******************************************************************	
	// Slave proceduce 2:
	// Import intermediate verticies 5 from slave 1 and calculate the gradient
	// Export half of the gradient for interm 5 to slave 1
	// Export half of the gradient for interm 6 to slave 3
	// Then set the start flag back to 0
	
	cout << "waiting for procedure 2 to start ..." << endl;
	while (flag_start == 0) {
		file_flag_read.open("flag2_start.m", ios::in);
		file_flag_read >> flag_start;
		file_flag_read.close();
		sleep(.1);
		//cout << "waiting for flag2_start ..." << endl;
	}
	
	flag_start = 0;
	file_flag_write.open("flag2_start.m", ios::out);
	file_flag_write << flag_start << endl;
	file_flag_write.close();
	cout << "Procedure 2 started" << endl;
	
	// Import intermediate verticies 5 from slave 1
	file_inter_read.open("V_inter_5.m", ios::in);
	for (i=0; i<Nv; i++){
		if (V_slave_id[i] != 5) continue;
		file_inter_read >> Vx[i];
		file_inter_read >> Vy[i];
		file_inter_read >> Vz[i];
	}
	file_inter_read.close();
	
	// The action	
	#include "Gradient_Calc_slave.h"
	// Export interm vert gradient 5 to slave 1
	file_inter_write.open("V_grad_5_2.m", ios::out);
	for (i=0; i<Nv; i++){
		if (V_slave_id[i] == 5) {
			file_inter_write << V_grad[i][0] << endl << V_grad[i][1] << endl << V_grad[i][2] << endl;
		}
	}
	file_inter_write.close();
	
	// Export interm vert gradient 6 to slave 3
	file_inter_write.open("V_grad_6_2.m", ios::out);
	for (i=0; i<Nv; i++){
		if (V_slave_id[i] == 6) {
			file_inter_write << V_grad[i][0] << endl << V_grad[i][1] << endl << V_grad[i][2] << endl;
		}
	}
	file_inter_write.close();
	
	// Set end flag to 1
	flag_end = 1;
	file_flag_write.open("flag2_end.m", ios::out);
	file_flag_write << flag_end << endl;
	file_flag_write.close();
//*******************************************************************
	// Slave proceduce 3:
	// Import interm vert 5 grad from slave 1
	// Import interm vert 6 grad from slave 3
	// Calculate the maximum magnitude of the gradient
	// Then set the start flag back to 0
	
	cout << "waiting for procedure 3 to start ..." << endl;
	while (flag_start == 0) {
		file_flag_read.open("flag2_start.m", ios::in);
		file_flag_read >> flag_start;
		file_flag_read.close();
		sleep(.1);
		//cout << "waiting for flag2_start ..." << endl;
	}
	
	flag_start = 0;
	file_flag_write.open("flag2_start.m", ios::out);
	file_flag_write << flag_start << endl;
	file_flag_write.close();
	cout << "Procedure 3 started" << endl;
	
	// The action	
	// Import interm vert gradient 5 from slave 1
	file_inter_read.open("V_grad_5_1.m", ios::in);
	for (i=0; i<Nv; i++){
		if (V_slave_id[i] != 5) continue;
		file_inter_read >> V_grad_tmp; V_grad[i][0] += V_grad_tmp; 
		file_inter_read >> V_grad_tmp; V_grad[i][1] += V_grad_tmp;
		file_inter_read >> V_grad_tmp; V_grad[i][2] += V_grad_tmp;
	}
	file_inter_read.close();
	
	// Import interm vert gradient 6 from slave 3
	file_inter_read.open("V_grad_6_3.m", ios::in);
	for (i=0; i<Nv; i++){
		if (V_slave_id[i] != 6) continue;
		file_inter_read >> V_grad_tmp; V_grad[i][0] += V_grad_tmp; 
		file_inter_read >> V_grad_tmp; V_grad[i][1] += V_grad_tmp;
		file_inter_read >> V_grad_tmp; V_grad[i][2] += V_grad_tmp;
	}
	file_inter_read.close();
	
	// Record the maximum 
	V_grad_mag = 0;
	for (i=0; i<Nv; i++) {// Calculate the maximum value of the step
		if (V_slave_id[i] != SLAVE_ID && V_slave_id[i] != 6) continue;
		V_grad_tmp = sqrt(V_grad[i][0]*V_grad[i][0] + V_grad[i][1]*V_grad[i][1] + V_grad[i][2]*V_grad[i][2]);
		if (V_grad_tmp > V_grad_mag) {
			V_grad_mag = V_grad_tmp;
		}
	}
	file_inter_write.open("V_mag_2.m", ios::out);
	file_inter_write << V_grad_mag << endl;
	file_inter_write.close();	
	
	// Set end flag to 1
	flag_end = 1;
	file_flag_write.open("flag2_end.m", ios::out);
	file_flag_write << flag_end << endl;
	file_flag_write.close();
//*******************************************************************
	// Slave proceduce 4:
	// Rescale the gradient and export interm grad 6 to slave 3
	// Calculate E_old and export E_old to the master program
	// Then set the start flag back to 0
	
	while (flag_start == 0) {
		file_flag_read.open("flag2_start.m", ios::in);
		file_flag_read >> flag_start;
		file_flag_read.close();
		sleep(.1);
		//cout << "waiting for flag2_start ..." << endl;
	}
	
	flag_start = 0;
	file_flag_write.open("flag2_start.m", ios::out);
	file_flag_write << flag_start << endl;
	file_flag_write.close();
	cout << "Procedure 4 started" << endl;
	
	// The action	
	file_inter_read.open("V_mag.m", ios::in);
	file_inter_read >> V_grad_mag;
	file_inter_read.close();
	
	for (i=0; i<Nv; i++) { // Rescale the gradient
		if (V_slave_id[i] != SLAVE_ID && V_slave_id[i] != 6) continue;
		V_grad[i][0] *= dx/V_grad_mag;
		V_grad[i][1] *= dx/V_grad_mag;
		V_grad[i][2] *= dx/V_grad_mag;
	}
	
	E_old = 0;
	
	// Energy from cells
	for (i=0; i<Nc; i++) {
		// Get the apical verticies
		if (C_slave_id[i] != SLAVE_ID) continue;
		
		for (k=0; k<4; k++) {
			AP[k][0] = Vx[C_apver[i][k]]; AP[k][1] = Vy[C_apver[i][k]]; AP[k][2] = Vz[C_apver[i][k]];
			BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
		}
		K_par[0] = A_K_cell[i]; K_par[1] = B_K_cell[i]; K_par[2] = V_K_cell[i];
		K_par[3] = L_K_cell[i]*C_side_factor[i][0]; K_par[4] = L_K_cell[i]*C_side_factor[i][1];
		K_par[5] = L_K_cell[i]*C_side_factor[i][2]; K_par[6] = L_K_cell[i]*C_side_factor[i][3];
		K_par[7] = D_K_cell[i];
		
		O_par[0] = A_0_cell[i]; O_par[1] = B_0_cell[i]; O_par[2] = V_0_cell[i];
		O_par[3] = L_0_cell[i]; O_par[4] = L_0_cell[i]; O_par[5] = L_0_cell[i];
		O_par[6] = L_0_cell[i]; O_par[7] = D_0_cell[i];
		E_old += E_from_cell_V2_diag(AP, BS, K_par, O_par);
	}
	
	// Energy from Plane constraint
	if (plane_constraint_present == 1){
		for (i=0; i<Nv; i++) {
			for (j=0; j<24; j++) {
				
				if (V_slave_id[Plane_test_p1[i][j]] != SLAVE_ID && V_slave_id[Plane_test_p2[i][j]] != SLAVE_ID && V_slave_id[Plane_test_p3[i][j]] != SLAVE_ID) 
					continue;
				
				if ((Plane_test_p1[i][j] < 0) || (Plane_test_p2[i][j] < 0) || (Plane_test_p3[i][j] < 0))
					continue;
				
				Point1[0] = Vx[Plane_test_p1[i][j]];Point1[1] = Vy[Plane_test_p1[i][j]];Point1[2] = Vz[Plane_test_p1[i][j]];
				Point2[0] = Vx[Plane_test_p2[i][j]];Point2[1] = Vy[Plane_test_p2[i][j]];Point2[2] = Vz[Plane_test_p2[i][j]];
				Point3[0] = Vx[Plane_test_p3[i][j]];Point3[1] = Vy[Plane_test_p3[i][j]];Point3[2] = Vz[Plane_test_p3[i][j]];
				
				// Pertubation for x
				Point0[0] = Vx[i];Point0[1] = Vy[i];Point0[2] = Vz[i];				
				Dis_tmp = Dis_to_plane(Point1, Point2, Point3, Point0);
				if (Dis_tmp < R_max) {
					E_old += Plane_potential[int(Dis_tmp/dx_plane)];
				}
				
				
			}
		}
	}
	
	file_inter_write.open("Energy_2.m", ios::out);
	file_inter_write << E_old << endl;
	file_inter_write.close();
	
	
	// Set end flag to 1
	flag_end = 1;
	file_flag_write.open("flag2_end.m", ios::out);
	file_flag_write << flag_end << endl;
	file_flag_write.close();
//*******************************************************************
// At this point we know the position and the gradient of the 
// verticies for all the cells used by this slave

// Slave proceduce 5:
// Loop and change alpha based on the master program

alpha = alpha_init;
for (i=0; i<Nv; i++) {
	Vx_nxt[i] = Vx[i];
	Vy_nxt[i] = Vy[i];
	Vz_nxt[i] = Vz[i];
}

// Loop until flag_start is not -1	
while (1) {		
	while (flag_start == 0) {
		file_flag_read.open("flag2_start.m", ios::in);
		file_flag_read >> flag_start;
		file_flag_read.close();
		sleep(.1);
		//cout << "waiting for flag2_start ..." << endl;
	}
	
	if (flag_start == -1) {
		flag_start = 0;
		file_flag_write.open("flag2_start.m", ios::out);
		file_flag_write << flag_start << endl;
		file_flag_write.close();
		break;
	}
	
	flag_start = 0;
	file_flag_write.open("flag2_start.m", ios::out);
	file_flag_write << flag_start << endl;
	file_flag_write.close();
	
	// Revert to the original position
	for (i=0; i<Nv; i++) {
		Vx[i] = Vx_nxt[i];
		Vy[i] = Vy_nxt[i];
		Vz[i] = Vz_nxt[i];
	}
	
	// Move the verticies
	// Note that you are doing x_k = x_k + alpha*p_k, after the appropriate BC of course
	// Move Apical
	for (i=0; i<(Nv/2); i++) {
		if (V_slave_id[i] != SLAVE_ID && V_slave_id[i] != INTERM_ID1 && V_slave_id[i] != INTERM_ID2) continue;
		Vx[i] += alpha*V_grad[i][0]*V_not_move_x[i];//*V_not_move[i];
		Vy[i] += alpha*V_grad[i][1]*V_not_move_y[i];
		Vz[i] += alpha*V_grad[i][2]*V_not_move_z[i];//*V_not_move[i];
	}
	
	// Move Basal
	for (i=Nv/2; i<Nv; i++) {
		if (V_slave_id[i] != SLAVE_ID && V_slave_id[i] != INTERM_ID1 && V_slave_id[i] != INTERM_ID2) continue;
		Vx[i] += alpha*V_grad[i][0]*V_not_move_x[i]*Basal_friction;//*V_not_move[i];
		Vy[i] += alpha*V_grad[i][1]*V_not_move_y[i]*Basal_friction;
		Vz[i] += alpha*V_grad[i][2]*V_not_move_z[i]*Basal_friction;//*V_not_move[i];
	}
	
	// Apply boundary conditions 
	if (fixed_top == 1) {
		if (Sheet_condition == 2) {
			for (i=0; i<Nv; i++) {
				if (Vz[i] > Z_spacing) {
					Vz[i] = Z_spacing;
				}
			}
		}
		else if (Sheet_condition == 1){
			for (i=0; i<Nv; i++) {
				//R_tmp = sqrt(Vx[i]*Vx[i] + Vy[i]*Vy[i] + Vz[i]*Vz[i]);
				R_tmp = sqrt(Vx[i]*Vx[i] + Vz[i]*Vz[i]);
				if (R_tmp > r_out) {
					
					Vx[i] = Vx[i]*r_out/R_tmp;
					Vz[i] = Vz[i]*r_out/R_tmp;
				}
			}
		}
	}
	
	// Apply midline conditions
	if (midline_wall_flag == 1) {
		for (i=0; i<Nv; i++){
			if (V_side[i] == -1 && Vx[i] > midline_wall) {
				Vx[i] = midline_wall;
			}
			else if (V_side[i] == 1 && Vx[i] < midline_wall) {
				Vx[i] = midline_wall;
			}
		}
	}
	
	// Calculate E_grad_dir = grad(E)_k*alpha*p_k = -grad(E)_k*grad(E)_k
	E_grad_dir = 0;
	for (i=0; i<Nv; i++) {
		if (V_slave_id[i] != SLAVE_ID && V_slave_id[i] != INTERM_ID1 && V_slave_id[i] != INTERM_ID2) continue;
		E_grad_dir += -(V_grad[i][1]*(Vx[i] - Vx_prv[i]) + V_grad[i][2]*(Vy[i] - Vy_prv[i]) + V_grad[i][3]*(Vz[i] - Vz_prv[i]));
	}
	
	// Calculate the E_new = E(x_k + alpha*p_k)
	E_new = 0;
	for (i=0; i<Nc; i++) {
		if (C_slave_id[i] != SLAVE_ID) continue;
		for (k=0; k<4; k++) {
			AP[k][0] = Vx[C_apver[i][k]]; AP[k][1] = Vy[C_apver[i][k]]; AP[k][2] = Vz[C_apver[i][k]];
			BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
		}
		K_par[0] = A_K_cell[i]; K_par[1] = B_K_cell[i]; K_par[2] = V_K_cell[i];
		K_par[3] = L_K_cell[i]*C_side_factor[i][0]; K_par[4] = L_K_cell[i]*C_side_factor[i][1];
		K_par[5] = L_K_cell[i]*C_side_factor[i][2]; K_par[6] = L_K_cell[i]*C_side_factor[i][3];
		K_par[7] = D_K_cell[i];
		
		O_par[0] = A_0_cell[i]; O_par[1] = B_0_cell[i]; O_par[2] = V_0_cell[i];
		O_par[3] = L_0_cell[i]; O_par[4] = L_0_cell[i]; O_par[5] = L_0_cell[i];
		O_par[6] = L_0_cell[i]; O_par[7] = D_0_cell[i];
		E_new += E_from_cell_V2_diag(AP, BS, K_par, O_par);
	}
	
	// Energy from Plane constraint
	if (plane_constraint_present == 1){
		for (i=0; i<Nv; i++) {
			for (j=0; j<24; j++) {
				
				if (V_slave_id[Plane_test_p1[i][j]] != SLAVE_ID && V_slave_id[Plane_test_p2[i][j]] != SLAVE_ID && V_slave_id[Plane_test_p3[i][j]] != SLAVE_ID) 
					continue;
				
				if ((Plane_test_p1[i][j] < 0) || (Plane_test_p2[i][j] < 0) || (Plane_test_p3[i][j] < 0))
					continue;
				
				Point1[0] = Vx[Plane_test_p1[i][j]];Point1[1] = Vy[Plane_test_p1[i][j]];Point1[2] = Vz[Plane_test_p1[i][j]];
				Point2[0] = Vx[Plane_test_p2[i][j]];Point2[1] = Vy[Plane_test_p2[i][j]];Point2[2] = Vz[Plane_test_p2[i][j]];
				Point3[0] = Vx[Plane_test_p3[i][j]];Point3[1] = Vy[Plane_test_p3[i][j]];Point3[2] = Vz[Plane_test_p3[i][j]];
				
				// Pertubation for x
				Point0[0] = Vx[i];Point0[1] = Vy[i];Point0[2] = Vz[i];				
				Dis_tmp = Dis_to_plane(Point1, Point2, Point3, Point0);
				if (Dis_tmp < R_max) {
					E_new += Plane_potential[int(Dis_tmp/dx_plane)];
				}
			}
		}
	}
	
	// Write E_grad_dir*c and E_new to file
	file_inter_write.open("Energy_2.m", ios::out);
	file_inter_write << E_new << endl << E_grad_dir*c << endl;
	file_inter_write.close();
	
	alpha *= rho;
	
	// Set end flag to 1
	flag_end = 1;
	file_flag_write.open("flag2_end.m", ios::out);
	file_flag_write << flag_end << endl;
	file_flag_write.close();
	
}
//*******************************************************************

}
	
// Now V_grad was only calculated for those verticies 
// for which V_slave_id = SLAVE_ID

return 0;



/*
while (1) {
	
	if (Exit_flag == 0 && Change_A_flag == 1) {
		break;
	}
	
	// Isotropically decrease A0
	if (Change_A_flag == 1){
		if ((Current_con_factor - A_change_amount) >= A_con_factor){
			Current_con_factor -= A_change_amount;
			for (j=0; j<(Ny-1); j++) {
				for (i=0; i<(Nx-1); i++) {
					id_tmp = Cell_Id(i, j, Nx);
					
					// Assing which cells will constrict
					if ((i>= Constrict_start) && (i<= Constrict_end) && (j >= y_start) && (j <= y_stop)) {
						A_0_cell[id_tmp] = Current_con_factor*Apical_area_initial;
					}
					
				}
			}
		}
		else {
			Exit_flag = 0;
		}

		cout << endl <<  "The Apical cells are now constricting to " << Current_con_factor << " of their original lengths" << endl;
		Change_A_flag = 0;
	}
	
	
	
	// Calculate initial yolk volumes
	Yolk_V = 0;
	for (i=0; i<Nc; i++) {
		
		for (k=0; k<4; k++) {
			BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
			AP[k][0] = Yolk_x[C_ykver[i][k]]; AP[k][1] = Yolk_y[C_ykver[i][k]]; AP[k][2] = Yolk_z[C_ykver[i][k]];
		}
		
		Yolk_volumes[i] = Volume_Calc(AP, BS);
		Yolk_V += Yolk_volumes[i];
	}
	
	//cout << "Yolk Volume is " << Yolk_V << endl;
	
	// Store the previous and the next vectors
	for (i=0; i<Nv; i++) {
		Vx_nxt[i] = Vx[i];
		Vy_nxt[i] = Vy[i];
		Vz_nxt[i] = Vz[i];
		
		Vx_prv[i] = Vx[i];
		Vy_prv[i] = Vy[i];
		Vz_prv[i] = Vz[i];
	}
	
	
	
	// Loop over all verticies and calculate the gradient
	#include "Gradient_Calc.h"
	alpha = alpha_init;
	//*
	// Scale the gradient so that the maximum step would be equal to dx
	V_grad_mag = 0;
	for (i=0; i<Nv; i++) {// Calculate the maximum value of the step
		V_grad_tmp = sqrt(V_grad[i][0]*V_grad[i][0] + V_grad[i][1]*V_grad[i][1] + V_grad[i][2]*V_grad[i][2]);
		if (V_grad_tmp > V_grad_mag) {
			V_grad_mag = V_grad_tmp;
		}
	}
	
	for (i=0; i<Nv; i++) { // Rescale the gradient
		V_grad[i][0] *= dx/V_grad_mag;
		V_grad[i][1] *= dx/V_grad_mag;
		V_grad[i][2] *= dx/V_grad_mag;
	}
	
	// Calculate the energy
	E_old = 0;
	// Energy from cells
	for (i=0; i<Nc; i++) {
		// Get the apical verticies
		for (k=0; k<4; k++) {
			AP[k][0] = Vx[C_apver[i][k]]; AP[k][1] = Vy[C_apver[i][k]]; AP[k][2] = Vz[C_apver[i][k]];
			BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
		}
		K_par[0] = A_K_cell[i]; K_par[1] = B_K_cell[i]; K_par[2] = V_K_cell[i];
		K_par[3] = L_K_cell[i]*C_side_factor[i][0]; K_par[4] = L_K_cell[i]*C_side_factor[i][1];
		K_par[5] = L_K_cell[i]*C_side_factor[i][2]; K_par[6] = L_K_cell[i]*C_side_factor[i][3];
		K_par[7] = D_K_cell[i];
		
		O_par[0] = A_0_cell[i]; O_par[1] = B_0_cell[i]; O_par[2] = V_0_cell[i];
		O_par[3] = L_0_cell[i]; O_par[4] = L_0_cell[i]; O_par[5] = L_0_cell[i];
		O_par[6] = L_0_cell[i]; O_par[7] = D_0_cell[i];
		E_old += E_from_cell_V2_diag(AP, BS, K_par, O_par);
	}
	
	// Energy from Plane constraint
	if (plane_constraint_present == 1){
		for (i=0; i<Nv; i++) {
			for (j=0; j<24; j++) {
				
				if ((Plane_test_p1[i][j] < 0) || (Plane_test_p2[i][j] < 0) || (Plane_test_p3[i][j] < 0))
					continue;
				
				Point1[0] = Vx[Plane_test_p1[i][j]];Point1[1] = Vy[Plane_test_p1[i][j]];Point1[2] = Vz[Plane_test_p1[i][j]];
				Point2[0] = Vx[Plane_test_p2[i][j]];Point2[1] = Vy[Plane_test_p2[i][j]];Point2[2] = Vz[Plane_test_p2[i][j]];
				Point3[0] = Vx[Plane_test_p3[i][j]];Point3[1] = Vy[Plane_test_p3[i][j]];Point3[2] = Vz[Plane_test_p3[i][j]];
				
				// Pertubation for x
				Point0[0] = Vx[i];Point0[1] = Vy[i];Point0[2] = Vz[i];				
				Dis_tmp = Dis_to_plane(Point1, Point2, Point3, Point0);
				if (Dis_tmp < R_max) {
					E_old += Plane_potential[int(Dis_tmp/dx_plane)];
				}
				
				
			}
		}
	}
	
	// Energy from Yolk
	if (yolk_present == 1){
		Yolk_V = 0;
		for (i=0; i<Nc; i++) {
			
			for (k=0; k<4; k++) {
				BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
				AP[k][0] = Yolk_x[C_ykver[i][k]]; AP[k][1] = Yolk_y[C_ykver[i][k]]; AP[k][2] = Yolk_z[C_ykver[i][k]];
			}
			
			Yolk_volumes[i] = Volume_Calc(AP, BS);
			Yolk_V += Yolk_volumes[i];
		}
		E_old += K_yolk*pow(Yolk_V - Yolk_O, 2);
	}
	
	
	
	// This is to calculate the average displacement and to revert back to the original position
	for (i=0; i<Nv; i++) {
		Vx_nxt[i] = Vx[i];
		Vy_nxt[i] = Vy[i];
		Vz_nxt[i] = Vz[i];
	}
	
	// Now move with the backtrackong algorithm
	
	while (1){

		// Revert to the original position
		for (i=0; i<Nv; i++) {
			Vx[i] = Vx_nxt[i];
			Vy[i] = Vy_nxt[i];
			Vz[i] = Vz_nxt[i];
		}
		
		// Move the verticies
		// Note that you are doing x_k = x_k + alpha*p_k, after the appropriate BC of course
		// Move Apical
		for (i=0; i<(Nv/2); i++) {
			Vx[i] += alpha*V_grad[i][0]*V_not_move_x[i];//*V_not_move[i];
			Vy[i] += alpha*V_grad[i][1]*V_not_move_y[i];
			Vz[i] += alpha*V_grad[i][2]*V_not_move_z[i];//*V_not_move[i];
		}
		
		// Move Basal
		for (i=Nv/2; i<Nv; i++) {
			Vx[i] += alpha*V_grad[i][0]*V_not_move_x[i]*Basal_friction;//*V_not_move[i];
			Vy[i] += alpha*V_grad[i][1]*V_not_move_y[i]*Basal_friction;
			Vz[i] += alpha*V_grad[i][2]*V_not_move_z[i]*Basal_friction;//*V_not_move[i];
		}
		
		
		// Apply boundary conditions 
		if (fixed_top == 1) {
			if (Sheet_condition == 2) {
				for (i=0; i<Nv; i++) {
					if (Vz[i] > Z_spacing) {
						Vz[i] = Z_spacing;
					}
				}
			}
			else if (Sheet_condition == 1){
				for (i=0; i<Nv; i++) {
					//R_tmp = sqrt(Vx[i]*Vx[i] + Vy[i]*Vy[i] + Vz[i]*Vz[i]);
					R_tmp = sqrt(Vx[i]*Vx[i] + Vz[i]*Vz[i]);
					if (R_tmp > r_out) {
						
						Vx[i] = Vx[i]*r_out/R_tmp;
						Vz[i] = Vz[i]*r_out/R_tmp;
					}
				}
			}
		}
		
		if (midline_wall_flag == 1) {
			if (V_side[i] == -1 && Vx[i] > midline_wall) {
				Vx[i] = midline_wall;
			}
			else if (V_side[i] == 1 && Vx[i] < midline_wall) {
				Vx[i] = midline_wall;
			}
		}
		
		// Calculate E_grad_dir = grad(E)_k*alpha*p_k = -grad(E)_k*grad(E)_k
		E_grad_dir = 0;
		for (i=0; i<Nv; i++) {
			E_grad_dir += -(V_grad[i][1]*(Vx[i] - Vx_prv[i]) + V_grad[i][2]*(Vy[i] - Vy_prv[i]) + V_grad[i][3]*(Vz[i] - Vz_prv[i]));
		}
		
		
		// Calculate the E_new = E(x_k + alpha*p_k)
		E_new = 0;
		for (i=0; i<Nc; i++) {
			for (k=0; k<4; k++) {
				AP[k][0] = Vx[C_apver[i][k]]; AP[k][1] = Vy[C_apver[i][k]]; AP[k][2] = Vz[C_apver[i][k]];
				BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
			}
			K_par[0] = A_K_cell[i]; K_par[1] = B_K_cell[i]; K_par[2] = V_K_cell[i];
			K_par[3] = L_K_cell[i]*C_side_factor[i][0]; K_par[4] = L_K_cell[i]*C_side_factor[i][1];
			K_par[5] = L_K_cell[i]*C_side_factor[i][2]; K_par[6] = L_K_cell[i]*C_side_factor[i][3];
			K_par[7] = D_K_cell[i];
			
			O_par[0] = A_0_cell[i]; O_par[1] = B_0_cell[i]; O_par[2] = V_0_cell[i];
			O_par[3] = L_0_cell[i]; O_par[4] = L_0_cell[i]; O_par[5] = L_0_cell[i];
			O_par[6] = L_0_cell[i]; O_par[7] = D_0_cell[i];
			E_new += E_from_cell_V2_diag(AP, BS, K_par, O_par);
		}
		
		// Energy from Plane constraint
		if (plane_constraint_present == 1){
			for (i=0; i<Nv; i++) {
				for (j=0; j<24; j++) {
					
					if ((Plane_test_p1[i][j] < 0) || (Plane_test_p2[i][j] < 0) || (Plane_test_p3[i][j] < 0))
						continue;
					
					Point1[0] = Vx[Plane_test_p1[i][j]];Point1[1] = Vy[Plane_test_p1[i][j]];Point1[2] = Vz[Plane_test_p1[i][j]];
					Point2[0] = Vx[Plane_test_p2[i][j]];Point2[1] = Vy[Plane_test_p2[i][j]];Point2[2] = Vz[Plane_test_p2[i][j]];
					Point3[0] = Vx[Plane_test_p3[i][j]];Point3[1] = Vy[Plane_test_p3[i][j]];Point3[2] = Vz[Plane_test_p3[i][j]];
					
					// Pertubation for x
					Point0[0] = Vx[i];Point0[1] = Vy[i];Point0[2] = Vz[i];				
					Dis_tmp = Dis_to_plane(Point1, Point2, Point3, Point0);
					if (Dis_tmp < R_max) {
						E_new += Plane_potential[int(Dis_tmp/dx_plane)];
						E_Planes[i] += Plane_potential[int(Dis_tmp/dx_plane)];

					}
				}
			}
		}
		
		// Energy from Yolk
		if (yolk_present == 1){
			Yolk_V = 0;
			for (i=0; i<Nc; i++) {
				
				for (k=0; k<4; k++) {
					BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
					AP[k][0] = Yolk_x[C_ykver[i][k]]; AP[k][1] = Yolk_y[C_ykver[i][k]]; AP[k][2] = Yolk_z[C_ykver[i][k]];
				}
				
				Yolk_volumes[i] = Volume_Calc(AP, BS);
				Yolk_V += Yolk_volumes[i];
			}
			E_new += K_yolk*pow(Yolk_V - Yolk_O, 2);
		}
		
		if (E_new - E_old <= E_grad_dir*c) {
			break;
		}
		else {
			alpha *= rho;
		}
		
		//cout << "E_old = " << E_old << "  E_new = " << E_new << "  t = " << t <<  endl << endl;
	}
	
	//cout << "Time loop exited with alpha = " << alpha << endl;
	
	
	// Calculate Average displacement
	Average_Displacement = 0;
	for (i=0; i<Nv; i++) {
		Average_Displacement += sqrt(pow(Vx[i] - Vx_nxt[i], 2) + pow(Vy[i] - Vy_nxt[i], 2) + pow(Vz[i] - Vz_nxt[i], 2));
	}
	Average_Displacement /= Nv;
	
	// Calculate total energy
	E_total = 0;
	for (i=0; i<Nc; i++) {
		// Get the apical verticies
		for (k=0; k<4; k++) {
			AP[k][0] = Vx[C_apver[i][k]]; AP[k][1] = Vy[C_apver[i][k]]; AP[k][2] = Vz[C_apver[i][k]];
			BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
		}
		K_par[0] = A_K_cell[i]; K_par[1] = B_K_cell[i]; K_par[2] = V_K_cell[i];
		K_par[3] = L_K_cell[i]*C_side_factor[i][0]; K_par[4] = L_K_cell[i]*C_side_factor[i][1];
		K_par[5] = L_K_cell[i]*C_side_factor[i][2]; K_par[6] = L_K_cell[i]*C_side_factor[i][3];
		K_par[7] = D_K_cell[i];
		
		O_par[0] = A_0_cell[i]; O_par[1] = B_0_cell[i]; O_par[2] = V_0_cell[i];
		O_par[3] = L_0_cell[i]; O_par[4] = L_0_cell[i]; O_par[5] = L_0_cell[i];
		O_par[6] = L_0_cell[i]; O_par[7] = D_0_cell[i];
		E_total += E_from_cell_V2_diag(AP, BS, K_par, O_par);
	}
	
	// Energy from Plane constraint
	if (plane_constraint_present == 1){
		for (i=0; i<Nv; i++) {
			for (j=0; j<24; j++) {
				
				if ((Plane_test_p1[i][j] < 0) || (Plane_test_p2[i][j] < 0) || (Plane_test_p3[i][j] < 0))
					continue;
				
				Point1[0] = Vx[Plane_test_p1[i][j]];Point1[1] = Vy[Plane_test_p1[i][j]];Point1[2] = Vz[Plane_test_p1[i][j]];
				Point2[0] = Vx[Plane_test_p2[i][j]];Point2[1] = Vy[Plane_test_p2[i][j]];Point2[2] = Vz[Plane_test_p2[i][j]];
				Point3[0] = Vx[Plane_test_p3[i][j]];Point3[1] = Vy[Plane_test_p3[i][j]];Point3[2] = Vz[Plane_test_p3[i][j]];
				
				// Pertubation for x
				Point0[0] = Vx[i];Point0[1] = Vy[i];Point0[2] = Vz[i];				
				Dis_tmp = Dis_to_plane(Point1, Point2, Point3, Point0);
				if (Dis_tmp < R_max) {
					E_total += Plane_potential[int(Dis_tmp/dx_plane)];
				}
			}
		}
	}
	
	// Energy from Yolk
	if (yolk_present == 1){
		Yolk_V = 0;
		for (i=0; i<Nc; i++) {
			
			for (k=0; k<4; k++) {
				BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
				AP[k][0] = Yolk_x[C_ykver[i][k]]; AP[k][1] = Yolk_y[C_ykver[i][k]]; AP[k][2] = Yolk_z[C_ykver[i][k]];
			}
			
			Yolk_volumes[i] = Volume_Calc(AP, BS);
			Yolk_V += Yolk_volumes[i];
		}
		E_total += K_yolk*pow(Yolk_V - Yolk_O, 2);
	}
	
	
	if (t % T_display == 0)
		cout << "t = " << t << " E = " << E_total<< " E_diff = " << fabs(E_total - E_total_old) << " dx average = " << Average_Displacement << " count = " << t_count << endl;
		
	if ((fabs(E_total - E_total_old)/E_total < Percent_stop) || t_count == T_count_max) {
		Change_A_flag = 1;
		cout << "Energy Minimum Reached after = " << t_count << " interations" << endl;
		t_count = 0;
	}
	else {
		t_count++;
	}

		
		if (Change_A_flag == 1){
			
			for (i=0; i<Nc; i++) {
				
				for (k=0; k<4; k++) {
					AP[k][0] = Vx[C_apver[i][k]]; AP[k][1] = Vy[C_apver[i][k]]; AP[k][2] = Vz[C_apver[i][k]];
					BS[k][0] = Vx[C_bsver[i][k]]; BS[k][1] = Vy[C_bsver[i][k]]; BS[k][2] = Vz[C_bsver[i][k]];
				}
				
				Cell_Apical[i] = Area_Calc(AP);//A_0 = A_from_cell(AP, BS);
				Cell_Basal[i] = Area_Calc(BS);//B_0 = B_from_cell(AP, BS);
				Cell_Volume[i] = Volume_Calc(AP, BS);//V_0 = V_from_cell(AP, BS);
				Cell_Lengths_1[i] = sqrt(pow(AP[0][0] - BS[0][0], 2) + pow(AP[0][1] - BS[0][1], 2) + pow(AP[0][2] - BS[0][2], 2));
				Cell_Lengths_2[i] = sqrt(pow(AP[1][0] - BS[1][0], 2) + pow(AP[1][1] - BS[1][1], 2) + pow(AP[1][2] - BS[1][2], 2));
				Cell_Lengths_3[i] = sqrt(pow(AP[2][0] - BS[2][0], 2) + pow(AP[2][1] - BS[2][1], 2) + pow(AP[2][2] - BS[2][2], 2));
				Cell_Lengths_4[i] = sqrt(pow(AP[3][0] - BS[3][0], 2) + pow(AP[3][1] - BS[3][1], 2) + pow(AP[3][2] - BS[3][2], 2));
				
				E_Apical[i] = K_par[0]*pow(Cell_Apical[i] - A_0_cell[i], 2);
				E_Basal[i] = K_par[1]*pow(Cell_Basal[i] - B_0_cell[i], 2);
				E_Sides[i] = K_par[3]*pow(Cell_Lengths_1[i] - L_0_cell[i], 2);
				E_Sides[i] += K_par[4]*pow(Cell_Lengths_2[i] - L_0_cell[i], 2);
				E_Sides[i] += K_par[5]*pow(Cell_Lengths_3[i] - L_0_cell[i], 2);
				E_Sides[i] += K_par[6]*pow(Cell_Lengths_4[i] - L_0_cell[i], 2);
				E_Volume[i] = K_par[2]*pow(Cell_Volume[i] - V_0_cell[i], 2);				
			}
			
			Record_Vert_data(Vx, Vy, Vz, Nv, count, p);
			Record_Cell_data(Cell_Apical, Cell_Basal, Cell_Volume, Cell_Lengths_1, Cell_Lengths_2, Cell_Lengths_3, Cell_Lengths_4, Yolk_V_tmp, Nc, count, p);
			Record_Energy_data(E_Apical, E_Basal, E_Sides, E_Volume, E_Planes, Nc, Nv, count++, p);
			
			
			
			p2 = filename_temp;
			
			count1 = count_tmp/100;
			count2 = (count_tmp - count1*100)/10;
			count3 = count_tmp - count1*100 - count2*10;
			
			*p2 = 'd';
			*(++p2) = 'a';
			*(++p2) = 't';
			*(++p2) = 'a';
			*(++p2) = 'o';
			*(++p2) = 'u';
			*(++p2) = 't';
			*(++p2) = '_';
			*(++p2) = a_record;
			*(++p2) = '_';
			*(++p2) = b_record;
			*(++p2) = '_';
			*(++p2) = digit2char(count1);
			*(++p2) = digit2char(count2);
			*(++p2) = digit2char(count3);
			*(++p2) = '.';
			*(++p2) = 'm';
			p2 = filename_temp;
			
			if (record_init == 1){
				cout << "filename is " << filename_temp << endl;
				Record_All_Data_new_file(Vx, Vy, Vz, Nv, p2); count_tmp++;
			}
		
		
		}
		
		if (E_total_old < E_total && t>0) 
		cout << "WARNING: Energy is increasing." << endl;
		
		if (Percent_stop_flag == 1 && fabs(E_total - E_total_old)/E_total < Percent_stop) {
			cout << "Energy change is less than .1%" << endl;
			break;
		}
		
		E_total_old = E_total;
		t++;
		
		
		}
*/

