// This file is part of the ESPResSo distribution (http://www.espresso.mpg.de).
// It is therefore subject to the ESPResSo license agreement which you accepted upon receiving the distribution
// and by which you are legally bound while utilizing this file in any form or way.
// There is NO WARRANTY, not even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
// You should have received a copy of that license along with this program;
// if not, refer to http://www.espresso.mpg.de/license.html where its current version can be found, or
// write to Max-Planck-Institute for Polymer Research, Theory Group, PO Box 3148, 55021 Mainz, Germany.
// Copyright (c) 2002-2006; all rights reserved unless otherwise stated.
#ifndef GB_DIHED_H
#define GB_DIHED_H
/** \file gb_dihed.h Routines to calculate the dihedral energy or/and
 *  and force for a quaternion particle pair.  Note that usage of dihedrals
 *  increases the interaction range of bonded interactions to 2 times
 *  the maximal bond length!  \ref forces.c
 *
 *  <b>Responsible:</b>
 *  <a href="mailto:address@hidden">Michael</a>
*/
#include <imd.h>
/// set gb_dihedral parameters
MDINLINE int gb_dihedral_set_params(int bond_type, int mult[5], double bend[5], double phase[5])
{
  if(bond_type < 0)
    return TCL_ERROR;

  make_bond_type_exist(bond_type);

  bonded_ia_params[bond_type].type = BONDED_IA_GB_DIHEDRAL;
  bonded_ia_params[bond_type].num  = 1;
  int i;

  for(i = 0; i < 5; i++) {
  bonded_ia_params[bond_type].p.gb_dihedral.gb_mult[i] = mult[i];
  bonded_ia_params[bond_type].p.gb_dihedral.gb_bend[i] = bend[i];
  bonded_ia_params[bond_type].p.gb_dihedral.gb_phase[i] = phase[i];
  }

  mpi_bcast_ia_params(bond_type, -1); 

  return TCL_OK;
}

/// parse parameters for the dihedral potential
MDINLINE int inter_parse_gb_dihedral(Tcl_Interp *interp, int bond_type, int argc, char **argv)
{
  int mult0,mult[5],i,j;
  double bend0,bend[5], phase0,phase[5];
  if (argc < 2 ) {
    Tcl_AppendResult(interp, "gay-berne dihedral needs 1 parameter: "
		     "<mult> <bend> <phase>", (char *) NULL);
    return (TCL_ERROR);
  }

  for (i = 0; i < 5; i++) {
  j=3*i+1;
    if ( !ARG_IS_I(j, mult0) || !ARG_IS_D(j+1, bend0) || !ARG_IS_D(j+2, phase0) ) {
    Tcl_AppendResult(interp, "gay-berne dihedral needs 15 parameters of types INT DOUBLE DOUBLE: "
		     "<mult> <bend> <phase> ", (char *) NULL);
    return TCL_ERROR;
    }
//    printf("Good: %d %d %f %f \n",i,mult0,bend0,phase0); 
    mult[i]=mult0;
    bend[i]=bend0;
    phase[i]=phase0;
  }
//printf("%d %d\n",argc,bond_type);
  
  CHECK_VALUE(gb_dihedral_set_params(bond_type, mult, bend, phase), "bond type must be nonnegative");
  printf("aaa\n");
}

/* Calculate "dihedral" energy between coupled GB particles p1, p2 from quaternion 
    Written by Michael Winokur*/
MDINLINE void calc_quat_dihedral_angle(Particle *p2, Particle *p3, double u[3], double v[3],  double uu[3], double vv[3],
	 double v23[3], double *cosphi, double *phi, double *coschi2, double *chi2, double *coschi3, double *chi3, double *coschi2p, double *chi2p, double *coschi3p, double *chi3p) 
{
  /* vectors for dihedral calculations. */
  double qa0, qa1, qa2, qa3, qb0, qb1, qb2, qb3, len;
  double t2,t3,t4,t5,t6,t7,t8,t9,t10;
  double s2,s3,s4,s5,s6,s7,s8,s9,s10;

#ifdef ROTATION
      qa0 = p2->r.quat[0];
      qa1 = p2->r.quat[1];
      qa2 = p2->r.quat[2];
      qa3 = p2->r.quat[3];
      qb0 = p3->r.quat[0];
      qb1 = p3->r.quat[1];
      qb2 = p3->r.quat[2];
      qb3 = p3->r.quat[3];
#endif
      t2 =  qa0*qa1;
      t3 =  qa0*qa2;
      t4 =  qa0*qa3;
      t5 = -qa1*qa1;
      t6 =  qa1*qa2;
      t7 =  qa1*qa3;
      t8 = -qa2*qa2;
      t9 =  qa2*qa3;
      t10= -qa3*qa3;
//  printf("pp2 %6.4f\t%6.4f\t%6.4f\t%6.4f\n",qa0,qa1,qa2,qa3);
//      u[0] = 2*(t7 + t3);
//      u[1] = 2*(t9 - t2);
/*      u[2] = (qa0*qa0 - qa1*qa1 - qa2*qa2 + qa3*q3a);
      1. = qa0*qa0 + qa1*qa1 + qa2*qa2 + qa3*qa3 
      The u unit vector initially is 0 0 1, perpendicular to the plane of ring.  
      This gives a consistent cosphi regardless of the quaternion */
//      u[2]  = 1. + 2. * (t5 + t8);
#ifdef ROTATION
      u[0] = p2->r.quatu[0];
      u[1] = p2->r.quatu[1];
      u[2] = p2->r.quatu[2];
#endif
      s2 =  qb0*qb1;
      s3 =  qb0*qb2;
      s4 =  qb0*qb3;
      s5 = -qb1*qb1;
      s6 =  qb1*qb2;
      s7 =  qb1*qb3;
      s8 = -qb2*qb2;
      s9 =  qb2*qb3;
      s10= -qb3*qb3;
//  printf("pp3 %6.4f\t%6.4f\t%6.4f\t%6.4f\n",qb0,qb1,qb2,qb3);
//      v[0] = 2*(s7 + s3);
//      v[1] = 2*(s9 - s2);
/*      v[2] = (qb0*qb0 - qb1*qb1 - qb2*qb2 + qb3*qb3); */
//      v[2]  = 1. + 2. * (s5 + s8);
#ifdef ROTATION
      v[0] = p3->r.quatu[0];
      v[1] = p3->r.quatu[1];
      v[2] = p3->r.quatu[2];
#endif
   *cosphi = scalar(v,u);
  if ( fabs(fabs(*cosphi)-1)  < TINY_SIN_VALUE  ) *cosphi = dround(*cosphi);
   *phi = acos(*cosphi);
    get_mi_vector(v23, p2->r.p, p3->r.p);
    len = sqrt(sqrlen(v23));
    v23[0]/=len;
    v23[1]/=len;
    v23[2]/=len;
   *coschi2 = v23[0]*u[0]+v23[1]*u[1]+v23[2]*u[2];
   if ( fabs(fabs(*coschi2)-1)  < TINY_SIN_VALUE  ) *coschi2 = dround(*coschi2);
   *chi2 = acos(*coschi2);
   *coschi3 = v23[0]*v[0]+v23[1]*v[1]+v23[2]*v[2];
  if ( fabs(fabs(*coschi3)-1)  < TINY_SIN_VALUE  ) *coschi3 = dround(*coschi3);
   *chi3 = acos(*coschi3);   
#ifdef SUBPARTICLE /* Need subparticle mapping to impliment this degree of freedom */
    double x1p[6];
    double x1,y1,z1,x2,y2,z2; 
    getSubparticleMapping(p2->r.sub_par[2],p3->r.sub_par[2],&x1p[0]);
    x1=x1p[0];
    y1=x1p[1];
    z1=x1p[2];
    x2=x1p[3];
    y2=x1p[4];
    z2=x1p[5];
//    printf("A: %d %d %f %f %f \n", p2->r.sub_par[2], p3->r.sub_par[2],x1,y1,z1);
    uu[0] = 2*( (t8 + t10)*x1 + (t6 -  t4)*y1 + (t3 + t7)*z1 ) + x1;
    uu[1] = 2*( (t4 +  t6)*x1 + (t5 + t10)*y1 + (t9 - t2)*z1 ) + y1;
    uu[2] = 2*( (t7 -  t3)*x1 + (t2 +  t9)*y1 + (t5 + t8)*z1 ) + z1;
   *coschi2p = v23[0]*uu[0]+v23[1]*uu[1]+v23[2]*uu[2];
//     printf("B: %d %d %f %f %f \n",p2->r.sub_par[2], p3->r.sub_par[2],x2,y2,z2);
    vv[0] = 2*( (s8 + s10)*x2 + (s6 -  s4)*y2 + (s3 + s7)*z2 ) + x2;
    vv[1] = 2*( (s4 +  s6)*x2 + (s5 + s10)*y2 + (s9 - s2)*z2 ) + y2;
    vv[2] = 2*( (s7 -  s3)*x2 + (s2 +  s9)*y2 + (s5 + s8)*z2 ) + z2;
   *coschi3p =  v23[0]*vv[0]+v23[1]*vv[1]+v23[2]*vv[2];
   if ( fabs(fabs(*coschi2p)-1)  < TINY_SIN_VALUE  ) *coschi2p = dround(*coschi2p);
   *chi2p = acos(*coschi2p);
   if ( fabs(fabs(*coschi3p)-1)  < TINY_SIN_VALUE  ) *coschi3p = dround(*coschi3p);
   *chi3p = acos(*coschi3p);   
//printf(" chi2p chi3p %f  %f \n",*chi2p,*chi3p);
#else
   *chi2p = 0.;
   *chi3p = 0.;   
#endif
}


/* calculate dihedral force/torque between particles p2 p3, by Michael Winokur */
MDINLINE int calc_gb_dihedral_torque(Particle *p2, Particle *p3,
	       Bonded_ia_parameters *iaparams, double dx[3], double torque2[3],double torque3[3], double force2[3],double force3[3])
{
  /* vectors for dihedral angle calculation */
  double u[3], v[3], uu[3], vv[3], v23[3];
  /* dihedral angle, cosine of the dihedral angle */
  double phi, cosphi, chi2, coschi2, chi3, coschi3 ;
  double chi2p, coschi2p, chi3p, coschi3p ;
  double tiny,dist;
  double t2[3],t3[3],uXv23[3],vXv23[3],uXv[3],t1[3];
  double dU_da,dU_db,dU_dc,d[3],dU_daa,dU_dbb;

  /* dihedral angle */
  double phase0;
  tiny = 1e-20;
  get_mi_vector(d, p2->r.p, p3->r.p);
  dist=sqrt(d[0]*d[0]+d[1]*d[1]+d[2]*d[2]);
  calc_quat_dihedral_angle(p2, p3, u, v, uu, vv, v23, &cosphi, &phi, &coschi2, &chi2, &coschi3, &chi3, &coschi2p, &chi2p, &coschi3p, &chi3p);
  dU_da=2.*iaparams->p.gb_dihedral.gb_bend[3]*coschi2;
  dU_db=2.*iaparams->p.gb_dihedral.gb_bend[3]*coschi3;
  phase0 = iaparams->p.gb_dihedral.gb_mult[0]*phi -iaparams->p.gb_dihedral.gb_phase[0]; 
  /* Dihedral torque is along v23 with a nominal magnitude of K*sin(n phi -p)*n  */
  dU_dc =  (iaparams->p.gb_dihedral.gb_bend[0]*sin(phase0)*iaparams->p.gb_dihedral.gb_mult[0]); 
  phase0 = iaparams->p.gb_dihedral.gb_mult[1]*phi -iaparams->p.gb_dihedral.gb_phase[1]; 
  dU_dc += (iaparams->p.gb_dihedral.gb_bend[1]*sin(phase0)*iaparams->p.gb_dihedral.gb_mult[1]); 
  phase0 = iaparams->p.gb_dihedral.gb_mult[2]*phi -iaparams->p.gb_dihedral.gb_phase[2]; 
  dU_dc += (iaparams->p.gb_dihedral.gb_bend[2]*sin(phase0)*iaparams->p.gb_dihedral.gb_mult[2]); 
  vector_product(u, v, uXv);
  t1[0] = dU_dc*uXv[0];
  t1[1] = dU_dc*uXv[1];
  t1[2] = dU_dc*uXv[2];

//  printf("coschi2,coshi3,dU_da,dU_db,dU_dc : %f %f %f %f %f \n",coschi2,coschi3,dU_da,dU_db,dU_dc);

#ifdef SUBPARTICLE /* Need subparticle mapping to impliment this degree of freedom */
/* In plane torque is along quat_a(v23)Xv23 &  quat_b(v23)Xv23 and 
              proportional to its magnitude */
  vector_product(uu, v23, uXv23);
  vector_product(vv, v23, vXv23);
//  In principle we should use sinchi2 but chi2p gives a linear response out to high angles
  dU_daa = -2.*coschi2p*iaparams->p.gb_dihedral.gb_bend[4];
  dU_dbb = -2.*coschi3p*iaparams->p.gb_dihedral.gb_bend[4];
  t2[0] = uXv23[0]*dU_daa;
  t2[1] = uXv23[1]*dU_daa;
  t2[2] = uXv23[2]*dU_daa;
  t3[0] = vXv23[0]*dU_dbb;
  t3[1] = vXv23[1]*dU_dbb;
  t3[2] = vXv23[2]*dU_dbb;
#endif
  /* Net torque is then the counter torque/force from nominal center of mass between pair
     The direction is perpendicular to the net torque vector and v23 
     This assumes the masses of the two objects is nearly equal.  If not then the torques/forces
     may need to reference the center of mass position */

      /* force on molecule 1 due to molecule 2 */
      force2[0] = (dU_da*(u[0]-coschi2*v23[0])+dU_db*(v[0]-coschi3*v23[0]))/dist;
      force2[1] = (dU_da*(u[1]-coschi2*v23[1])+dU_db*(v[1]-coschi3*v23[1]))/dist;
      force2[2] = (dU_da*(u[2]-coschi2*v23[2])+dU_db*(v[2]-coschi3*v23[2]))/dist;
      /* force on molecule 1 due to molecule 2 */
      force2[0] += (dU_daa*(uu[0]-coschi2p*v23[0])+dU_dbb*(vv[0]-coschi3p*v23[0]))/dist;
      force2[1] += (dU_daa*(uu[1]-coschi2p*v23[1])+dU_dbb*(vv[1]-coschi3p*v23[1]))/dist;
      force2[2] += (dU_daa*(uu[2]-coschi2p*v23[2])+dU_dbb*(vv[2]-coschi3p*v23[2]))/dist;

      /* torque on molecule 1 due to molecule 2 */
      torque2[0] = (dU_da*(u[1]*v23[2]-u[2]*v23[1])+t1[0]);
      torque2[1] = (dU_da*(u[2]*v23[0]-u[0]*v23[2])+t1[1]);
      torque2[2] = (dU_da*(u[0]*v23[1]-u[1]*v23[0])+t1[2]);
      torque2[0] += t2[0];
      torque2[1] += t2[1];
      torque2[2] += t2[2];

      /* torque on molecule 2 due to molecule 1 */      
      torque3[0] = (dU_db*(v[1]*v23[2]-v[2]*v23[1])-t1[0]);
      torque3[1] = (dU_db*(v[2]*v23[0]-v[0]*v23[2])-t1[1]);
      torque3[2] = (dU_db*(v[0]*v23[1]-v[1]*v23[0])-t1[2]);
      torque3[0] += t3[0];
      torque3[1] += t3[1];
      torque3[2] += t3[2];
 
  force3[0] = -force2[0];
  force3[1] = -force2[1];
  force3[2] = -force2[2];

  return 0;
}

/** calculate dihedral energy between particles p2 p3    Written by Michael Winokur */
MDINLINE int gb_dihedral_energy(Particle *p2, Particle *p3, Bonded_ia_parameters *iaparams, double *_energy) 
{
  /* vectors for dihedral calculations. */
  double u[3], v[3], uu[3], vv[3], phi, cosphi, v23[3];
  double chi2, coschi2, chi3, coschi3,chi2p, coschi2p, chi3p, coschi3p;
  /* energy factors */
  double fac,tfac;
  int i;
  calc_quat_dihedral_angle(p2, p3, u, v,  uu, vv,v23, &cosphi, &phi, &coschi2, &chi2, &coschi3, &chi3, &coschi2p, &chi2p, &coschi3p, &chi3p);
  tfac = 0;
  for(i = 0; i < 3; i++) {
	#ifdef OLD_DIHEDRAL
  	fac = iaparams->p.gb_dihedral.gb_phase[i] * cos(iaparams->p.gb_dihedral.gb_mult[i]*phi);
	#else
  	fac = -cos(iaparams->p.gb_dihedral.gb_mult[i]*phi -iaparams->p.gb_dihedral.gb_phase[i]);
	#endif
//  fac += 1.0;
// Since iaparams->p.gb_dihedral.gb_bend[i] < 0, this gives a maxmimum energy of zero...but we want 0
// to be a minimum
  fac += -1.0;
  fac *=  iaparams->p.gb_dihedral.gb_bend[i];
  tfac += fac;
  }
  tfac = 0.;
/* Out of plane bending energy...keep ring planes inline with r23; no double counting; 2 multiplicity */
// Shift minimum to zero
//  tfac += iaparams->p.gb_dihedral.gb_bend[3]*(2.-coschi2*coschi2-coschi3*coschi3);
  tfac += iaparams->p.gb_dihedral.gb_bend[3]*(-coschi2*coschi2-coschi3*coschi3);
/* In plane rotation energy...keep rings from rotating about 0 0 1 direction; no double counting; 1 multiplicity */
/* p.dihedral.bend is < 0 for the phenylene twist */
#ifdef SUBPARTICLE /* Need subparticle mapping to impliment this degree of freedom */
  tfac += iaparams->p.gb_dihedral.gb_bend[4]*(-2.+coschi2p*coschi2p+coschi3p*coschi3p);
#endif     
  *_energy = tfac;
  return 0;
}

#endif