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

inline void AdHarmonicAngleAcceleration(double* bond, double* ang_pot)
{
	
	register int i;
	int atom_one, atom_two, atom_three; 
	double coff_A, ang_cnst, eq_ang;
	double cosine_ang, angle, d_theta, ang_accel;
	double accel_one, accel_three;
	double numerator, denominator, dtheta_du; 
	double rmass_one, rmass_two, rmass_three;
	Vector3D ba_v, bc_v;

	//decode bond

	atom_one = (int)bond[0];
	atom_two = (int)bond[1];
	atom_three = (int)bond[2];
	ang_cnst = bond[3];	
	eq_ang = bond[4];

	//assign reciprocal masses to local variables

	rmass_one = coordinates[atom_one][4];
	rmass_two = coordinates[atom_two][4];
	rmass_three = coordinates[atom_three][4];

	//find the two vectors ba_v, bc_v

	for(i=0; i<3; i++)
	{
		ba_v.vector[i] = coordinates[atom_one][i] - coordinates[atom_two][i];
		bc_v.vector[i] = coordinates[atom_three][i] - coordinates[atom_two][i];
	}
	
	//ba.bc/|ba||bc| = cos(theta)
	//therefore calculate ba.bc 

	numerator = Ad3DDotProduct(&ba_v, &bc_v);
	
	//now find the length of ba and bc

	Ad3DVectorLength(&ba_v);
	Ad3DVectorLength(&bc_v);

	//find |ba|*|bc|

	denominator = ba_v.length*bc_v.length;

	//now calculate cosine of theta

	cosine_ang = numerator/denominator;

	//find the associated angle

	angle = acos(cosine_ang);

	//calculate d_theta and hence the potential and the angular acceleration 

	d_theta =  angle - eq_ang;

	ang_accel = -ang_cnst*d_theta;

	*ang_pot -= ang_accel*d_theta*0.5;

	//find dtheta_du

	dtheta_du = (1 - cosine_ang*cosine_ang);

	dtheta_du = -1/sqrt(dtheta_du);

	//calculate coff_A 

	coff_A = (ang_accel*dtheta_du);

	//find the nine partial derivatives needed

	for(i=0; i<3;i++)
	{
		accel_one = (coff_A/denominator)*( bc_v.vector[i] - (bc_v.length/ba_v.length)*cosine_ang*ba_v.vector[i]);
		accel_three = (coff_A/denominator)*( ba_v.vector[i] - (ba_v.length/bc_v.length)*cosine_ang*bc_v.vector[i]);

		accelerations[atom_two][i] -= (accel_one + accel_three)*rmass_two;
		accelerations[atom_one][i] += accel_one*rmass_one;
		accelerations[atom_three][i] += accel_three*rmass_three;
	}

}

inline void AdHarmonicAngleForce(double* bond, double* ang_pot)
{
	
	register int i;
	int atom_one, atom_two, atom_three; 
	double coff_A, ang_cnst, eq_ang;
	double cosine_ang, angle, d_theta, ang_accel;
	double accel_one, accel_three;
	double numerator, denominator, dtheta_du; 
	Vector3D ba_v, bc_v;

	//decode bond

	atom_one = (int)bond[0];
	atom_two = (int)bond[1];
	atom_three = (int)bond[2];
	ang_cnst = bond[3];	
	eq_ang = bond[4];

	//find the two vectors ba_v, bc_v

	for(i=0; i<3; i++)
	{
		ba_v.vector[i] = coordinates[atom_one][i] - coordinates[atom_two][i];
		bc_v.vector[i] = coordinates[atom_three][i] - coordinates[atom_two][i];
	}
	
	//ba.bc/|ba||bc| = cos(theta)
	//therefore calculate ba.bc 

	numerator = Ad3DDotProduct(&ba_v, &bc_v);
	
	//now find the length of ba and bc

	Ad3DVectorLength(&ba_v);
	Ad3DVectorLength(&bc_v);

	//find |ba|*|bc|

	denominator = ba_v.length*bc_v.length;

	//now calculate cosine of theta

	cosine_ang = numerator/denominator;

	//find the associated angle

	angle = acos(cosine_ang);

	//calculate d_theta and hence the potential and the angular acceleration 

	d_theta =  angle - eq_ang;

	ang_accel = -ang_cnst*d_theta;

	*ang_pot -= ang_accel*d_theta*0.5;

	//find dtheta_du

	dtheta_du = (1 - cosine_ang*cosine_ang);

	dtheta_du = -1/sqrt(dtheta_du);

	//calculate coff_A 

	coff_A = (ang_accel*dtheta_du);

	//find the nine partial derivatives needed

	for(i=0; i<3;i++)
	{
		accel_one = (coff_A/denominator)*( bc_v.vector[i] - (bc_v.length/ba_v.length)*cosine_ang*ba_v.vector[i]);
		accel_three = (coff_A/denominator)*( ba_v.vector[i] - (ba_v.length/bc_v.length)*cosine_ang*bc_v.vector[i]);

		accelerations[atom_two][i] -= (accel_one + accel_three);
		accelerations[atom_one][i] += accel_one;
		accelerations[atom_three][i] += accel_three;
	}

}

inline void AdHarmonicAngleEnergy(double* bond, double* ang_pot, double** coordinates)
{
	register int i;
	int atom_one, atom_two, atom_three; 
	double ang_cnst, eq_ang;
	double cosine_ang, angle, d_theta, ang_accel;
	double numerator, denominator; 
	Vector3D ba_v, bc_v;

	//decode bond

	atom_one = (int)bond[0];
	atom_two = (int)bond[1];
	atom_three = (int)bond[2];
	ang_cnst = bond[3];	
	eq_ang = bond[4];

	//find the two vectors ba_v, bc_v

	for(i=0; i<3; i++)
	{
		ba_v.vector[i] = coordinates[atom_one][i] - coordinates[atom_two][i];
		bc_v.vector[i] = coordinates[atom_three][i] - coordinates[atom_two][i];
	}
	
	//ba.bc/|ba||bc| = cos(theta)
	//therefore calculate ba.bc 

	numerator = Ad3DDotProduct(&ba_v, &bc_v);
	
	//now find the length of ba and bc

	Ad3DVectorLength(&ba_v);
	Ad3DVectorLength(&bc_v);

	//find |ba|*|bc|

	denominator = ba_v.length*bc_v.length;

	//now calculate cosine of theta

	cosine_ang = numerator/denominator;

	//find the associated angle

	angle = acos(cosine_ang);

	//calculate d_theta and hence the potential and the angular acceleration 

	d_theta =  angle - eq_ang;

	ang_accel = -ang_cnst*d_theta;

	*ang_pot -= ang_accel*d_theta*0.5;
}
