/*
   Project: Adun

   Copyright (C) 2005 Michael Johnston & Jordi Villa-Freixa

   Author: Michael Johnston

   This application is free software; you can redistribute it and/or
   modify it under the terms of the GNU General Public
   License as published by the Free Software Foundation; either
   version 2 of the License, or (at your option) any later version.

   This application is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Library General Public License for more details.

   You should have received a copy of the GNU General Public
   License along with this library; if not, write to the Free
   Software Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111 USA.
*/

#include <Base/AdForceFieldFunctions.h>

void AdFourierTorsionEnergy(double* bond, double* tor_pot, double** coordinates)
{
	register int i;
	int atom_one, atom_two, atom_three, atom_four;
	double num, denom, cosine_ang, angle;
	double  n2_n1, n1_n2, period, phase, holder, tor_cnst;
	Vector3D n_one, n_two, ba, bc, cd;

	//decode bond

	atom_one = (int)bond[0];
	atom_two = (int)bond[1];
	atom_three = (int)bond[2];
	atom_four = (int)bond[3];
	tor_cnst = bond[4];
	period = bond[5];
	phase = bond[6];

	//calculate vectors needed ba, bc, cd 

	for(i=3; --i>=0;)
	{
		*(ba.vector + i) = coordinates[atom_two][i] - coordinates[atom_one][i];
		*(bc.vector + i) = coordinates[atom_three][i] - coordinates[atom_two][i];
		*(cd.vector + i) = coordinates[atom_four][i] - coordinates[atom_three][i];
	}

	//calculate the cross product ba X bc, cd X cb
	
	Ad3DCrossProduct(&ba, &bc, &n_one);
	Ad3DCrossProduct(&bc, &cd, &n_two);
	Ad3DVectorLength(&n_one);
	Ad3DVectorLength(&n_two);

	//find the dot product of the two normal vectors to find the angle

	num = Ad3DDotProduct(&n_one, &n_two);
	denom = n_one.length*n_two.length;
	n2_n1 = n_two.length/n_one.length;
	n1_n2 = n_one.length/n_two.length;

	cosine_ang = num/denom;

	//check if the cosine of the angle is between -1 or 1. 
	//It could have slipped beyond these bounds becuase of the limits to precision of doubles

	if(cosine_ang > 1)
	{
		cosine_ang = 1;
		angle = 0;
	}
	else if(cosine_ang < -1)
	{
		cosine_ang = -1;
		angle = M_PI;
	}
	else
		angle = acos(cosine_ang);

	//calculate potential energy due to the bond

	if(phase == 0)
	{
		holder = cos(period*angle);
		if(isnan(holder))
		{
			fprintf(stderr, "AdFourierTorsion - ERROR\n");
			fprintf(stderr, "Angle %lf. Period %lf\n", angle, period);
		}
			
		
		*tor_pot += tor_cnst*(1 + holder);
	}
	else
	{
		holder = cos(period*angle);
		if(isnan(holder))
		{
			fprintf(stderr, "AdFourierTorsion - ERROR\n");
			fprintf(stderr, "Angle %lf. Period %lf\n", angle, period);
		}

		*tor_pot += tor_cnst*(1 - holder);
	}
}

inline void AdFourierTorsionAccleration(double* bond, double* tor_pot)
{
	register int i, j;
	int counter;
	int atom_one, atom_two, atom_three, atom_four;
	double num, denom, cosine_ang, angle;
	double coff_A, coff_B, n2_n1, n1_n2, period, phase, holder, tor_cnst, rmass;
	double dot[12], product[12];
	double *accel_holder;
	Vector3D n_one, n_two, ba, bc, cd, accel_h;

	//decode bond

	atom_one = (int)bond[0];
	atom_two = (int)bond[1];
	atom_three = (int)bond[2];
	atom_four = (int)bond[3];
	tor_cnst = bond[4];
	period = bond[5];
	phase = bond[6];

	//calculate vectors needed ba, bc, cd 

	for(i=3; --i>=0;)
	{
		*(ba.vector + i) = coordinates[atom_two][i] - coordinates[atom_one][i];
		*(bc.vector + i) = coordinates[atom_three][i] - coordinates[atom_two][i];
		*(cd.vector + i) = coordinates[atom_four][i] - coordinates[atom_three][i];
	}

	//calculate the cross product ba X bc, cd X cb
	
	Ad3DCrossProduct(&ba, &bc, &n_one);
	Ad3DCrossProduct(&bc, &cd, &n_two);
	Ad3DVectorLength(&n_one);
	Ad3DVectorLength(&n_two);

	//find the dot product of the two normal vectors to find the angle

	num = Ad3DDotProduct(&n_one, &n_two);
	denom = n_one.length*n_two.length;
	n2_n1 = n_two.length/n_one.length;
	n1_n2 = n_one.length/n_two.length;

	cosine_ang = num/denom;

	//check if the cosine of the angle is between -1 or 1. 
	//It could have slipped beyond these bounds becuase of the limits to precision of doubles
	
	if(cosine_ang > 1)
	{
		cosine_ang = 1;
		angle = 0;
	}
	else if(cosine_ang < -1)
	{
		cosine_ang = -1;
		angle = M_PI;
	}
	else
		angle = acos(cosine_ang);

	//find some reoccuring quantities and common premultiplication factors now

	coff_A = 4*tor_cnst*cosine_ang; 	//if potential = A*(1 + cos(2*x))  i.e period is 2
	
	if(period == 3)
		coff_A = coff_A*3*cosine_ang - 3*tor_cnst;

	coff_A = (-1*coff_A)/denom;

	//calculate potential energy due to the bond
	if(phase == 0)
	{
		holder = cos(period*angle);
		if(isnan(holder))
		{
			fprintf(stderr, "AdFourierTorsion - ERROR\n");
			fprintf(stderr, "Angle %lf. Period %lf\n", angle, period);
		}
			
		
		*tor_pot += tor_cnst*(1 + holder);
	}
	else
	{
		holder = cos(period*angle);
		if(isnan(holder))
		{
			fprintf(stderr, "AdFourierTorsion - ERROR\n");
			fprintf(stderr, "Angle %lf. Period %lf\n", angle, period);
		}

		*tor_pot += tor_cnst*(1 - holder);
		coff_A = -1*coff_A;	
	}

	coff_B = coff_A*cosine_ang;

	/*
	 *Calculate the partial derivatives	
	 *There are 12 pd's for n1.n2 and 12 for |n1||n2|. 
	 *Its easier to calculate each of these individulally even though it looks long and complicated.
	 *Do it on an atom by atom basis
	 */

	//atom 1

	dot[0] = bc.vector[2]*n_two.vector[1] - bc.vector[1]*n_two.vector[2];
	dot[1] = bc.vector[0]*n_two.vector[2] - bc.vector[2]*n_two.vector[0];
	dot[2] = bc.vector[1]*n_two.vector[0] - bc.vector[0]*n_two.vector[1];

	product[0] = n2_n1*(n_one.vector[1]*bc.vector[2] - n_one.vector[2]*bc.vector[1]); 
	product[1] = n2_n1*(n_one.vector[2]*bc.vector[0] - n_one.vector[0]*bc.vector[2]);
	product[2] = n2_n1*(n_one.vector[0]*bc.vector[1] - n_one.vector[1]*bc.vector[0]);
	
	//atom 2

	dot[3] = (ba.vector[1] + bc.vector[1])*n_two.vector[2] - (ba.vector[2] + bc.vector[2])*n_two.vector[1] + cd.vector[2]*n_one.vector[1] - cd.vector[1]*n_one.vector[2]; 
	dot[4] = (ba.vector[2] + bc.vector[2])*n_two.vector[0] - (ba.vector[0] + bc.vector[0])*n_two.vector[2] + cd.vector[0]*n_one.vector[2] - cd.vector[2]*n_one.vector[0]; 
	dot[5] = (ba.vector[0] + bc.vector[0])*n_two.vector[1] - (ba.vector[1] + bc.vector[1])*n_two.vector[0] + cd.vector[1]*n_one.vector[0] - cd.vector[0]*n_one.vector[1]; 
	
	product[3] = n2_n1*((ba.vector[1] + bc.vector[1])*n_one.vector[2] - (ba.vector[2] + bc.vector[2])*n_one.vector[1]) + n1_n2*(cd.vector[2]*n_two.vector[1] - cd.vector[1]*n_two.vector[2]);
	product[4] = n2_n1*((ba.vector[2] + bc.vector[2])*n_one.vector[0] - (ba.vector[0] + bc.vector[0])*n_one.vector[2]) + n1_n2*(cd.vector[0]*n_two.vector[2] - cd.vector[2]*n_two.vector[0]);
	product[5] = n2_n1*((ba.vector[0] + bc.vector[0])*n_one.vector[1] - (ba.vector[1] + bc.vector[1])*n_one.vector[0]) + n1_n2*(cd.vector[1]*n_two.vector[0] - cd.vector[0]*n_two.vector[1]);
	
	//atom 3

	dot[6] = (bc.vector[1] + cd.vector[1])*n_one.vector[2] - (bc.vector[2] + cd.vector[2])*n_one.vector[1] + ba.vector[2]*n_two.vector[1] - ba.vector[1]*n_two.vector[2]; 
	dot[7] = (bc.vector[2] + cd.vector[2])*n_one.vector[0] - (bc.vector[0] + cd.vector[0])*n_one.vector[2] + ba.vector[0]*n_two.vector[2] - ba.vector[2]*n_two.vector[0]; 
	dot[8] = (bc.vector[0] + cd.vector[0])*n_one.vector[1] - (bc.vector[1] + cd.vector[1])*n_one.vector[0] + ba.vector[1]*n_two.vector[0] - ba.vector[0]*n_two.vector[1]; 
	
	product[6] = n1_n2*((bc.vector[1] + cd.vector[1])*n_two.vector[2] - (bc.vector[2] + cd.vector[2])*n_two.vector[1]) + n2_n1*(ba.vector[2]*n_one.vector[1] - ba.vector[1]*n_one.vector[2]);
	product[7] = n1_n2*((bc.vector[2] + cd.vector[2])*n_two.vector[0] - (bc.vector[0] + cd.vector[0])*n_two.vector[2]) + n2_n1*(ba.vector[0]*n_one.vector[2] - ba.vector[2]*n_one.vector[0]);
	product[8] = n1_n2*((bc.vector[0] + cd.vector[0])*n_two.vector[1] - (bc.vector[1] + cd.vector[1])*n_two.vector[0]) + n2_n1*(ba.vector[1]*n_one.vector[0] - ba.vector[0]*n_one.vector[1]);
	

	//atom 4
	
	dot[9] =  bc.vector[2]*n_one.vector[1] - bc.vector[1]*n_one.vector[2];
	dot[10] = bc.vector[0]*n_one.vector[2] - bc.vector[2]*n_one.vector[0];
	dot[11] = bc.vector[1]*n_one.vector[0] - bc.vector[0]*n_one.vector[1];
	
	product[9] =  n1_n2*(n_two.vector[1]*bc.vector[2] - n_two.vector[2]*bc.vector[1]); 
	product[10] = n1_n2*(n_two.vector[2]*bc.vector[0] - n_two.vector[0]*bc.vector[2]);
	product[11] = n1_n2*(n_two.vector[0]*bc.vector[1] - n_two.vector[1]*bc.vector[0]);

	counter = 0;

	for(i=0; i< 4; i++)
	{
		accel_holder = accelerations[(int)bond[i]];
		rmass = coordinates[(int)bond[i]][4];

		for(j=0; j<3; j++)
		{
			accel_h.vector[j] = (coff_A*dot[counter] - coff_B*product[counter])*rmass;
			counter += 1;
		}

		//add the an acceleration vector to the atoms total acceleration vector

		for(j=0; j<3; j++)
			accel_holder[j] += accel_h.vector[j];
	}

}

inline void AdFourierTorsionForce(double* bond, double* tor_pot)
{
	register int i, j;
	int counter;
	int atom_one, atom_two, atom_three, atom_four;
	double num, denom, cosine_ang, angle;
	double coff_A, coff_B, n2_n1, n1_n2, period, phase, holder, tor_cnst;
	double dot[12], product[12];
	double *accel_holder;
	Vector3D n_one, n_two, ba, bc, cd, accel_h;

	//decode bond

	atom_one = (int)bond[0];
	atom_two = (int)bond[1];
	atom_three = (int)bond[2];
	atom_four = (int)bond[3];
	tor_cnst = bond[4];
	period = bond[5];
	phase = bond[6];

	//calculate vectors needed ba, bc, cd 

	for(i=3; --i>=0;)
	{
		*(ba.vector + i) = coordinates[atom_two][i] - coordinates[atom_one][i];
		*(bc.vector + i) = coordinates[atom_three][i] - coordinates[atom_two][i];
		*(cd.vector + i) = coordinates[atom_four][i] - coordinates[atom_three][i];
	}

	//calculate the cross product ba X bc, cd X cb
	
	Ad3DCrossProduct(&ba, &bc, &n_one);
	Ad3DCrossProduct(&bc, &cd, &n_two);
	Ad3DVectorLength(&n_one);
	Ad3DVectorLength(&n_two);

	//find the dot product of the two normal vectors to find the angle

	num = Ad3DDotProduct(&n_one, &n_two);
	denom = n_one.length*n_two.length;
	n2_n1 = n_two.length/n_one.length;
	n1_n2 = n_one.length/n_two.length;

	cosine_ang = num/denom;

	//check if the cosine of the angle is between -1 or 1. 
	//It could have slipped beyond these bounds becuase of the limits to precision of doubles
	
	if(cosine_ang > 1)
	{
		cosine_ang = 1;
		angle = 0;
	}
	else if(cosine_ang < -1)
	{
		cosine_ang = -1;
		angle = M_PI;
	}
	else
		angle = acos(cosine_ang);


	//find some reoccuring quantities and common premultiplication factors now

	coff_A = 4*tor_cnst*cosine_ang; 	//if potential = A*(1 + cos(2*x))  i.e period is 2
	
	if(period == 3)
		coff_A = coff_A*3*cosine_ang - 3*tor_cnst;

	coff_A = (-1*coff_A)/denom;

	//calculate potential energy due to the bond

	if(phase == 0)
	{
		holder = cos(period*angle);
		if(isnan(holder))
		{
			fprintf(stderr, "AdFourierTorsion - ERROR\n");
			fprintf(stderr, "Angle %lf. Period %lf\n", angle, period);
		}
			
		
		*tor_pot += tor_cnst*(1 + holder);
	}
	else
	{
		holder = cos(period*angle);
		if(isnan(holder))
		{
			fprintf(stderr, "AdFourierTorsion - ERROR\n");
			fprintf(stderr, "Angle %lf. Period %lf\n", angle, period);
		}

		*tor_pot += tor_cnst*(1 - holder);
		coff_A = -1*coff_A;	
	}
	
	coff_B = coff_A*cosine_ang;

	/*
	 *Calculate the partial derivatives	
	 *There are 12 pd's for n1.n2 and 12 for |n1||n2|. 
  	 *Its easier to calculate each of these individulally even though it looks long and complicated.
	 *Do it on an atom by atom basis
	 * 
	 */

	//atom 1

	dot[0] = bc.vector[2]*n_two.vector[1] - bc.vector[1]*n_two.vector[2];
	dot[1] = bc.vector[0]*n_two.vector[2] - bc.vector[2]*n_two.vector[0];
	dot[2] = bc.vector[1]*n_two.vector[0] - bc.vector[0]*n_two.vector[1];

	product[0] = n2_n1*(n_one.vector[1]*bc.vector[2] - n_one.vector[2]*bc.vector[1]); 
	product[1] = n2_n1*(n_one.vector[2]*bc.vector[0] - n_one.vector[0]*bc.vector[2]);
	product[2] = n2_n1*(n_one.vector[0]*bc.vector[1] - n_one.vector[1]*bc.vector[0]);
	
	//atom 2

	dot[3] = (ba.vector[1] + bc.vector[1])*n_two.vector[2] - (ba.vector[2] + bc.vector[2])*n_two.vector[1] + cd.vector[2]*n_one.vector[1] - cd.vector[1]*n_one.vector[2]; 
	dot[4] = (ba.vector[2] + bc.vector[2])*n_two.vector[0] - (ba.vector[0] + bc.vector[0])*n_two.vector[2] + cd.vector[0]*n_one.vector[2] - cd.vector[2]*n_one.vector[0]; 
	dot[5] = (ba.vector[0] + bc.vector[0])*n_two.vector[1] - (ba.vector[1] + bc.vector[1])*n_two.vector[0] + cd.vector[1]*n_one.vector[0] - cd.vector[0]*n_one.vector[1]; 
	
	product[3] = n2_n1*((ba.vector[1] + bc.vector[1])*n_one.vector[2] - (ba.vector[2] + bc.vector[2])*n_one.vector[1]) + n1_n2*(cd.vector[2]*n_two.vector[1] - cd.vector[1]*n_two.vector[2]);
	product[4] = n2_n1*((ba.vector[2] + bc.vector[2])*n_one.vector[0] - (ba.vector[0] + bc.vector[0])*n_one.vector[2]) + n1_n2*(cd.vector[0]*n_two.vector[2] - cd.vector[2]*n_two.vector[0]);
	product[5] = n2_n1*((ba.vector[0] + bc.vector[0])*n_one.vector[1] - (ba.vector[1] + bc.vector[1])*n_one.vector[0]) + n1_n2*(cd.vector[1]*n_two.vector[0] - cd.vector[0]*n_two.vector[1]);
	
	//atom 3

	dot[6] = (bc.vector[1] + cd.vector[1])*n_one.vector[2] - (bc.vector[2] + cd.vector[2])*n_one.vector[1] + ba.vector[2]*n_two.vector[1] - ba.vector[1]*n_two.vector[2]; 
	dot[7] = (bc.vector[2] + cd.vector[2])*n_one.vector[0] - (bc.vector[0] + cd.vector[0])*n_one.vector[2] + ba.vector[0]*n_two.vector[2] - ba.vector[2]*n_two.vector[0]; 
	dot[8] = (bc.vector[0] + cd.vector[0])*n_one.vector[1] - (bc.vector[1] + cd.vector[1])*n_one.vector[0] + ba.vector[1]*n_two.vector[0] - ba.vector[0]*n_two.vector[1]; 
	
	product[6] = n1_n2*((bc.vector[1] + cd.vector[1])*n_two.vector[2] - (bc.vector[2] + cd.vector[2])*n_two.vector[1]) + n2_n1*(ba.vector[2]*n_one.vector[1] - ba.vector[1]*n_one.vector[2]);
	product[7] = n1_n2*((bc.vector[2] + cd.vector[2])*n_two.vector[0] - (bc.vector[0] + cd.vector[0])*n_two.vector[2]) + n2_n1*(ba.vector[0]*n_one.vector[2] - ba.vector[2]*n_one.vector[0]);
	product[8] = n1_n2*((bc.vector[0] + cd.vector[0])*n_two.vector[1] - (bc.vector[1] + cd.vector[1])*n_two.vector[0]) + n2_n1*(ba.vector[1]*n_one.vector[0] - ba.vector[0]*n_one.vector[1]);
	

	//atom 4
	
	dot[9] =  bc.vector[2]*n_one.vector[1] - bc.vector[1]*n_one.vector[2];
	dot[10] = bc.vector[0]*n_one.vector[2] - bc.vector[2]*n_one.vector[0];
	dot[11] = bc.vector[1]*n_one.vector[0] - bc.vector[0]*n_one.vector[1];
	
	product[9] =  n1_n2*(n_two.vector[1]*bc.vector[2] - n_two.vector[2]*bc.vector[1]); 
	product[10] = n1_n2*(n_two.vector[2]*bc.vector[0] - n_two.vector[0]*bc.vector[2]);
	product[11] = n1_n2*(n_two.vector[0]*bc.vector[1] - n_two.vector[1]*bc.vector[0]);

	counter = 0;

	for(i=0; i< 4; i++)
	{
		accel_holder = accelerations[(int)bond[i]];

		for(j=0; j<3; j++)
		{
			accel_h.vector[j] = (coff_A*dot[counter] - coff_B*product[counter]);
			counter += 1;
		}

		//add the an acceleration vector to the atoms total acceleration vector

		for(j=0; j<3; j++)
			accel_holder[j] += accel_h.vector[j];
	}

}

