/* vim: set ts=4 sts=4 sw=4 noet : */
#include<stdlib.h>
#include "general.h"
#include "energy.h"
#include "vertex.h"
#include "bond.h"
#include<math.h>
#include<stdio.h>
#include <gsl/gsl_vector_complex.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_eigen.h>
/** @brief Wrapper that calculates energy of every vertex in vesicle
 *  
 *  Function calculated energy of every vertex in vesicle. It can be used in
 *  initialization procedure or in recalculation of the energy after non-MCsweep *  operations. However, when random move of vertex or flip of random bond occur *  call to this function is not necessary nor recommended. 
 *  @param *vesicle is a pointer to vesicle.
 *  @returns TS_SUCCESS on success.
*/
ts_bool mean_curvature_and_energy(ts_vesicle *vesicle){

    ts_uint i;
    
    ts_vertex_list *vlist=vesicle->vlist;
    ts_vertex **vtx=vlist->vtx;

    for(i=0;i<vlist->n;i++){
        energy_vertex(vtx[i]);
        
    }

    return TS_SUCCESS;
}

/** @brief Calculate energy of a bond (in models where energy is bond related)
 *
 *  This function is experimental and currently only used in polymeres calculation (PEGs or polymeres inside the vesicle).
 *
 *  @param *bond is a pointer to a bond between two vertices in polymere
 *  @param *poly is a pointer to polymere in which we calculate te energy of the bond
 *  @returns TS_SUCCESS on successful calculation
*/
inline ts_bool bond_energy(ts_bond *bond,ts_poly *poly){
//TODO: This value to be changed and implemented in data structure:
	ts_double d_relaxed=1.0;
	bond->energy=poly->k*pow(bond->bond_length-d_relaxed,2);
	return TS_SUCCESS;
};

/** @brief Calculation of the bending energy of the vertex.
 *  
 *  Main function that calculates energy of the vertex \f$i\f$. Function returns \f$\frac{1}{2}(c_1+c_2-c)^2 s\f$, where \f$(c_1+c_2)/2\f$ is mean curvature,
 * \f$c/2\f$ is spontaneous curvature and \f$s\f$ is area per vertex \f$i\f$.
 *
 * Nearest neighbors (NN) must be ordered in counterclockwise direction for this function to work.
 *  Firstly NNs that form two neighboring triangles are found (\f$j_m\f$, \f$j_p\f$ and common \f$j\f$). Later, the scalar product of vectors \f$x_1=(\mathbf{i}-\mathbf{j_p})\cdot (\mathbf{i}-\mathbf{j_p})(\mathbf{i}-\mathbf{j_p})\f$, \f$x_2=(\mathbf{j}-\mathbf{j_p})\cdot  (\mathbf{j}-\mathbf{j_p})\f$  and \f$x_3=(\mathbf{j}-\mathbf{j_p})\cdot (\mathbf{i}-\mathbf{j_p})\f$  are calculated. From these three vectors the \f$c_{tp}=\frac{1}{\tan(\varphi_p)}\f$ is calculated, where \f$\varphi_p\f$ is the inner angle at vertex \f$j_p\f$. The procedure is repeated for \f$j_m\f$ instead of \f$j_p\f$ resulting in \f$c_{tn}\f$.
 *  
\begin{tikzpicture}{
\coordinate[label=below:$i$] (i) at (2,0);
\coordinate[label=left:$j_m$] (jm) at (0,3.7);
\coordinate[label=above:$j$] (j) at (2.5,6.4);
\coordinate[label=right:$j_p$] (jp) at (4,2.7);

\draw (i) -- (jm) -- (j) -- (jp) -- (i) -- (j);

\begin{scope}
\path[clip] (jm)--(i)--(j);
\draw (jm) circle (0.8);
\node[right] at (jm) {$\varphi_m$};
\end{scope}

\begin{scope}
\path[clip] (jp)--(i)--(j);
\draw (jp) circle (0.8);
\node[left] at (jp) {$\varphi_p$};
\end{scope}

%%vertices
\draw [fill=gray] (i) circle (0.1);
\draw [fill=white] (j) circle (0.1);
\draw [fill=white] (jp) circle (0.1);
\draw [fill=white] (jm) circle (0.1);
%\node[draw,circle,fill=white] at (i) {};
\end{tikzpicture}

 * The curvature is then calculated as \f$\mathbf{h}=\frac{1}{2}\Sigma_{k=0}^{\mathrm{neigh\_no}} c_{tp}^{(k)}+c_{tm}^{(k)} (\mathbf{j_k}-\mathbf{i})\f$, where \f$c_{tp}^{(k)}+c_{tm}^k=2\sigma^{(k)}\f$ (length in dual lattice?) and the previous equation can be written as \f$\mathbf{h}=\Sigma_{k=0}^{\mathrm{neigh\_no}}\sigma^{(k)}\cdot(\mathbf{j}-\mathbf{i})\f$ (See Kroll, p. 384, eq 70).
 *
 * From the curvature the enery is calculated by equation \f$E=\frac{1}{2}\mathbf{h}\cdot\mathbf{h}\f$.
 * @param *vtx is a pointer to vertex at which we want to calculate the energy
 * @returns TS_SUCCESS on successful calculation.
*/
inline ts_bool energy_vertex(ts_vertex *vtx){
    ts_uint jj, i, j;
    ts_double edge_vector_x[7]={0,0,0,0,0,0,0};
    ts_double edge_vector_y[7]={0,0,0,0,0,0,0};
    ts_double edge_vector_z[7]={0,0,0,0,0,0,0};
    ts_double edge_normal_x[7]={0,0,0,0,0,0,0};
    ts_double edge_normal_y[7]={0,0,0,0,0,0,0};
    ts_double edge_normal_z[7]={0,0,0,0,0,0,0};
    ts_double edge_binormal_x[7]={0,0,0,0,0,0,0};
    ts_double edge_binormal_y[7]={0,0,0,0,0,0,0};
    ts_double edge_binormal_z[7]={0,0,0,0,0,0,0};
    ts_double vertex_normal_x=0.0;
    ts_double vertex_normal_y=0.0;
    ts_double vertex_normal_z=0.0;
//    ts_triangle *triedge[2]={NULL,NULL};

    ts_uint nei,neip,neim;
    ts_vertex *it, *k, *kp,*km;
    ts_triangle *lm=NULL, *lp=NULL;
    ts_double sumnorm;
    ts_double temp_length;


    ts_double Se11, Se21, Se22, Se31, Se32, Se33;
    ts_double Pv11, Pv21, Pv22, Pv31, Pv32, Pv33;
    ts_double We;
    ts_double Av, We_Av;

	gsl_matrix *gsl_Sv=gsl_matrix_alloc(3,3);
	gsl_vector_complex *Sv_eigen=gsl_vector_complex_alloc(3);
	gsl_eigen_nonsymm_workspace *workspace=gsl_eigen_nonsymm_alloc(3);

	ts_double mprod[7], phi[7], he[7];
	ts_double Sv[3][3]={{0,0,0},{0,0,0},{0,0,0}};
    // Here edge vector is calculated
//    fprintf(stderr, "Vertex has neighbours=%d\n", vtx->neigh_no);




	Av=0;
	for(i=0; i<vtx->tristar_no; i++){
		vertex_normal_x=vertex_normal_x + vtx->tristar[i]->xnorm*vtx->tristar[i]->area;
		vertex_normal_y=vertex_normal_y + vtx->tristar[i]->ynorm*vtx->tristar[i]->area;
		vertex_normal_z=vertex_normal_z + vtx->tristar[i]->znorm*vtx->tristar[i]->area;
		Av+=vtx->tristar[i]->area/3;
	}
	temp_length=sqrt(pow(vertex_normal_x,2)+pow(vertex_normal_y,2)+pow(vertex_normal_z,2));
	vertex_normal_x=vertex_normal_x/temp_length;
	vertex_normal_y=vertex_normal_y/temp_length;
	vertex_normal_z=vertex_normal_z/temp_length;

	Pv11=1-vertex_normal_x*vertex_normal_x;
	Pv22=1-vertex_normal_y*vertex_normal_y;
	Pv33=1-vertex_normal_z*vertex_normal_z;
	Pv21=vertex_normal_x*vertex_normal_y;
	Pv31=vertex_normal_x*vertex_normal_z;
	Pv32=vertex_normal_y*vertex_normal_z;




    for(jj=0;jj<vtx->neigh_no;jj++){
	edge_vector_x[jj]=vtx->neigh[jj]->x-vtx->x;
	edge_vector_y[jj]=vtx->neigh[jj]->y-vtx->y;
	edge_vector_z[jj]=vtx->neigh[jj]->z-vtx->z;

	//Here we calculate normalized edge vector

	temp_length=sqrt(edge_vector_x[jj]*edge_vector_x[jj]+edge_vector_y[jj]*edge_vector_y[jj]+edge_vector_z[jj]*edge_vector_z[jj]);
	edge_vector_x[jj]=edge_vector_x[jj]/temp_length;
	edge_vector_y[jj]=edge_vector_y[jj]/temp_length;
	edge_vector_z[jj]=edge_vector_z[jj]/temp_length;

	//end normalization
//	printf("(%f %f %f)\n", vertex_normal_x, vertex_normal_y, vertex_normal_z);


	it=vtx;
	k=vtx->neigh[jj];
	nei=0;
    	for(i=0;i<it->neigh_no;i++){ // Finds the nn of it, that is k 
        	if(it->neigh[i]==k){
            	nei=i;
            	break;
        	}
    	}
    	neip=nei+1;  // I don't like it.. Smells like I must have it in correct order
    	neim=nei-1;
    	if(neip>=it->neigh_no) neip=0;
    	if((ts_int)neim<0) neim=it->neigh_no-1; /* casting is essential... If not
there the neim is never <0 !!! */
  //  fprintf(stderr,"The numbers are: %u %u\n",neip, neim);
    	km=it->neigh[neim];  // We located km and kp
    	kp=it->neigh[neip];

    	if(km==NULL || kp==NULL){
        	fatal("energy_vertex: cannot determine km and kp!",233);
    	}

   for(i=0;i<it->tristar_no;i++){
        for(j=0;j<k->tristar_no;j++){
            if((it->tristar[i] == k->tristar[j])){ //ce gre za skupen trikotnik
                if((it->tristar[i]->vertex[0] == km || it->tristar[i]->vertex[1]
== km || it->tristar[i]->vertex[2]== km )){
                lm=it->tristar[i];
         //       lmidx=i;
                }
                else
                {
                lp=it->tristar[i];
         //       lpidx=i;
                }

            }
        }
    }
if(lm==NULL || lp==NULL) fatal("energy_vertex: Cannot find triangles lm and lp!",233);

	//Triangle normals are NORMALIZED!

	sumnorm=sqrt( pow((lm->xnorm + lp->xnorm),2) + pow((lm->ynorm + lp->ynorm), 2) + pow((lm->znorm + lp->znorm), 2));

	edge_normal_x[jj]=(lm->xnorm+ lp->xnorm)/sumnorm;
	edge_normal_y[jj]=(lm->ynorm+ lp->ynorm)/sumnorm;
	edge_normal_z[jj]=(lm->znorm+ lp->znorm)/sumnorm;


	edge_binormal_x[jj]=(edge_normal_y[jj]*edge_vector_z[jj])-(edge_normal_z[jj]*edge_vector_y[jj]);
	edge_binormal_y[jj]=-(edge_normal_x[jj]*edge_vector_z[jj])+(edge_normal_z[jj]*edge_vector_x[jj]);
	edge_binormal_z[jj]=(edge_normal_x[jj]*edge_vector_y[jj])-(edge_normal_y[jj]*edge_vector_x[jj]);


	mprod[jj]=it->x*(k->y*edge_vector_z[jj]-edge_vector_y[jj]*k->z)-it->y*(k->x*edge_vector_z[jj]-k->z*edge_vector_x[jj])+it->z*(k->x*edge_vector_y[jj]-k->y*edge_vector_x[jj]);
	phi[jj]=copysign(acos(lm->xnorm*lp->xnorm+lm->ynorm*lp->ynorm+lm->znorm*lp->znorm-1e-8),mprod[jj])+M_PI;
//	printf("ACOS arg=%e\n", lm->xnorm*lp->xnorm+lm->ynorm*lp->ynorm+lm->znorm*lp->znorm);
	//he was multiplied with 2 before...
	he[jj]=sqrt( pow((edge_vector_x[jj]),2) + pow((edge_vector_y[jj]), 2) + pow((edge_vector_z[jj]), 2))*cos(phi[jj]/2.0);
//	printf("phi[%d]=%f\n", jj,phi[jj]);

	Se11=edge_binormal_x[jj]*edge_binormal_x[jj]*he[jj];
	Se21=edge_binormal_x[jj]*edge_binormal_y[jj]*he[jj];
	Se22=edge_binormal_y[jj]*edge_binormal_y[jj]*he[jj];
	Se31=edge_binormal_x[jj]*edge_binormal_z[jj]*he[jj];
	Se32=edge_binormal_y[jj]*edge_binormal_z[jj]*he[jj];
	Se33=edge_binormal_z[jj]*edge_binormal_z[jj]*he[jj];

	We=vertex_normal_x*edge_normal_x[jj]+vertex_normal_y*edge_normal_y[jj]+vertex_normal_z*edge_normal_z[jj];
	We_Av=We/Av;

	Sv[0][0]+=We_Av* ( Pv11*(Pv11*Se11+Pv21*Se21+Pv31*Se31)+Pv21*(Pv11*Se21+Pv21*Se22+Pv31*Se32)+Pv31*(Pv11*Se31+Pv21*Se32+Pv31*Se33) );
	Sv[0][1]+=We_Av* (Pv21*(Pv11*Se11+Pv21*Se21+Pv31*Se31)+Pv22*(Pv11*Se21+Pv21*Se22+Pv31*Se32)+Pv32*(Pv11*Se31+Pv21*Se32+Pv31*Se33));
	Sv[0][2]+=We_Av* (Pv31*(Pv11*Se11+Pv21*Se21+Pv31*Se31)+Pv32*(Pv11*Se21+Pv21*Se22+Pv31*Se32)+Pv33*(Pv11*Se31+Pv21*Se32+Pv31*Se33));
	
	Sv[1][0]+=We_Av* (Pv11*(Pv21*Se11+Pv22*Se21+Pv32*Se31)+Pv21*(Pv21*Se21+Pv22*Se22+Pv32*Se32)+Pv31*(Pv21*Se31+Pv22*Se32+Pv32*Se33));
	Sv[1][1]+=We_Av* (Pv21*(Pv21*Se11+Pv22*Se21+Pv32*Se31)+Pv22*(Pv21*Se21+Pv22*Se22+Pv32*Se32)+Pv32*(Pv21*Se31+Pv22*Se32+Pv32*Se33));
	Sv[1][2]+=We_Av* (Pv31*(Pv21*Se11+Pv22*Se21+Pv32*Se31)+Pv32*(Pv21*Se21+Pv22*Se22+Pv32*Se32)+Pv33*(Pv21*Se31+Pv22*Se32+Pv32*Se33));

	Sv[2][0]+=We_Av* (Pv11*(Pv31*Se11+Pv32*Se21+Pv33*Se31)+Pv21*(Pv31*Se21+Pv32*Se22+Pv33*Se32)+Pv31*(Pv31*Se31+Pv32*Se32+Pv33*Se33));
	Sv[2][1]+=We_Av* (Pv21*(Pv31*Se11+Pv32*Se21+Pv33*Se31)+Pv22*(Pv31*Se21+Pv32*Se22+Pv33*Se32)+Pv32*(Pv31*Se31+Pv32*Se32+Pv33*Se33));
	Sv[2][2]+=We_Av* (Pv31*(Pv31*Se11+Pv32*Se21+Pv33*Se31)+Pv32*(Pv31*Se21+Pv32*Se22+Pv33*Se32)+Pv33*(Pv31*Se31+Pv32*Se32+Pv33*Se33));
//	printf("(%f %f %f); (%f %f %f); (%f %f %f)\n", edge_vector_x[jj], edge_vector_y[jj], edge_vector_z[jj], edge_normal_x[jj], edge_normal_y[jj], edge_normal_z[jj], edge_binormal_x[jj], edge_binormal_y[jj], edge_binormal_z[jj]);

    } // END FOR JJ

	gsl_matrix_set(gsl_Sv, 0,0, Sv[0][0]);
	gsl_matrix_set(gsl_Sv, 0,1, Sv[0][1]);
	gsl_matrix_set(gsl_Sv, 0,2, Sv[0][2]);
	gsl_matrix_set(gsl_Sv, 1,0, Sv[1][0]);
	gsl_matrix_set(gsl_Sv, 1,1, Sv[1][1]);
	gsl_matrix_set(gsl_Sv, 1,2, Sv[1][2]);
	gsl_matrix_set(gsl_Sv, 2,0, Sv[2][0]);
	gsl_matrix_set(gsl_Sv, 2,1, Sv[2][1]);
	gsl_matrix_set(gsl_Sv, 2,2, Sv[2][2]);

//	printf("Se= %f, %f, %f\n    %f, %f, %f\n    %f, %f, %f\n", Se11, Se21, Se31, Se21, Se22, Se32, Se31, Se32, Se33);
//	printf("Pv= %f, %f, %f\n    %f, %f, %f\n    %f, %f, %f\n", Pv11, Pv21, Pv31, Pv21, Pv22, Pv32, Pv31, Pv32, Pv33);
	printf("Sv= %f, %f, %f\n    %f, %f, %f\n    %f, %f, %f\n", Sv[0][0], Sv[0][1], Sv[0][2], Sv[1][0], Sv[1][1], Sv[1][2], Sv[2][0], Sv[2][1], Sv[2][2]);

	gsl_eigen_nonsymm_params(0, 1, workspace);
	gsl_eigen_nonsymm(gsl_Sv, Sv_eigen, workspace);

	printf("Eigenvalues: %f, %f, %f\n", 
	GSL_REAL(gsl_vector_complex_get(Sv_eigen, 0)),
	GSL_REAL(gsl_vector_complex_get(Sv_eigen, 1)),
	GSL_REAL(gsl_vector_complex_get(Sv_eigen, 2))
	);
	vtx->energy=0.0;

	gsl_matrix_free(gsl_Sv);
	gsl_vector_complex_free(Sv_eigen);
	gsl_eigen_nonsymm_free(workspace);
	return TS_SUCCESS;
}

ts_bool sweep_attraction_bond_energy(ts_vesicle *vesicle){
	int i;
	for(i=0;i<vesicle->blist->n;i++){
		attraction_bond_energy(vesicle->blist->bond[i], vesicle->tape->w);
	}
	return TS_SUCCESS;
}


inline ts_bool attraction_bond_energy(ts_bond *bond, ts_double w){

	if(fabs(bond->vtx1->c)>1e-16 && fabs(bond->vtx2->c)>1e-16){
		bond->energy=-w;
	}
	else {
		bond->energy=0.0;
	}
	return TS_SUCCESS;
}

ts_double direct_force_energy(ts_vesicle *vesicle, ts_vertex *vtx, ts_vertex *vtx_old){
	if(fabs(vtx->c)<1e-15) return 0.0;
//	printf("was here");
	if(fabs(vesicle->tape->F)<1e-15) return 0.0;

	ts_double norml,ddp=0.0;
	ts_uint i;
	ts_double xnorm=0.0,ynorm=0.0,znorm=0.0;
	/*find normal of the vertex as sum of all the normals of the triangles surrounding it. */
	for(i=0;i<vtx->tristar_no;i++){
			xnorm+=vtx->tristar[i]->xnorm;
			ynorm+=vtx->tristar[i]->ynorm;
			znorm+=vtx->tristar[i]->znorm;
	}
	/*normalize*/
	norml=sqrt(xnorm*xnorm+ynorm*ynorm+znorm*znorm);
	xnorm/=norml;
	ynorm/=norml;
	znorm/=norml;
	/*calculate ddp, perpendicular displacement*/
	ddp=xnorm*(vtx->x-vtx_old->x)+ynorm*(vtx->y-vtx_old->y)+znorm*(vtx->z-vtx_old->z);
	/*calculate dE*/
//	printf("ddp=%e",ddp);
	return vesicle->tape->F*ddp;		
	
}

void stretchenergy(ts_vesicle *vesicle, ts_triangle *triangle){
	triangle->energy=vesicle->tape->xkA0/2.0*pow((triangle->area/vesicle->tlist->a0-1.0),2);
}