/* 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>


/** @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, cnt=0;
    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_double sumnorm;

    // Here edge vector is calculated
//    fprintf(stderr, "Vertex has neighbours=%d\n", vtx->neigh_no);
    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;
	// We find lm and lp from k->tristar !
	cnt=0;
    	for(i=0;i<vtx->tristar_no;i++){
        	for(j=0;j<vtx->neigh[jj]->tristar_no;j++){
            		if((vtx->tristar[i] == vtx->neigh[jj]->tristar[j])){ //ce gre za skupen trikotnik
                		triedge[cnt]=vtx->tristar[i];
				cnt++;
            		}
        	}
    	}
	if(cnt!=2) fatal("ts_energy_vertex: both triangles not found!", 133);
	sumnorm=sqrt( pow((triedge[0]->xnorm + triedge[1]->xnorm),2) + pow((triedge[0]->ynorm + triedge[1]->ynorm), 2) + pow((triedge[0]->znorm + triedge[1]->znorm), 2));

	edge_normal_x[jj]=(triedge[0]->xnorm+ triedge[1]->xnorm)/sumnorm;
	edge_normal_y[jj]=(triedge[0]->ynorm+ triedge[1]->ynorm)/sumnorm;
	edge_normal_z[jj]=(triedge[0]->znorm+ triedge[1]->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]);

	printf("(%f %f %f); (%f %f %f); (%f %f %f), %d\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],triedge[0]->idx);

    }
	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;
	}
	printf("(%f %f %f)\n", vertex_normal_x, vertex_normal_y, vertex_normal_z);
	vtx->energy=0.0;
	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);
}