// This is for recording the data

char digit2char(int digit)
{
	switch (digit) {
		case 0:
			return '0';
			break;
		case 1:
			return '1';
			break;
		case 2:
			return '2';
			break;
		case 3:
			return '3';
			break;
		case 4:
			return '4';
			break;
		case 5:
			return '5';
			break;
		case 6:
			return '6';
			break;
		case 7:
			return '7';
			break;
		case 8:
			return '8';
			break;
		case 9:
			return '9';
			break;
		default:
			break;
	}
	
	
}

void Record_Verticies(double *V_x, double *V_y, double *V_z, int** V_c, int N, char m, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::app);
	
	myfile<<"V_x_"<<m<<" = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_x[i]<<", ";
	}
	myfile<<V_x[N-1]<<"];"<<endl;
	
	myfile<<"V_y_"<<m<<" = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_y[i]<<", ";
	}
	myfile<<V_y[N-1]<<"];"<<endl;
	
	myfile<<"V_z_"<<m<<" = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_z[i]<<", ";
	}
	myfile<<V_z[N-1]<<"];"<<endl;
	
	myfile<<"V_c = cell(" << N << ", 1);";
	for (int i=0; i<N; i++) {
		myfile<<"V_c{"<<i+1<<"} = ["<< V_c[i][0] << ", " << V_c[i][1] << ", " << V_c[i][2] << ", " << V_c[i][3] << "];" << endl;
	}
	
	myfile.close();
}

void Record_Vert_data(double *V_x, double *V_y, double *V_z, int N, int c, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::app);
	
	myfile<<"V_x = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_x[i]<<", ";
	}
	myfile<<V_x[N-1]<<"];"<<endl;
	
	myfile<<"V_y = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_y[i]<<", ";
	}
	myfile<<V_y[N-1]<<"];"<<endl;
	
	myfile<<"V_z = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_z[i]<<", ";
	}
	myfile<<V_z[N-1]<<"];"<<endl;
	myfile<<"V_cell{"<<c<<"} = [V_x;V_y;V_z];"<<endl;
	
	myfile.close();
}

void Record_Energy_data(double *E_A, double *E_B, double *E_L, double *E_V,  double *E_P, int Nc, int Nv, int c, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::app);
	
	myfile<<"E_A_cell{"<<c<<"} = [";
	for (int i=0; i<Nc-1; i++) {
		myfile<<E_A[i]<<", ";
	}
	myfile<<E_A[Nc-1]<<"];"<<endl;
	
	myfile<<"E_B_cell{"<<c<<"} = [";
	for (int i=0; i<Nc-1; i++) {
		myfile<<E_B[i]<<", ";
	}
	myfile<<E_B[Nc-1]<<"];"<<endl;
	
	myfile<<"E_V_cell{"<<c<<"} = [";
	for (int i=0; i<Nc-1; i++) {
		myfile<<E_V[i]<<", ";
	}
	myfile<<E_V[Nc-1]<<"];"<<endl;
	
	myfile<<"E_L_cell{"<<c<<"} = [";
	for (int i=0; i<Nc-1; i++) {
		myfile<<E_L[i]<<", ";
	}
	myfile<<E_L[Nc-1]<<"];"<<endl;
	
	myfile<<"E_P_cell{"<<c<<"} = [";
	for (int i=0; i<Nv-1; i++) {
		myfile<<E_P[i]<<", ";
	}
	myfile<<E_P[Nv-1]<<"];"<<endl<<endl;
	
	myfile.close();
}

void Record_All_Data_new_file(double *V_x, double *V_y, double *V_z, int N, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::out);
	
	myfile<<"V_x = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_x[i]<<", ";
	}
	myfile<<V_x[N-1]<<"];"<<endl;
	
	myfile<<"V_y = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_y[i]<<", ";
	}
	myfile<<V_y[N-1]<<"];"<<endl;
	
	myfile<<"V_z = [";
	for (int i=0; i<N-1; i++) {
		myfile<<V_z[i]<<", ";
	}
	myfile<<V_z[N-1]<<"];"<<endl;
	
	myfile.close();
}

void Record_Cell_data(double *C_A, double *C_B, double *C_V, double *C_L_1, double *C_L_2, double *C_L_3, double *C_L_4, double Yolk, int N, int c, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::app);
	
	myfile<<"Apical_cell{"<<c<<"} = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_A[i]<<", ";
	}
	myfile<<C_A[N-1]<<"];"<<endl;
	
	myfile<<"Basal_cell{"<<c<<"} = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_B[i]<<", ";
	}
	myfile<<C_B[N-1]<<"];"<<endl;
	
	myfile<<"Volume_cell{"<<c<<"} = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_V[i]<<", ";
	}
	myfile<<C_V[N-1]<<"];"<<endl;
	
	myfile<<"L1_cell{"<<c<<"} = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_L_1[i]<<", ";
	}
	myfile<<C_L_1[N-1]<<"];"<<endl;
	
	myfile<<"L2_cell{"<<c<<"} = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_L_2[i]<<", ";
	}
	myfile<<C_L_2[N-1]<<"];"<<endl;
	
	myfile<<"L3_cell{"<<c<<"} = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_L_3[i]<<", ";
	}
	myfile<<C_L_3[N-1]<<"];"<<endl;
	
	myfile<<"L4_cell{"<<c<<"} = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_L_4[i]<<", ";
	}
	myfile<<C_L_4[N-1]<<"];"<<endl;
	
	myfile<<"Yolk_cell{"<<c<<"} = ["<<Yolk<<"];"<<endl<<endl;
	
	myfile.close();
}

void Record_Cell_Parameters(double *K_A, double *K_B, double *K_V, double *K_L, double K_Y, int N, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::app);
	
	myfile<<"KA_cell = [";
	for (int i=0; i<N-1; i++) {
		myfile<<K_A[i]<<", ";
	}
	myfile<<K_A[N-1]<<"];"<<endl;
	
	myfile<<"KB_cell = [";
	for (int i=0; i<N-1; i++) {
		myfile<<K_B[i]<<", ";
	}
	myfile<<K_B[N-1]<<"];"<<endl;
	
	myfile<<"KV_cell = [";
	for (int i=0; i<N-1; i++) {
		myfile<<K_V[i]<<", ";
	}
	myfile<<K_V[N-1]<<"];"<<endl;
	
	myfile<<"KL_cell = [";
	for (int i=0; i<N-1; i++) {
		myfile<<K_L[i]<<", ";
	}
	myfile<<K_L[N-1]<<"];"<<endl;
	
	myfile<<"KY_cell = ["<<K_Y<<"];"<<endl<<endl;
	
	myfile.close();
}

void Record_Center(double *C_x, double *C_y, double *C_z, int N, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::app);
	
	myfile<<"Center_x = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_x[i]<<", ";
	}
	myfile<<C_x[N-1]<<"];"<<endl;
	
	myfile<<"Center_y = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_y[i]<<", ";
	}
	myfile<<C_y[N-1]<<"];"<<endl;
	
	myfile<<"Center_z = [";
	for (int i=0; i<N-1; i++) {
		myfile<<C_z[i]<<", ";
	}
	myfile<<C_z[N-1]<<"];"<<endl;
	
	
	myfile.close();
}

void Record_Cells(int** AP_ver, int** BS_ver, int N, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::app);
	
	myfile<<"AP_ver_id = cell(" << N << ", 1);";
	for (int i=0; i<N; i++) {
		myfile<<"AP_ver_id{"<<i+1<<"} = ["<<AP_ver[i][0]<<", "<<AP_ver[i][1] <<", "<<AP_ver[i][2] <<", "<<AP_ver[i][3]<< "];"<<endl;
	}
	
	myfile<<"BS_ver_id = cell(" << N << ", 1);";
	for (int i=0; i<N; i++) {
		myfile<<"BS_ver_id{"<<i+1<<"} = ["<< BS_ver[i][0] << ", " << BS_ver[i][1] << ", " << BS_ver[i][2] << ", " << BS_ver[i][3] << "];" << endl;
	}
	
	myfile.close();
}

void Record_Grad(double** Grad, int N, char* filename)
{
	ofstream myfile;
	myfile.open(filename, ios::app);
	
	myfile<<"V_grad = zeros(" << N << ", 3);";
	for (int i=0; i<N; i++) {
		myfile<<"V_grad("<<i+1<<", :) = ["<<Grad[i][0]<<", "<<Grad[i][1] <<", "<<Grad[i][2] << "];"<<endl;
	}
	
	myfile.close();
}

void Record_Init(int Nx, int Ny, int Nv, int Nc, double* P1,  double* P2, double* P3, char* filename)
{		
	ofstream file;
	file.open(filename, ios::out);
	
	file<<"Nx = "<<Nx<<";"<<endl;
	file<<"Ny = "<<Ny<<";"<<endl;
	file<<"Nv = "<<Nv<<";"<<endl;
	file<<"Nc = "<<Nc<<";"<<endl;
	file<<"K_apical = "<<P1[0]<<";"<<endl;
	file<<"K_basal = "<<P1[1]<<";"<<endl;
	file<<"K_volume = "<<P1[2]<<";"<<endl;
	file<<"K_length = "<<P1[3]<<";"<<endl;
	file<<"K_yolk = "<<P3[5]<<";"<<endl;
	file<<"A_0 = "<<P2[0]<<";"<<endl;
	file<<"A_0_c = "<<P3[4]<<";"<<endl;
	file<<"B_0 = "<<P2[1]<<";"<<endl;
	file<<"V_0 = "<<P2[2]<<";"<<endl;
	file<<"L_0 = "<<P2[3]<<";"<<endl;
	file<<"Y_0 = "<<P3[6]<<";"<<endl;
	file<<"y_start = "<<P3[0]<<";"<<endl;
	file<<"y_stop = "<<P3[1]<<";"<<endl;
	file<<"x_start = "<<P3[2]<<";"<<endl;
	file<<"x_stop = "<<P3[3]<<";"<<endl;
	
	file.close();
}

// This is for initialization and calculation

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

int Cell_Id_P(int i, int j, int Nx){
	return i + Nx*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;
	
}


double E_from_cell_V2(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;
	
	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], 2);
	Energy_Apical = K_P[0]*pow(Apical_Area - O_P[0], 2);
	Energy_Basal = K_P[1]*pow(Basal_Area - O_P[1], 2);
	
	Energy_Side = K_P[3]*pow(Norm(S0) - O_P[3], 2);
	Energy_Side += K_P[4]*pow(Norm(S1) - O_P[4], 2);
	Energy_Side += K_P[5]*pow(Norm(S2) - O_P[5], 2);
	Energy_Side += K_P[6]*pow(Norm(S3) - O_P[6], 2);
	
	delete[] S0;
	delete[] S1;
	delete[] S2;
	delete[] S3;
	
	return Energy_Volume + Energy_Apical + Energy_Basal + Energy_Side;
}

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], 2);
	Energy_Apical = K_P[0]*pow(Apical_Area - O_P[0], 2);
	Energy_Basal = K_P[1]*pow(Basal_Area - O_P[1], 2);
	
	Energy_Side = K_P[3]*pow(Norm(S0) - O_P[3], 2);
	Energy_Side += K_P[4]*pow(Norm(S1) - O_P[4], 2);
	Energy_Side += K_P[5]*pow(Norm(S2) - O_P[5], 2);
	Energy_Side += K_P[6]*pow(Norm(S3) - O_P[6], 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], 2);
	Energy_Diag += K_P[7]*pow(Norm(S1) - O_P[7], 2);
	Energy_Diag += K_P[7]*pow(Norm(S2) - O_P[7], 2);
	Energy_Diag += K_P[7]*pow(Norm(S3) - O_P[7], 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 E_from_cell_circ(double** AP, double** BS, double** P){
	
	int i=0;
	
	double* P1 = new double[3];
	double* P2 = new double[3];
	double* P3 = new double[3];
	double* P4 = new double[3];
	double* P5 = new double[3];
	double* P6 = new double[3];
	
	// Initialize vectors
	double* B1 = new double[3];
	double* B2 = new double[3];
	double* B3 = new double[3];
	double* B4 = new double[3];
	
	double* A = new double[3];
	
	double* S1 = new double[3];
	double* S2 = new double[3];
	double* S3 = new double[3];
	double* S4 = new double[3];
	
	// Initialize the cross product vectors
	double* C1 = new double[3];
	double* C2 = new double[3];
	double* C3 = new double[3];
	
	double Volume;
	double Area_apical;
	double Area_basal;
	
	double E_apical = 0;
	double E_basal = 0;
	double E_volume = 0;
	
	//************************************************************************************************
	// Assing points for the basal part
	Volume = 0; Area_basal = 0; Area_apical = 0;
	for (i=0; i<3; i++) {
		P1[i] = BS[0][i];
		P2[i] = BS[1][i];
		P3[i] = BS[2][i];
		P4[i] = BS[3][i];
		P5[i] = AP[1][i];
		P6[i] = AP[3][i];
	}
	
	// Assing Vectors
	for (i=0; i<3; i++) {
		B1[i] = P1[i] - P2[i]; B2[i] = P4[i] - P2[i]; B3[i] = P1[i] - P3[i]; B4[i] = P4[i] - P3[i];
		A[i] = P5[i] - P6[i];
		S1[i] = P5[i] - P2[i]; S2[i] = P4[i] - P6[i]; S3[i] = P1[i] - P6[i]; S4[i] = P6[i] - P3[i];
	}
	
	// Calculate Cross products
	for (i=0; i<3; i++) {
		C1[i] = Cross_Product(B1, B2, i); C2[i] = Cross_Product(S2, A, i); C3[i] = Cross_Product(B3, B4, i);
	}
	
	// Calculate Volume of basal part of cell
	Volume += fabs(Dot(S1, C1)) + fabs(Dot(S3, C2)) + fabs(Dot(S4, C3));
	Area_basal += Norm(C1)/2.0 + Norm(C3)/2.0;
	
	// Assing points for the apical part
	for (i=0; i<3; i++) {
		P1[i] = AP[1][i];
		P2[i] = AP[0][i];
		P3[i] = AP[3][i];
		P4[i] = AP[2][i];
		P5[i] = BS[0][i];
		P6[i] = BS[2][i];
	}
	
	// Assing Vectors
	for (i=0; i<3; i++) {
		B1[i] = P1[i] - P2[i]; B2[i] = P4[i] - P2[i]; B3[i] = P1[i] - P3[i]; B4[i] = P4[i] - P3[i];
		A[i] = P5[i] - P6[i];
		S1[i] = P5[i] - P2[i]; S2[i] = P4[i] - P6[i]; S3[i] = P1[i] - P6[i]; S4[i] = P6[i] - P3[i];
	}
	
	// Calculate Cross products
	for (i=0; i<3; i++) {
		C1[i] = Cross_Product(B1, B2, i); C2[i] = Cross_Product(S2, A, i); C3[i] = Cross_Product(B3, B4, i);
	}
	
	// Calculate Volume of apical part of cell
	Volume += fabs(Dot(S1, C1)) + fabs(Dot(S3, C2)) + fabs(Dot(S4, C3));
	Volume /= 6.0;
	E_volume += P[1][0]*(Volume - P[1][1])*(Volume - P[1][1]);
	
	Area_apical += Norm(C1)/2.0 + Norm(C3)/2.0;
	E_apical += P[0][0]*(Area_apical - P[0][1])*(Area_apical - P[0][1]);
	E_basal += P[2][0]*(Area_basal - P[2][1])*(Area_basal - P[2][1]);
	
	//************************************************************************************************
	// Now rotate the verticies (1)
	//************************************************************************************************
	// Assing points for the basal part
	Volume = 0; Area_basal = 0; Area_apical = 0;
	for (i=0; i<3; i++) {
		P1[i] = BS[2][i];
		P2[i] = BS[0][i];
		P3[i] = BS[3][i];
		P4[i] = BS[1][i];
		P5[i] = AP[0][i];
		P6[i] = AP[1][i];
	}
	
	// Assing Vectors
	for (i=0; i<3; i++) {
		B1[i] = P1[i] - P2[i]; B2[i] = P4[i] - P2[i]; B3[i] = P1[i] - P3[i]; B4[i] = P4[i] - P3[i];
		A[i] = P5[i] - P6[i];
		S1[i] = P5[i] - P2[i]; S2[i] = P4[i] - P6[i]; S3[i] = P1[i] - P6[i]; S4[i] = P6[i] - P3[i];
	}
	
	// Calculate Cross products
	for (i=0; i<3; i++) {
		C1[i] = Cross_Product(B1, B2, i); C2[i] = Cross_Product(S2, A, i); C3[i] = Cross_Product(B3, B4, i);
	}
	
	// Calculate Volume of basal part of cell
	Volume += fabs(Dot(S1, C1)) + fabs(Dot(S3, C2)) + fabs(Dot(S4, C3));
	Area_basal += Norm(C1)/2.0 + Norm(C3)/2.0;
	
	// Assing points for the apical part
	for (i=0; i<3; i++) {
		P1[i] = AP[0][i];
		P2[i] = AP[2][i];
		P3[i] = AP[1][i];
		P4[i] = AP[3][i];
		P5[i] = BS[2][i];
		P6[i] = BS[3][i];
	}
	
	// Assing Vectors
	for (i=0; i<3; i++) {
		B1[i] = P1[i] - P2[i]; B2[i] = P4[i] - P2[i]; B3[i] = P1[i] - P3[i]; B4[i] = P4[i] - P3[i];
		A[i] = P5[i] - P6[i];
		S1[i] = P5[i] - P2[i]; S2[i] = P4[i] - P6[i]; S3[i] = P1[i] - P6[i]; S4[i] = P6[i] - P3[i];
	}
	
	// Calculate Cross products
	for (i=0; i<3; i++) {
		C1[i] = Cross_Product(B1, B2, i); C2[i] = Cross_Product(S2, A, i); C3[i] = Cross_Product(B3, B4, i);
	}
	
	// Calculate Volume of apical part of cell
	Volume += fabs(Dot(S1, C1)) + fabs(Dot(S3, C2)) + fabs(Dot(S4, C3));
	Volume /= 6.0;
	E_volume += P[1][0]*(Volume - P[1][1])*(Volume - P[1][1]);
	
	Area_apical += Norm(C1)/2.0 + Norm(C3)/2.0;
	E_apical += P[0][0]*(Area_apical - P[0][1])*(Area_apical - P[0][1]);
	E_basal += P[2][0]*(Area_basal - P[2][1])*(Area_basal - P[2][1]);
	
	//************************************************************************************************
	// Now rotate the verticies again (2)
	//************************************************************************************************
	// Assing points for the basal part
	Volume = 0;
	for (i=0; i<3; i++) {
		P1[i] = BS[3][i];
		P2[i] = BS[2][i];
		P3[i] = BS[1][i];
		P4[i] = BS[0][i];
		P5[i] = AP[2][i];
		P6[i] = AP[0][i];
	}
	
	// Assing Vectors
	for (i=0; i<3; i++) {
		B1[i] = P1[i] - P2[i]; B2[i] = P4[i] - P2[i]; B3[i] = P1[i] - P3[i]; B4[i] = P4[i] - P3[i];
		A[i] = P5[i] - P6[i];
		S1[i] = P5[i] - P2[i]; S2[i] = P4[i] - P6[i]; S3[i] = P1[i] - P6[i]; S4[i] = P6[i] - P3[i];
	}
	
	// Calculate Cross products
	for (i=0; i<3; i++) {
		C1[i] = Cross_Product(B1, B2, i); C2[i] = Cross_Product(S2, A, i); C3[i] = Cross_Product(B3, B4, i);
	}
	
	// Calculate Volume of basal part of cell
	Volume += fabs(Dot(S1, C1)) + fabs(Dot(S3, C2)) + fabs(Dot(S4, C3));
	
	// Assing points for the apical part
	for (i=0; i<3; i++) {
		P1[i] = AP[2][i];
		P2[i] = AP[3][i];
		P3[i] = AP[0][i];
		P4[i] = AP[1][i];
		P5[i] = BS[3][i];
		P6[i] = BS[1][i];
	}
	
	// Assing Vectors
	for (i=0; i<3; i++) {
		B1[i] = P1[i] - P2[i]; B2[i] = P4[i] - P2[i]; B3[i] = P1[i] - P3[i]; B4[i] = P4[i] - P3[i];
		A[i] = P5[i] - P6[i];
		S1[i] = P5[i] - P2[i]; S2[i] = P4[i] - P6[i]; S3[i] = P1[i] - P6[i]; S4[i] = P6[i] - P3[i];
	}
	
	// Calculate Cross products
	for (i=0; i<3; i++) {
		C1[i] = Cross_Product(B1, B2, i); C2[i] = Cross_Product(S2, A, i); C3[i] = Cross_Product(B3, B4, i);
	}
	
	// Calculate Volume of apical part of cell
	Volume += fabs(Dot(S1, C1)) + fabs(Dot(S3, C2)) + fabs(Dot(S4, C3));
	Volume /= 6.0;
	E_volume += P[1][0]*(Volume - P[1][1])*(Volume - P[1][1]);
	
	//************************************************************************************************
	// Now rotate the verticies again (3)
	//************************************************************************************************
	// Assing points for the basal part
	Volume = 0;
	for (i=0; i<3; i++) {
		P1[i] = BS[1][i];
		P2[i] = BS[2][i];
		P3[i] = BS[0][i];
		P4[i] = BS[2][i];
		P5[i] = AP[3][i];
		P6[i] = AP[2][i];
	}
	
	// Assing Vectors
	for (i=0; i<3; i++) {
		B1[i] = P1[i] - P2[i]; B2[i] = P4[i] - P2[i]; B3[i] = P1[i] - P3[i]; B4[i] = P4[i] - P3[i];
		A[i] = P5[i] - P6[i];
		S1[i] = P5[i] - P2[i]; S2[i] = P4[i] - P6[i]; S3[i] = P1[i] - P6[i]; S4[i] = P6[i] - P3[i];
	}
	
	// Calculate Cross products
	for (i=0; i<3; i++) {
		C1[i] = Cross_Product(B1, B2, i); C2[i] = Cross_Product(S2, A, i); C3[i] = Cross_Product(B3, B4, i);
	}
	
	// Calculate Volume of basal part of cell
	Volume += fabs(Dot(S1, C1)) + fabs(Dot(S3, C2)) + fabs(Dot(S4, C3));
	
	// Assing points for the apical part
	for (i=0; i<3; i++) {
		P1[i] = AP[3][i];
		P2[i] = AP[1][i];
		P3[i] = AP[2][i];
		P4[i] = AP[0][i];
		P5[i] = BS[1][i];
		P6[i] = BS[0][i];
	}
	
	// Assing Vectors
	for (i=0; i<3; i++) {
		B1[i] = P1[i] - P2[i]; B2[i] = P4[i] - P2[i]; B3[i] = P1[i] - P3[i]; B4[i] = P4[i] - P3[i];
		A[i] = P5[i] - P6[i];
		S1[i] = P5[i] - P2[i]; S2[i] = P4[i] - P6[i]; S3[i] = P1[i] - P6[i]; S4[i] = P6[i] - P3[i];
	}
	
	// Calculate Cross products
	for (i=0; i<3; i++) {
		C1[i] = Cross_Product(B1, B2, i); C2[i] = Cross_Product(S2, A, i); C3[i] = Cross_Product(B3, B4, i);
	}
	
	// Calculate Volume of apical part of cell
	Volume += fabs(Dot(S1, C1)) + fabs(Dot(S3, C2)) + fabs(Dot(S4, C3));
	Volume /= 6.0;
	E_volume += P[1][0]*(Volume - P[1][1])*(Volume - P[1][1]);
	
	//************************************************************************************************
	//************************************************************************************************
	// Calculate energy from sides
	for (i=0; i<3; i++) {
		S1[i] = AP[0][i] - BS[0][i];
		S2[i] = AP[1][i] - BS[1][i];
		S3[i] = AP[2][i] - BS[2][i];
		S4[i] = AP[3][i] - BS[3][i];
	}
	
	double E_side = 0;
	E_side = P[3][0]*P[4][0]*(Norm(S1) - P[3][1])*(Norm(S1) - P[3][1]);
	E_side += P[3][0]*P[4][1]*(Norm(S2) - P[3][1])*(Norm(S2) - P[3][1]);
	E_side += P[3][0]*P[5][0]*(Norm(S3) - P[3][1])*(Norm(S3) - P[3][1]);
	E_side += P[3][0]*P[5][1]*(Norm(S4) - P[3][1])*(Norm(S4) - P[3][1]);
	
	// Calculate energy from diagonals
	for (i=0; i<3; i++) {
		S1[i] = AP[3][i] - BS[0][i];
		S2[i] = AP[2][i] - BS[1][i];
		S3[i] = AP[1][i] - BS[2][i];
		S4[i] = AP[0][i] - BS[3][i];
	}
	
	E_side += P[6][0]*(Norm(S1) - P[6][1])*(Norm(S1) - P[6][1]);
	E_side += P[6][0]*(Norm(S2) - P[6][1])*(Norm(S2) - P[6][1]);
	E_side += P[6][0]*(Norm(S3) - P[6][1])*(Norm(S3) - P[6][1]);
	E_side += P[6][0]*(Norm(S4) - P[6][1])*(Norm(S4) - P[6][1]);
	
	//************************************************************************************************
	
	
	delete P1;
	delete P2;
	delete P3;
	delete P4;
	delete P5;
	delete P6;
	
	delete B1;
	delete B2;
	delete B3;
	delete B4;
	
	delete A;
	
	delete S1;
	delete S2;
	delete S3;
	delete S4;
	
	delete C1;
	delete C2;
	delete C3;
	
	return E_side + E_apical + E_basal + E_volume;
	
}


double E_from_cell_center(double** AP, double** BS, double** P){
	
	int i=0;
	
	double* C = new double[3];
	
	double* P1 = new double[3];
	double* P2 = new double[3];
	double* P3 = new double[3];
	
	
	double Volume;
	double Area_apical;
	double Area_basal;
	
	double E_apical = 0;
	double E_basal = 0;
	double E_volume = 0;
	
	// Calculate the center
	for (i=0; i<3; i++) {
		C[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
	Volume = 0;
	
	//&&&&&&&&&&&&&& Do top
	for (i=0; i<3; i++) {
		P1[i] = AP[0][i];
		P2[i] = AP[1][i];
		P3[i] = AP[2][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = AP[1][i];
		P2[i] = AP[2][i];
		P3[i] = AP[3][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = AP[2][i];
		P2[i] = AP[3][i];
		P3[i] = AP[0][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = AP[3][i];
		P2[i] = AP[0][i];
		P3[i] = AP[1][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	//%%%%%%%%%%%%%%%% Do bottom

	for (i=0; i<3; i++) {
		P1[i] = BS[0][i];
		P2[i] = BS[1][i];
		P3[i] = BS[2][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[1][i];
		P2[i] = BS[2][i];
		P3[i] = BS[3][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[2][i];
		P2[i] = BS[3][i];
		P3[i] = BS[0][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[3][i];
		P2[i] = BS[0][i];
		P3[i] = BS[1][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	//%%%%%%%%%%%%%%%% SIDE 1
	
	for (i=0; i<3; i++) {
		P1[i] = AP[0][i];
		P2[i] = AP[1][i];
		P3[i] = BS[1][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = AP[1][i];
		P2[i] = BS[1][i];
		P3[i] = BS[0][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[1][i];
		P2[i] = BS[0][i];
		P3[i] = AP[0][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[0][i];
		P2[i] = AP[0][i];
		P3[i] = AP[1][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	//%%%%%%%%%%%%%%%% SIDE 2
	
	for (i=0; i<3; i++) {
		P1[i] = AP[1][i];
		P2[i] = AP[2][i];
		P3[i] = BS[2][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = AP[2][i];
		P2[i] = BS[2][i];
		P3[i] = BS[1][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[2][i];
		P2[i] = BS[1][i];
		P3[i] = AP[1][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[1][i];
		P2[i] = AP[1][i];
		P3[i] = AP[2][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);	
	//%%%%%%%%%%%%%%%% SIDE 3
	
	for (i=0; i<3; i++) {
		P1[i] = AP[2][i];
		P2[i] = AP[3][i];
		P3[i] = BS[3][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = AP[3][i];
		P2[i] = BS[3][i];
		P3[i] = BS[2][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[3][i];
		P2[i] = BS[2][i];
		P3[i] = AP[2][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[2][i];
		P2[i] = AP[2][i];
		P3[i] = AP[3][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	//%%%%%%%%%%%%%%%% SIDE 4
	
	for (i=0; i<3; i++) {
		P1[i] = AP[3][i];
		P2[i] = AP[0][i];
		P3[i] = BS[0][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = AP[0][i];
		P2[i] = BS[0][i];
		P3[i] = BS[3][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[0][i];
		P2[i] = BS[3][i];
		P3[i] = AP[3][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	for (i=0; i<3; i++) {
		P1[i] = BS[3][i];
		P2[i] = AP[3][i];
		P3[i] = AP[0][i];
	}
	
	Volume += Volume_triplet(P1, P2, P3, C);
	
	Volume /= 12;
	E_volume = P[1][0]*(Volume - P[1][1])*(Volume - P[1][1]);
	
	//************************************************************************************************
	// Calculate Apical and basal areas
	// %%%%%%%%% Apical 
	Area_apical = 0;
	for (i=0; i<3; i++) {
		P1[i] = AP[1][i] - AP[0][i]; P2[i] = AP[2][i] - AP[0][i];
	}
	
	for (i=0; i<3; i++) {
		P3[i] = Cross_Product(P1, P2, i);
	}
	
	Area_apical += Norm(P3)/2.0;
	
	for (i=0; i<3; i++) {
		P1[i] = AP[1][i] - AP[3][i]; P2[i] = AP[2][i] - AP[3][i];
	}
	
	for (i=0; i<3; i++) {
		P3[i] = Cross_Product(P1, P2, i);
	}
	
	Area_apical += Norm(P3)/2.0;
	
	for (i=0; i<3; i++) {
		P1[i] = AP[1][i] - AP[2][i]; P2[i] = AP[3][i] - AP[2][i];
	}
	
	for (i=0; i<3; i++) {
		P3[i] = Cross_Product(P1, P2, i);
	}
	
	Area_apical += Norm(P3)/2.0;
	
	for (i=0; i<3; i++) {
		P1[i] = AP[1][i] - AP[1][i]; P2[i] = AP[3][i] - AP[1][i];
	}
	
	for (i=0; i<3; i++) {
		P3[i] = Cross_Product(P1, P2, i);
	}
	
	Area_apical += Norm(P3)/2.0;
	Area_apical /= 2;
	E_apical = P[0][0]*(Area_apical - P[0][1])*(Area_apical - P[0][1]);
	
	// %%%%%%%%% Basal
	Area_basal = 0;
	for (i=0; i<3; i++) {
		P1[i] = BS[1][i] - BS[0][i]; P2[i] = BS[2][i] - BS[0][i];
	}
	
	for (i=0; i<3; i++) {
		P3[i] = Cross_Product(P1, P2, i);
	}
	
	Area_basal += Norm(P3)/2.0;
	
	for (i=0; i<3; i++) {
		P1[i] = BS[1][i] - BS[3][i]; P2[i] = BS[2][i] - BS[3][i];
	}
	
	for (i=0; i<3; i++) {
		P3[i] = Cross_Product(P1, P2, i);
	}
	
	Area_basal += Norm(P3)/2.0;
	
	for (i=0; i<3; i++) {
		P1[i] = BS[1][i] - BS[2][i]; P2[i] = BS[3][i] - BS[2][i];
	}
	
	for (i=0; i<3; i++) {
		P3[i] = Cross_Product(P1, P2, i);
	}
	
	Area_basal += Norm(P3)/2.0;
	
	for (i=0; i<3; i++) {
		P1[i] = BS[1][i] - BS[1][i]; P2[i] = BS[3][i] - BS[1][i];
	}
	
	for (i=0; i<3; i++) {
		P3[i] = Cross_Product(P1, P2, i);
	}
	
	Area_basal += Norm(P3)/2.0;
	Area_basal /= 2;
	E_basal = P[2][0]*(Area_basal - P[2][1])*(Area_basal - P[2][1]);
	
	//************************************************************************************************
	// Calculate energy from sides
	for (i=0; i<3; i++) {
		P1[i] = AP[0][i] - BS[0][i];
		P2[i] = AP[1][i] - BS[1][i];
		P3[i] = AP[2][i] - BS[2][i];
		C[i] = AP[3][i] - BS[3][i];
	}
	
	double E_side = 0;
	E_side = P[3][0]*P[4][0]*(Norm(P1) - P[3][1])*(Norm(P1) - P[3][1]);
	E_side += P[3][0]*P[4][1]*(Norm(P2) - P[3][1])*(Norm(P2) - P[3][1]);
	E_side += P[3][0]*P[5][0]*(Norm(P3) - P[3][1])*(Norm(P3) - P[3][1]);
	E_side += P[3][0]*P[5][1]*(Norm(C) - P[3][1])*(Norm(C) - P[3][1]);
	
	// Calculate energy from diagonals
	for (i=0; i<3; i++) {
		P1[i] = AP[3][i] - BS[0][i];
		P2[i] = AP[2][i] - BS[1][i];
		P3[i] = AP[1][i] - BS[2][i];
		C[i] = AP[0][i] - BS[3][i];
	}
	
	E_side += P[6][0]*(Norm(P1) - P[6][1])*(Norm(P1) - P[6][1]);
	E_side += P[6][0]*(Norm(P2) - P[6][1])*(Norm(P2) - P[6][1]);
	E_side += P[6][0]*(Norm(P3) - P[6][1])*(Norm(P3) - P[6][1]);
	E_side += P[6][0]*(Norm(C) - P[6][1])*(Norm(C) - P[6][1]);
	
	//************************************************************************************************
	
	
	delete P1;
	delete P2;
	delete P3;
	delete C;
	
	return E_side + E_apical + E_basal + E_volume;
	
}

double E_from_cell(double** AP, double** BS, double** P){
	
	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));
	
	// 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;
	
	Volume /= 6.0;
	Area_apical /= 2.0;
	Area_basal /= 2.0;
	
	// Calculate side edges
	
	double E_side = 0;
	E_side = P[3][0]*P[4][0]*(Norm(Sa0_b0) - P[3][1])*(Norm(Sa0_b0) - P[3][1]);
	E_side += P[3][0]*P[4][1]*(Norm(Sa1_b1) - P[3][1])*(Norm(Sa1_b1) - P[3][1]);
	E_side += P[3][0]*P[5][0]*(Norm(Sb2_a2) - P[3][1])*(Norm(Sb2_a2) - P[3][1]);
	E_side += P[3][0]*P[5][1]*(Norm(Sb3_a3) - P[3][1])*(Norm(Sb3_a3) - P[3][1]);
	
	
	double E_apical = P[0][0]*(Area_apical - P[0][1])*(Area_apical - P[0][1]);
	
	double E_basal = P[2][0]*(Area_basal - P[2][1])*(Area_basal - P[2][1]);
	
	double E_volume = P[1][0]*(Volume - P[1][1])*(Volume - P[1][1]);
	
	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 E_side + E_apical + E_basal + E_volume;
	
}
*/
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;	
}


