

int Cell_Id_Z(int i, int j, int k, int Nx, int Ny){
	return i + (Nx-1)*j + (Nx-1)*(Ny-1)*k;
}

int Cell_Id_P(int i, int j, int Nx){
	return i + Nx*j;
}

int Cell_Id(int i, int j, int Nx){
	return i + (Nx-1)*j;
}

int Ver_Id(int i, int j, int k, int Nx, int Ny){
	int id_tmp = 0;
	if ((i>=0)&&(j>=0)&&(i<Nx)&&(j<Ny)) {
		id_tmp = i + Nx*j + Nx*Ny*k;
	}
	else {
		id_tmp = -1;
	}
	
	return id_tmp;
}

double Dot(double* A, double* B){
	return A[0]*B[0] + A[1]*B[1] + A[2]*B[2];
}

double Norm(double* A){
	return sqrt(A[0]*A[0] + A[1]*A[1] + A[2]*A[2]);
}

double Cross_Product(double* A, double* B, int i){
	if (i==0) {
		return A[1]*B[2] - A[2]*B[1];
	}
	else if (i==1) {
		return B[0]*A[2] - A[0]*B[2];
	}
	else if (i==2) {
		return A[0]*B[1] - A[1]*B[0];
	}
	else {
		return -1000;
	}
	
}

double Dis_to_plane(double* P1, double* P2, double* P3, double* P){
	
	// First determine the equation of the plane
	double A = P1[1]*(P2[2] - P3[2]) + P2[1]*(P3[2] - P1[2]) + P3[1]*(P1[2] - P2[2]);
	double B = P1[2]*(P2[0] - P3[0]) + P2[2]*(P3[0] - P1[0]) + P3[2]*(P1[0] - P2[0]);
	double C = P1[0]*(P2[1] - P3[1]) + P2[0]*(P3[1] - P1[1]) + P3[0]*(P1[1] - P2[1]);
	double D = -(P1[0]*(P2[1]*P3[2] - P3[1]*P2[2]) + P2[0]*(P3[1]*P1[2] - P1[1]*P3[2]) + P3[0]*(P1[1]*P2[2] - P2[1]*P1[2]));
	
	
	return fabs(A*P[0] + B*P[1] + C*P[2] + D)/sqrt(A*A + B*B + C*C);
	
}

double Volume_triplet(double* A1, double* A2, double* A3, double* C){
	int i;
	
	// Initialize vectors
	double* R1 = new double[3];
	double* R2 = new double[3];
	double* R3 = new double[3];
	
	double* C0 = new double[3];
	
	double Volume = 0;
	
	for (i=0; i<3; i++) {
		R1[i] = A1[i] - C[i];
		R2[i] = A2[i] - C[i];
		R3[i] = A3[i] - C[i];
	}
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(R1, R2, i);
	}
	
	Volume = fabs(Dot(R3, C0));
	
	
	delete[] R1;
	delete[] R2;
	delete[] R3;
	delete[] C0;

	return Volume;
	
}



double V_R(double* A1, double* A2, double* A3, double* A0, double* C){
	/*
	V_R = @(R1, R2, R3, R0, C) abs(dot(C - R0, cross(R1 - R0, R2 - R0))) + ...
	abs(dot(C - R0, cross(R2 - R0, R3 - R0))) + ...
	abs(dot(C - R0, cross(R3 - R0, R1 - R0)));
	 */
	int i;
	double* R1 = new double[3];
	double* R2 = new double[3];
	double* R3 = new double[3];
	double* R0 = new double[3];
	
	double* C0 = new double[3];
	
	double Volume = 0;
	
	for (i=0;i<3;i++){
		R1[i] = A1[i] - A0[i];
		R2[i] = A2[i] - A0[i];
		R3[i] = A3[i] - A0[i];
		R0[i] = C[i] - A0[i];
	}
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(R1, R2, i);
	}
	
	Volume += fabs(Dot(R0, C0));
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(R2, R3, i);
	}
	
	Volume += fabs(Dot(R0, C0));
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(R3, R1, i);
	}
	
	Volume += fabs(Dot(R0, C0));
	
	delete[] R1;
	delete[] R2;
	delete[] R3;
	delete[] R0;
	delete[] C0;
	
	return Volume;
}

double Volume_Calc(double** AP, double** BS){
	int i;
	
	double* P1 = new double[3];
	double* P2 = new double[3];
	double* P3 = new double[3];
	double* P0 = new double[3];
	double* PC = new double[3];
	
	double Volume = 0;
	
	// Define center
	for (i=0; i<3; i++) {
		PC[i] = (AP[0][i] + AP[1][i] + AP[2][i] + AP[3][i] + BS[0][i] + BS[1][i] + BS[2][i] + BS[3][i])/8.0;
	}
	
	// Calculate volume from all 8 verticies
	// Apical
	for (i=0; i<3; i++) {
		P0[i] = AP[0][i]; P1[i] = AP[2][i]; P2[i] = AP[1][i]; P3[i] = BS[0][i];
	}
	Volume += V_R(P1, P2, P3, P0, PC)/12;
	
	for (i=0; i<3; i++) {
		P0[i] = AP[1][i]; P1[i] = AP[0][i]; P2[i] = AP[3][i]; P3[i] = BS[1][i];
	}
	Volume += V_R(P1, P2, P3, P0, PC)/12;
	
	for (i=0; i<3; i++) {
		P0[i] = AP[2][i]; P1[i] = AP[3][i]; P2[i] = AP[0][i]; P3[i] = BS[2][i];
	}
	Volume += V_R(P1, P2, P3, P0, PC)/12;
	
	for (i=0; i<3; i++) {
		P0[i] = AP[3][i]; P1[i] = AP[2][i]; P2[i] = AP[1][i]; P3[i] = BS[3][i];
	}
	Volume += V_R(P1, P2, P3, P0, PC)/12;
	
	// Basal
	for (i=0; i<3; i++) {
		P0[i] = BS[0][i]; P1[i] = BS[2][i]; P2[i] = BS[1][i]; P3[i] = AP[0][i];
	}
	Volume += V_R(P1, P2, P3, P0, PC)/12;
	
	for (i=0; i<3; i++) {
		P0[i] = BS[1][i]; P1[i] = BS[3][i]; P2[i] = BS[0][i]; P3[i] = AP[1][i];
	}
	Volume += V_R(P1, P2, P3, P0, PC)/12;
	
	for (i=0; i<3; i++) {
		P0[i] = BS[2][i]; P1[i] = BS[3][i]; P2[i] = BS[0][i]; P3[i] = AP[2][i];
	}
	Volume += V_R(P1, P2, P3, P0, PC)/12;
	
	for (i=0; i<3; i++) {
		P0[i] = BS[3][i]; P1[i] = BS[2][i]; P2[i] = BS[1][i]; P3[i] = AP[3][i];
	}
	Volume += V_R(P1, P2, P3, P0, PC)/12;
	
	delete[] P0;
	delete[] PC;
	delete[] P1;
	delete[] P2;
	delete[] P3;
	
	return Volume;
}

double Area_Calc(double** AP){
	
	int i;
	double* R1 = new double[3];
	double* R2 = new double[3];
	double* C = new double[3];
	
	double Area = 0;
	
	for (i=0; i<3; i++) {
		R1[i] = AP[2][i] - AP[0][i]; R2[i] = AP[1][i] - AP[0][i];
	}
	
	for (i=0; i<3; i++) {
		C[i] = Cross_Product(R1, R2, i);
	}
	
	Area += fabs(Norm(C))/4.0;
	
	for (i=0; i<3; i++) {
		R1[i] = AP[0][i] - AP[1][i]; R2[i] = AP[3][i] - AP[1][i];
	}
	
	for (i=0; i<3; i++) {
		C[i] = Cross_Product(R1, R2, i);
	}
	
	Area += fabs(Norm(C))/4.0;
	
	for (i=0; i<3; i++) {
		R1[i] = AP[1][i] - AP[3][i]; R2[i] = AP[2][i] - AP[3][i];
	}
	
	for (i=0; i<3; i++) {
		C[i] = Cross_Product(R1, R2, i);
	}
	
	Area += fabs(Norm(C))/4.0;
	
	for (i=0; i<3; i++) {
		R1[i] = AP[3][i] - AP[2][i]; R2[i] = AP[0][i] - AP[2][i];
	}
	
	for (i=0; i<3; i++) {
		C[i] = Cross_Product(R1, R2, i);
	}
	
	Area += fabs(Norm(C))/4.0;
	
	delete[] R1;
	delete[] R2;
	delete[] C;
	
	return Area;
	
}

double S_mag(double* R1, double* R2){
	
	int i;
	double* R = new double[3];
	
	for (i=0; i<3; i++) {
		R[i] = R1[i] - R2[1];
	}
	
	return Norm(R);
	delete[] R;
	
}


// MAIN ENERGY CALC FUNCTION
double E_from_cell_V2_diag_onecell(double** AP, double** MD, double** BS, double* K_P, double* O_P){
	
	int i;
	double Volume;
	double Apical_Area;
	double Basal_Area;
	
	double* S0 = new double[3];
	double* S1 = new double[3];
	double* S2 = new double[3];
	double* S3 = new double[3];
	
	double* S4 = new double[3];
	double* S5 = new double[3];
	double* S6 = new double[3];
	double* S7 = new double[3];
	
	double Energy_Volume;
	double Energy_Apical;
	double Energy_Basal;
	double Energy_Side;
	double Energy_Diag;
	double Energy_Basal_Diag;
	double Energy_Apical_Diag;
	double Energy_Mid_Diag;
	
	Volume = Volume_Calc(AP, MD) + Volume_Calc(MD, BS);
	Apical_Area = Area_Calc(AP);
	Basal_Area = Area_Calc(BS);
	
	
	//S0 = S_mag(AP[0], BS[0]);
	//S1 = S_mag(AP[1], BS[1]);
	//S2 = S_mag(AP[2], BS[2]);
	//S3 = S_mag(AP[3], BS[3]);
	
	for (i=0; i<3; i++) {
		S0[i] = AP[0][i] - MD[0][i];
		S1[i] = AP[1][i] - MD[1][i];
		S2[i] = AP[2][i] - MD[2][i];
		S3[i] = AP[3][i] - MD[3][i];
		
		S4[i] = MD[0][i] - BS[0][i];
		S5[i] = MD[1][i] - BS[1][i];
		S6[i] = MD[2][i] - BS[2][i];
		S7[i] = MD[3][i] - BS[3][i];
	}
	
	//cout << "O_P  = [" << O_P[0] << ", "  << O_P[1] << ", "  << O_P[2] << ", "  << O_P[3] << ", "  << O_P[4] << ", "  << O_P[5] << ", "  << O_P[6] << ", "  << O_P[7] << "]" << endl;
	
	//Energy_Volume = K_P[2]*pow(Volume/(O_P[2] + O_P[3]) - 1, 2);
	Energy_Volume = K_P[2]*(pow(Volume_Calc(AP, MD)/O_P[2] - 1, 2) + pow(Volume_Calc(MD, BS)/O_P[3] - 1, 2));
	Energy_Apical = K_P[0]*pow(Apical_Area/O_P[0] - 1, 2);
	Energy_Basal = K_P[1]*pow(Basal_Area/O_P[1] - 1, 2);
	
	//cout << "Energy_Volume = " << Energy_Volume << endl;
	//cout << "Energy_Apical = " << Energy_Apical << endl;
	//cout << "Energy_Basal = " << Energy_Basal << endl;
	/*
	Energy_Side = K_P[3]*pow((Norm(S0) + Norm(S4))/(O_P[4] + O_P[5]) - 1, 2);
	Energy_Side += K_P[4]*pow((Norm(S1) + Norm(S5))/(O_P[4] + O_P[5]) - 1, 2);
	Energy_Side += K_P[5]*pow((Norm(S2) + Norm(S6))/(O_P[4] + O_P[5]) - 1, 2);
	Energy_Side += K_P[6]*pow((Norm(S3) + Norm(S7))/(O_P[4] + O_P[5]) - 1, 2);
	*/
	
	Energy_Side = K_P[3]*pow(Norm(S0)/O_P[4] - 1, 2) + K_P[8]*pow(Norm(S4)/O_P[5] - 1, 2);
	Energy_Side += K_P[4]*pow(Norm(S1)/O_P[4] - 1, 2) + K_P[9]*pow(Norm(S5)/O_P[5] - 1, 2);
	Energy_Side += K_P[5]*pow(Norm(S2)/O_P[4] - 1, 2) + K_P[10]*pow(Norm(S6)/O_P[5] - 1, 2);
	Energy_Side += K_P[6]*pow(Norm(S3)/O_P[4] - 1, 2) + K_P[11]*pow(Norm(S7)/O_P[5] - 1, 2);
	 
	for (i=0; i<3; i++) {
		S0[i] = AP[0][i] - MD[2][i];
		S1[i] = AP[1][i] - MD[3][i];
		S2[i] = AP[2][i] - MD[0][i];
		S3[i] = AP[3][i] - MD[1][i];
		
		S4[i] = MD[0][i] - BS[2][i];
		S5[i] = MD[1][i] - BS[3][i];
		S6[i] = MD[2][i] - BS[0][i];
		S7[i] = MD[3][i] - BS[1][i];
	}
	/*
	Energy_Diag = K_P[7]*pow((Norm(S0) + Norm(S6))/(O_P[6] + O_P[7]) - 1, 2);
	Energy_Diag += K_P[7]*pow((Norm(S1) + Norm(S7))/(O_P[6] + O_P[7]) - 1, 2);
	Energy_Diag += K_P[7]*pow((Norm(S2) + Norm(S4))/(O_P[6] + O_P[7]) - 1, 2);
	Energy_Diag += K_P[7]*pow((Norm(S3) + Norm(S5))/(O_P[6] + O_P[7]) - 1, 2);
	*/
	
	Energy_Diag = K_P[7]*pow(Norm(S0)/O_P[6] - 1, 2) + K_P[12]*pow(Norm(S4)/O_P[7] - 1, 2);
	Energy_Diag += K_P[7]*pow(Norm(S1)/O_P[6] - 1, 2) + K_P[12]*pow(Norm(S5)/O_P[7] - 1, 2);
	Energy_Diag += K_P[7]*pow(Norm(S2)/O_P[6] - 1, 2) + K_P[12]*pow(Norm(S6)/O_P[7] - 1, 2);
	Energy_Diag += K_P[7]*pow(Norm(S3)/O_P[6] - 1, 2) + K_P[12]*pow(Norm(S7)/O_P[7] - 1, 2);
	
	for (i=0; i<3; i++) {
		S0[i] = BS[0][i] - BS[2][i];
		S1[i] = BS[1][i] - BS[3][i];
		
		S2[i] = AP[0][i] - AP[2][i];
		S3[i] = AP[1][i] - AP[3][i];
		
		S4[i] = MD[0][i] - MD[2][i];
		S5[i] = MD[1][i] - MD[3][i];
	}
	
	Energy_Basal_Diag = K_P[8]*pow(Norm(S0) - Norm(S1), 2);
	Energy_Apical_Diag = K_P[9]*pow(Norm(S2) - Norm(S3), 2);
	Energy_Mid_Diag = K_P[9]*pow(Norm(S4) - Norm(S5), 2);
	
	delete[] S0;
	delete[] S1;
	delete[] S2;
	delete[] S3;
	
	delete[] S4;
	delete[] S5;
	delete[] S6;
	delete[] S7;
	
	return Energy_Volume + Energy_Apical + Energy_Basal + Energy_Side + Energy_Diag + Energy_Basal_Diag + Energy_Apical_Diag + Energy_Mid_Diag;
}


double Energy_Total(double* Vx, double* Vy, double* Vz, int Nv, int Nc, int** C_apver, int** C_bsver, double* A_K, double* A_0, double* B_K, double* B_0, double* V_K, double* V_0, double* L_K, double* L_0, double* D_K, double* D_0, int** C_side){

	int i, j, k;
	double Energy_tmp = 0;
	double V_tmp, A_tmp, M_tmp, B_tmp;
	
	double** AP = new double*[4];
	double** MD = new double*[4];
	double** BS = new double*[4];
	
	double* S0 = new double[3];
	double* S1 = new double[3];
	double* S2 = new double[3];
	double* S3 = new double[3];
	
	double* S4 = new double[3];
	double* S5 = new double[3];
	double* S6 = new double[3];
	double* S7 = new double[3];
	
	for (i=0; i<4; i++) {
		AP[i] = new double[3];
		MD[i] = new double[3];
		BS[i] = new double[3];
	}
	
	for (i=0; i<Nc/2; 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]];
			MD[k][0] = Vx[C_apver[i+Nc/2][k]]; MD[k][1] = Vy[C_apver[i+Nc/2][k]]; MD[k][2] = Vz[C_apver[i+Nc/2][k]];
			BS[k][0] = Vx[C_bsver[i+Nc/2][k]]; BS[k][1] = Vy[C_bsver[i+Nc/2][k]]; BS[k][2] = Vz[C_bsver[i+Nc/2][k]];
		}
		V_tmp = Volume_Calc(AP, MD) + Volume_Calc(MD, BS);
		A_tmp = Area_Calc(AP);
		B_tmp = Area_Calc(BS);
		
		Energy_tmp += V_K[i]*pow(V_tmp/V_0[i] - 1, 2);
		Energy_tmp += A_K[i]*pow(A_tmp/A_0[i] - 1, 2);
		Energy_tmp += B_K[i]*pow(B_tmp/B_0[i] - 1, 2);
		
		for (i=0; i<3; i++) {
			S0[i] = AP[0][i] - MD[0][i];
			S1[i] = AP[1][i] - MD[1][i];
			S2[i] = AP[2][i] - MD[2][i];
			S3[i] = AP[3][i] - MD[3][i];
			
			S4[i] = MD[0][i] - BS[0][i];
			S5[i] = MD[1][i] - BS[1][i];
			S6[i] = MD[2][i] - BS[2][i];
			S7[i] = MD[3][i] - BS[3][i];
		}
		
		Energy_tmp += C_side[i][0]*L_K[i]*pow(Norm(S0)/L_0[i] - 1, 2) + C_side[i+Nc/2][0]*L_K[i+Nc/2]*pow(Norm(S4)/L_0[i+Nc/2] - 1, 2);
		Energy_tmp += C_side[i][1]*L_K[i]*pow(Norm(S1)/L_0[i] - 1, 2) + C_side[i+Nc/2][1]*L_K[i+Nc/2]*pow(Norm(S5)/L_0[i+Nc/2] - 1, 2);
		Energy_tmp += C_side[i][2]*L_K[i]*pow(Norm(S2)/L_0[i] - 1, 2) + C_side[i+Nc/2][2]*L_K[i+Nc/2]*pow(Norm(S6)/L_0[i+Nc/2] - 1, 2);
		Energy_tmp += C_side[i][3]*L_K[i]*pow(Norm(S3)/L_0[i] - 1, 2) + C_side[i+Nc/2][3]*L_K[i+Nc/2]*pow(Norm(S7)/L_0[i+Nc/2] - 1, 2);
		
		for (i=0; i<3; i++) {
			S0[i] = AP[0][i] - MD[2][i];
			S1[i] = AP[1][i] - MD[3][i];
			S2[i] = AP[2][i] - MD[0][i];
			S3[i] = AP[3][i] - MD[1][i];
			
			S4[i] = MD[0][i] - BS[2][i];
			S5[i] = MD[1][i] - BS[3][i];
			S6[i] = MD[2][i] - BS[0][i];
			S7[i] = MD[3][i] - BS[1][i];
		}
		
		Energy_tmp += D_K[i]*pow(Norm(S0)/D_0[i] - 1, 2) + D_K[i+Nc/2]*pow(Norm(S4)/D_0[i+Nc/2] - 1, 2);
		Energy_tmp += D_K[i]*pow(Norm(S1)/D_0[i] - 1, 2) + D_K[i+Nc/2]*pow(Norm(S5)/D_0[i+Nc/2] - 1, 2);
		Energy_tmp += D_K[i]*pow(Norm(S2)/D_0[i] - 1, 2) + D_K[i+Nc/2]*pow(Norm(S6)/D_0[i+Nc/2] - 1, 2);
		Energy_tmp += D_K[i]*pow(Norm(S3)/D_0[i] - 1, 2) + D_K[i+Nc/2]*pow(Norm(S7)/D_0[i+Nc/2] - 1, 2);
	
		for (i=0; i<3; i++) {
			S0[i] = BS[0][i] - BS[2][i];
			S1[i] = BS[1][i] - BS[3][i];
			
			S2[i] = AP[0][i] - AP[2][i];
			S3[i] = AP[1][i] - AP[3][i];
			
			S4[i] = MD[0][i] - MD[2][i];
			S5[i] = MD[1][i] - MD[3][i];
		}
		
		Energy_tmp += 10.0*pow(Norm(S0) - Norm(S1), 2);
		Energy_tmp += 10.0*pow(Norm(S2) - Norm(S3), 2);
		Energy_tmp += 10.0*pow(Norm(S4) - Norm(S5), 2);
	
	}
	
	delete[] S0;
	delete[] S1;
	delete[] S2;
	delete[] S3;
	
	delete[] S4;
	delete[] S5;
	delete[] S6;
	delete[] S7;
	
	delete[] AP;
	delete[] MD;
	delete[] BS;
	
	
	return Energy_tmp;
	
}


double Apical_Volume_Calc(double* Vx, double* Vy, double* Vz, int Nc, double r_out, int** C_apver){
	
	int i, k;
	
	double** A = new double*[4];
	double** B = new double*[4];
	
	for (i=0; i<4; i++) {
		A[i] = new double[3];
		B[i] = new double[3];
	}
	
	double Volume_tmp = 0;
	
	for (i=0; i<Nc; i++) {
		for (k=0; k<4; k++) {
			A[k][0] = Vx[C_apver[i][k]]; A[k][1] = Vy[C_apver[i][k]]; A[k][2] = Vz[C_apver[i][k]];
			
			B[k][0] = r_out*A[k][0]/sqrt(A[k][0]*A[k][0] + A[k][2]*A[k][2]);
			B[k][1] = A[k][1];
			B[k][2] = r_out*A[k][2]/sqrt(A[k][0]*A[k][0] + A[k][2]*A[k][2]);
		}
		Volume_tmp += Volume_Calc(A, B);
	}
	
	
	delete[] A;
	delete[] B;
	
	return Volume_tmp;
}

double E_from_cell_V2_diag(double** AP, double** BS, double* K_P, double* O_P){
	
	int i;
	double Volume;
	double Apical_Area;
	double Basal_Area;
	
	double* S0 = new double[3];
	double* S1 = new double[3];
	double* S2 = new double[3];
	double* S3 = new double[3];
	
	double Energy_Volume;
	double Energy_Apical;
	double Energy_Basal;
	double Energy_Side;
	double Energy_Diag;
	double Energy_Basal_Diag;
	double Energy_Apical_Diag;
	
	Volume = Volume_Calc(AP, BS);
	Apical_Area = Area_Calc(AP);
	Basal_Area = Area_Calc(BS);
	
	//S0 = S_mag(AP[0], BS[0]);
	//S1 = S_mag(AP[1], BS[1]);
	//S2 = S_mag(AP[2], BS[2]);
	//S3 = S_mag(AP[3], BS[3]);
	
	for (i=0; i<3; i++) {
		S0[i] = AP[0][i] - BS[0][i];
		S1[i] = AP[1][i] - BS[1][i];
		S2[i] = AP[2][i] - BS[2][i];
		S3[i] = AP[3][i] - BS[3][i];
	}
	
	Energy_Volume = K_P[2]*pow(Volume/O_P[2] - 1, 2);
	Energy_Apical = K_P[0]*pow(Apical_Area/O_P[0] - 1, 2);
	Energy_Basal = K_P[1]*pow(Basal_Area/O_P[1] - 1, 2);
	
	Energy_Side = K_P[3]*pow(Norm(S0)/O_P[3] - 1, 2);
	Energy_Side += K_P[4]*pow(Norm(S1)/O_P[4] - 1, 2);
	Energy_Side += K_P[5]*pow(Norm(S2)/O_P[5] - 1, 2);
	Energy_Side += K_P[6]*pow(Norm(S3)/O_P[6] - 1, 2);
	
	for (i=0; i<3; i++) {
		S0[i] = AP[0][i] - BS[2][i];
		S1[i] = AP[1][i] - BS[3][i];
		S2[i] = AP[2][i] - BS[0][i];
		S3[i] = AP[3][i] - BS[1][i];
	}
	
	Energy_Diag = K_P[7]*pow(Norm(S0)/O_P[7] - 1, 2);
	Energy_Diag += K_P[7]*pow(Norm(S1)/O_P[7] - 1, 2);
	Energy_Diag += K_P[7]*pow(Norm(S2)/O_P[7] - 1, 2);
	Energy_Diag += K_P[7]*pow(Norm(S3)/O_P[7] - 1, 2);
	
	for (i=0; i<3; i++) {
		S0[i] = BS[0][i] - BS[2][i];
		S1[i] = BS[1][i] - BS[3][i];
		S2[i] = AP[0][i] - AP[2][i];
		S3[i] = AP[1][i] - AP[3][i];
	}
	
	Energy_Basal_Diag = K_P[8]*pow(Norm(S0) - Norm(S1), 2);
	Energy_Apical_Diag = K_P[9]*pow(Norm(S2) - Norm(S3), 2);
	
	delete[] S0;
	delete[] S1;
	delete[] S2;
	delete[] S3;
	
	return Energy_Volume + Energy_Apical + Energy_Basal + Energy_Side + Energy_Diag + Energy_Basal_Diag;
}


double A_from_cell(double** AP, double** BS){
	
	int i=0;
	// Initialize vectors
	double* B01 = new double[3];
	double* B31 = new double[3];
	double* Sa1_b1 = new double[3];
	
	double* A13 = new double[3];
	double* Sb3_a3 = new double[3];
	double* Sb0_a3 = new double[3];
	
	double* B32 = new double[3];
	double* B02 = new double[3];
	double* Sa3_b2 = new double[3];
	
	double* A32 = new double[3];
	double* A02 = new double[3];
	double* Sb2_a2 = new double[3];
	
	double* B20 = new double[3];
	double* Sa0_b0 = new double[3];
	double* Sa3_b0 = new double[3];
	
	double* A31 = new double[3];
	double* A01 = new double[3];
	double* Sb0_a1 = new double[3];
	
	// Assing the vectors
	for (i=0; i<3; i++) {
		B01[i] = BS[0][i] - BS[1][i];
		B31[i] = BS[3][i] - BS[1][i];
		Sa1_b1[i] = AP[1][i] - BS[1][i];
		
		A13[i] = AP[1][i] - AP[3][i];
		Sb3_a3[i] = BS[3][i] - AP[3][i];
		Sb0_a3[i] = BS[0][i] - AP[3][i];
		
		B32[i] = BS[3][i] - BS[2][i];
		B02[i] = BS[0][i] - BS[2][i];
		Sa3_b2[i] = AP[3][i] - BS[2][i];
		
		A32[i] = AP[3][i] - AP[2][i];
		A02[i] = AP[0][i] - AP[2][i];
		Sb2_a2[i] = BS[2][i] - AP[2][i];
		
		B20[i] = BS[2][i] - BS[0][i];
		Sa0_b0[i] = AP[0][i] - BS[0][i];
		Sa3_b0[i] = AP[3][i] - BS[0][i];
		
		A31[i] = AP[3][i] - AP[1][i];
		A01[i] = AP[0][i] - AP[1][i];
		Sb0_a1[i] = BS[0][i] - AP[1][i];
	}
	
	
	// Initialize the cross product vectors
	double* C0 = new double[3];
	double* C1 = new double[3];
	double* C2 = new double[3];
	double* C3 = new double[3];
	double* C4 = new double[3];
	double* C5 = new double[3];
	
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(B01, B31, i);
		C1[i] = Cross_Product(A31, Sb3_a3, i);
		C2[i] = Cross_Product(B20, B32, i);
		
		C3[i] = Cross_Product(A32, A02, i);
		C4[i] = Cross_Product(B20, Sa0_b0, i);
		C5[i] = Cross_Product(A31, A01, i);
	}
	
	
	// Calculate Apical Area and Basal Area
	double Area_apical = 0; double Area_basal = 0;
	Area_basal += Norm(C0)/2.0 + Norm(C2)/2.0;
	Area_apical += Norm(C3)/2.0 + Norm(C5)/2.0;
	
	for (i=0; i<3; i++) {
		B01[i] = BS[1][i] - BS[0][i];
		B31[i] = BS[2][i] - BS[0][i];
		
		B32[i] = BS[2][i] - BS[3][i];
		B02[i] = BS[1][i] - BS[3][i];
		
		A32[i] = AP[2][i] - AP[3][i];
		A02[i] = AP[1][i] - AP[3][i];
		
		A31[i] = AP[1][i] - AP[0][i];
		A01[i] = AP[2][i] - AP[0][i];
	}
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(B01, B31, i);
		C2[i] = Cross_Product(B20, B32, i);
		
		C3[i] = Cross_Product(A32, A02, i);
		C5[i] = Cross_Product(A31, A01, i);
	}
	
	Area_basal += Norm(C0)/2.0 + Norm(C2)/2.0;
	Area_apical += Norm(C3)/2.0 + Norm(C5)/2.0;
	
	Area_apical /= 2.0;
	Area_basal /= 2.0;	
	
	delete B01;
	delete B31;
	delete Sa1_b1;
	
	delete A13;
	delete Sb3_a3;
	delete Sb0_a3;
	
	delete B32;
	delete B02;
	delete Sa3_b2;
	
	delete A32;
	delete A02;
	delete Sb2_a2;
	
	delete B20;
	delete Sa0_b0;
	delete Sa3_b0;
	
	delete A31;
	delete A01;
	delete Sb0_a1;
	
	delete C0;
	delete C1;
	delete C2;
	delete C3;
	delete C4;
	delete C5;
	
	return Area_apical;
	
}

double B_from_cell(double** AP, double** BS){
	
	int i=0;
	// Initialize vectors
	double* B01 = new double[3];
	double* B31 = new double[3];
	double* Sa1_b1 = new double[3];
	
	double* A13 = new double[3];
	double* Sb3_a3 = new double[3];
	double* Sb0_a3 = new double[3];
	
	double* B32 = new double[3];
	double* B02 = new double[3];
	double* Sa3_b2 = new double[3];
	
	double* A32 = new double[3];
	double* A02 = new double[3];
	double* Sb2_a2 = new double[3];
	
	double* B20 = new double[3];
	double* Sa0_b0 = new double[3];
	double* Sa3_b0 = new double[3];
	
	double* A31 = new double[3];
	double* A01 = new double[3];
	double* Sb0_a1 = new double[3];
	
	// Assing the vectors
	for (i=0; i<3; i++) {
		B01[i] = BS[0][i] - BS[1][i];
		B31[i] = BS[3][i] - BS[1][i];
		Sa1_b1[i] = AP[1][i] - BS[1][i];
		
		A13[i] = AP[1][i] - AP[3][i];
		Sb3_a3[i] = BS[3][i] - AP[3][i];
		Sb0_a3[i] = BS[0][i] - AP[3][i];
		
		B32[i] = BS[3][i] - BS[2][i];
		B02[i] = BS[0][i] - BS[2][i];
		Sa3_b2[i] = AP[3][i] - BS[2][i];
		
		A32[i] = AP[3][i] - AP[2][i];
		A02[i] = AP[0][i] - AP[2][i];
		Sb2_a2[i] = BS[2][i] - AP[2][i];
		
		B20[i] = BS[2][i] - BS[0][i];
		Sa0_b0[i] = AP[0][i] - BS[0][i];
		Sa3_b0[i] = AP[3][i] - BS[0][i];
		
		A31[i] = AP[3][i] - AP[1][i];
		A01[i] = AP[0][i] - AP[1][i];
		Sb0_a1[i] = BS[0][i] - AP[1][i];
	}
	
	
	// Initialize the cross product vectors
	double* C0 = new double[3];
	double* C1 = new double[3];
	double* C2 = new double[3];
	double* C3 = new double[3];
	double* C4 = new double[3];
	double* C5 = new double[3];
	
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(B01, B31, i);
		C1[i] = Cross_Product(A31, Sb3_a3, i);
		C2[i] = Cross_Product(B20, B32, i);
		
		C3[i] = Cross_Product(A32, A02, i);
		C4[i] = Cross_Product(B20, Sa0_b0, i);
		C5[i] = Cross_Product(A31, A01, i);
	}
	
	
	// Calculate Apical Area and Basal Area
	double Area_apical = 0; double Area_basal = 0;
	Area_basal += Norm(C0)/2.0 + Norm(C2)/2.0;
	Area_apical += Norm(C3)/2.0 + Norm(C5)/2.0;
	
	for (i=0; i<3; i++) {
		B01[i] = BS[1][i] - BS[0][i];
		B31[i] = BS[2][i] - BS[0][i];
		
		B32[i] = BS[2][i] - BS[3][i];
		B02[i] = BS[1][i] - BS[3][i];
		
		A32[i] = AP[2][i] - AP[3][i];
		A02[i] = AP[1][i] - AP[3][i];
		
		A31[i] = AP[1][i] - AP[0][i];
		A01[i] = AP[2][i] - AP[0][i];
	}
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(B01, B31, i);
		C2[i] = Cross_Product(B20, B32, i);
		
		C3[i] = Cross_Product(A32, A02, i);
		C5[i] = Cross_Product(A31, A01, i);
	}
	
	Area_basal += Norm(C0)/2.0 + Norm(C2)/2.0;
	Area_apical += Norm(C3)/2.0 + Norm(C5)/2.0;
	
	Area_apical /= 2.0;
	Area_basal /= 2.0;
	
	// Calculate side edges
	
	
	delete B01;
	delete B31;
	delete Sa1_b1;
	
	delete A13;
	delete Sb3_a3;
	delete Sb0_a3;
	
	delete B32;
	delete B02;
	delete Sa3_b2;
	
	delete A32;
	delete A02;
	delete Sb2_a2;
	
	delete B20;
	delete Sa0_b0;
	delete Sa3_b0;
	
	delete A31;
	delete A01;
	delete Sb0_a1;
	
	delete C0;
	delete C1;
	delete C2;
	delete C3;
	delete C4;
	delete C5;
	
	return Area_basal;
}


double V_from_cell(double** AP, double** BS){
	
	int i=0;
	// Initialize vectors
	double* B01 = new double[3];
	double* B31 = new double[3];
	double* Sa1_b1 = new double[3];
	
	double* A13 = new double[3];
	double* Sb3_a3 = new double[3];
	double* Sb0_a3 = new double[3];
	
	double* B32 = new double[3];
	double* B02 = new double[3];
	double* Sa3_b2 = new double[3];
	
	double* A32 = new double[3];
	double* A02 = new double[3];
	double* Sb2_a2 = new double[3];
	
	double* B20 = new double[3];
	double* Sa0_b0 = new double[3];
	double* Sa3_b0 = new double[3];
	
	double* A31 = new double[3];
	double* A01 = new double[3];
	double* Sb0_a1 = new double[3];
	
	// Assing the vectors
	for (i=0; i<3; i++) {
		B01[i] = BS[0][i] - BS[1][i];
		B31[i] = BS[3][i] - BS[1][i];
		Sa1_b1[i] = AP[1][i] - BS[1][i];
		
		A13[i] = AP[1][i] - AP[3][i];
		Sb3_a3[i] = BS[3][i] - AP[3][i];
		Sb0_a3[i] = BS[0][i] - AP[3][i];
		
		B32[i] = BS[3][i] - BS[2][i];
		B02[i] = BS[0][i] - BS[2][i];
		Sa3_b2[i] = AP[3][i] - BS[2][i];
		
		A32[i] = AP[3][i] - AP[2][i];
		A02[i] = AP[0][i] - AP[2][i];
		Sb2_a2[i] = BS[2][i] - AP[2][i];
		
		B20[i] = BS[2][i] - BS[0][i];
		Sa0_b0[i] = AP[0][i] - BS[0][i];
		Sa3_b0[i] = AP[3][i] - BS[0][i];
		
		A31[i] = AP[3][i] - AP[1][i];
		A01[i] = AP[0][i] - AP[1][i];
		Sb0_a1[i] = BS[0][i] - AP[1][i];
	}
	
	
	// Initialize the cross product vectors
	double* C0 = new double[3];
	double* C1 = new double[3];
	double* C2 = new double[3];
	double* C3 = new double[3];
	double* C4 = new double[3];
	double* C5 = new double[3];
	
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(B01, B31, i);
		C1[i] = Cross_Product(A31, Sb3_a3, i);
		C2[i] = Cross_Product(B20, B32, i);
		
		C3[i] = Cross_Product(A32, A02, i);
		C4[i] = Cross_Product(B20, Sa0_b0, i);
		C5[i] = Cross_Product(A31, A01, i);
	}
	
	// Calculate Volume of cell
	double Volume = 0;
	
	Volume += fabs(Dot(Sa1_b1, C0));
	Volume += fabs(Dot(Sb0_a3, C1));
	Volume += fabs(Dot(Sa3_b2, C2));
	Volume += fabs(Dot(Sb2_a2, C3));
	Volume += fabs(Dot(Sa3_b0, C4));
	Volume += fabs(Dot(Sb0_a1, C5));
	
	Volume /= 6.0;
	
	delete B01;
	delete B31;
	delete Sa1_b1;
	
	delete A13;
	delete Sb3_a3;
	delete Sb0_a3;
	
	delete B32;
	delete B02;
	delete Sa3_b2;
	
	delete A32;
	delete A02;
	delete Sb2_a2;
	
	delete B20;
	delete Sa0_b0;
	delete Sa3_b0;
	
	delete A31;
	delete A01;
	delete Sb0_a1;
	
	delete C0;
	delete C1;
	delete C2;
	delete C3;
	delete C4;
	delete C5;
	
	return Volume;	
}

double Yolk_Volume_from_cell(double** BS, double** S){
	
	int i=0;
	// Initialize vectors
	double* B20 = new double[3];
	double* B10 = new double[3];
	double* Ss0_b0 = new double[3];
	
	double* B23 = new double[3];
	double* B13 = new double[3];
	double* Ss1_b3 = new double[3];
	
	double* Ss0_s1 = new double[3];
	double* Sb2_s1 = new double[3];
	double* Sb3_s1 = new double[3];
	
	
	// Assing the vectors
	for (i=0; i<3; i++) {
		B20[i] = BS[2][i] - BS[0][i];
		B10[i] = BS[1][i] - BS[0][i];
		Ss0_b0[i] = S[0][i] - BS[0][i];
		
		B23[i] = BS[2][i] - BS[3][i];
		B13[i] = BS[1][i] - BS[3][i];
		Ss1_b3[i] = S[1][i] - BS[3][i];
		
		Ss0_s1[i] = S[0][i] - S[1][i];
		Sb2_s1[i] = BS[2][i] - S[1][i];
		Sb3_s1[i] = BS[3][i] - S[1][i];
	}
	
	
	// Initialize the cross product vectors
	double* C0 = new double[3];
	double* C1 = new double[3];
	double* C2 = new double[3];
	
	
	for (i=0; i<3; i++) {
		C0[i] = Cross_Product(B20, Ss0_b0, i);
		C1[i] = Cross_Product(B23, B13, i);
		C2[i] = Cross_Product(Ss0_s1, Sb2_s1, i);
	}
	
	// Calculate Volume of cell
	double Yolk_Volume = 0;
	double V0, V1, V2;
	
	V0 = fabs(Dot(B10, C0));
	V1 = fabs(Dot(Ss1_b3, C1));
	V2 = fabs(Dot(Sb3_s1, C2));
	/*
	cout << "V1 = " << V0 << " V2 = " << V1 << " V3 = " << V2 << " V_total = " << V0 + V1 + V2 << endl;
	cout << "|Ss0_s1| = " << Norm(Ss0_s1)<<endl;
	cout << "|Ss0_b0| = " << Norm(Ss0_b0)<<endl;
	cout << "|Sb2_s1| = " << Norm(Sb2_s1)<<endl<<endl;
	*/
	
	Yolk_Volume = (V0 + V1 + V2)/6.0;
	
	delete B20;
	delete B10;
	delete Ss0_b0;
	
	delete B23;
	delete B13;
	delete Ss1_b3;
	
	delete Ss0_s1;
	delete Sb2_s1;
	delete Sb3_s1;
	
	delete C0;
	delete C1;
	delete C2;
	
	return Yolk_Volume;	
}


