/* Portions copyright (c) 2006-2009 Stanford University and Simbios.
 * Contributors: Pande Group
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject
 * to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include <string.h>
#include <sstream>

#include "SimTKOpenMMCommon.h"
#include "SimTKOpenMMUtilities.h"
#include "ReferenceForce.h"
#include "ReferenceBondIxn.h"

using std::vector;
using namespace OpenMM;

/**---------------------------------------------------------------------------------------

   ReferenceBondIxn constructor

   --------------------------------------------------------------------------------------- */

ReferenceBondIxn::ReferenceBondIxn() {

   // ---------------------------------------------------------------------------------------

   // static const char* methodName = "\nReferenceBondIxn::ReferenceBondIxn";

   // ---------------------------------------------------------------------------------------

}

/**---------------------------------------------------------------------------------------

   ReferenceBondIxn destructor

   --------------------------------------------------------------------------------------- */

ReferenceBondIxn::~ReferenceBondIxn() {

   // ---------------------------------------------------------------------------------------

   // static const char* methodName = "\nReferenceBondIxn::~ReferenceBondIxn";

   // ---------------------------------------------------------------------------------------

}

/**---------------------------------------------------------------------------------------
      
   Calculate Bond Ixn -- virtual method -- does nothing
      
   @param atomIndices      bond indices
   @param atomCoordinates  atom coordinates
   @param parameters       parameters
   @param forces           force array (forces added)
   @param totalEnergy      if not null, the energy will be added to this
      
   --------------------------------------------------------------------------------------- */
     
   void ReferenceBondIxn::calculateBondIxn(int* atomIndices, vector<RealVec>& atomCoordinates,
                                           RealOpenMM* parameters, vector<RealVec>& forces,
                                           RealOpenMM* totalEnergy) const {
   // ---------------------------------------------------------------------------------------

   // static const std::string methodName = "\nReferenceBondIxn::calculateBondIxn";

   // ---------------------------------------------------------------------------------------

}
 
/**---------------------------------------------------------------------------------------

   Get normed dot product between two vectors

   Do computation in double?

   @param  vector1            first vector
   @param  vector2            second vector
   @param  hasREntry          if set, then vector1[ReferenceForce::RIndex] = norm of vector
                              defaults to 0 (i.e., R unavailable)

   @return dot product

   --------------------------------------------------------------------------------------- */

RealOpenMM ReferenceBondIxn::getNormedDotProduct(RealOpenMM* vector1, RealOpenMM* vector2,
                                                 int hasREntry = 0) {

   // ---------------------------------------------------------------------------------------

   // static const std::string methodName = "\nReferenceBondIxn::getNormedDotProduct";

   static const RealOpenMM zero        = 0.0;
   static const RealOpenMM one         = 1.0;

   // ---------------------------------------------------------------------------------------

// for angles near pi, double is required due to the 'steepness' of acos()
// in this regime.
  
//#define USE_DOUBLE_FOR_NORMED_DOT_PRODUCT

#if defined USE_DOUBLE_FOR_NORMED_DOT_PRODUCT
   double v1D[3];
   double v2D[3];
   v1D[0]                = static_cast<double>(vector1[0]);
   v1D[1]                = static_cast<double>(vector1[1]);
   v1D[2]                = static_cast<double>(vector1[2]);

   v2D[0]                = static_cast<double>(vector2[0]);
   v2D[1]                = static_cast<double>(vector2[1]);
   v2D[2]                = static_cast<double>(vector2[2]);
   double dotProductD    = DOT3(v1D, v2D);
   if (dotProductD != 0.0) {
      if (hasREntry) {
         dotProductD    /= (static_cast<double>(vector1[ReferenceForce::RIndex])*static_cast<double>(vector2[ReferenceForce::RIndex]));
      } else {
         double norm1    = DOT3(v1D, v1D);
         double norm2    = DOT3(v2D, v2D);
         dotProductD    /= sqrt(norm1*norm2);
      }
   }
   RealOpenMM dotProduct = static_cast<RealOpenMM>(dotProductD);

#else      

   RealOpenMM dotProduct = DOT3(vector1, vector2);
   if (dotProduct != zero) {
      if (hasREntry) {
         dotProduct       /= (vector1[ReferenceForce::RIndex]*vector2[ReferenceForce::RIndex]);
      } else {
         RealOpenMM norm1  = DOT3(vector1, vector1);
         RealOpenMM norm2  = DOT3(vector2, vector2);
         dotProduct       /= SQRT(norm1*norm2);
      }
   }

#endif
#undef USE_DOUBLE_FOR_NORMED_DOT_PRODUCT
      
   // clamp dot product to [-1,1]

   if (dotProduct > one) {
      dotProduct = one;
   } else if (dotProduct < -one) {
      dotProduct = -one;
   }

   return dotProduct;

}

/**---------------------------------------------------------------------------------------

   Get angle between two vectors

   @param  vector1            first vector
   @param  vector2            second vector
   @param  outputDotProduct   output cosine of angle between two vectors (optional)
   @param  hasREntry          if set, then vector1[ReferenceForce::RIndex] = norm of vector
                              defaults to 0 -> R unavailable

   @return cosine of angles in radians

   --------------------------------------------------------------------------------------- */

RealOpenMM ReferenceBondIxn::getAngleBetweenTwoVectors(RealOpenMM* vector1, RealOpenMM* vector2, 
                                                       RealOpenMM* outputDotProduct = NULL,
                                                       int hasREntry = 0) {

   // ---------------------------------------------------------------------------------------

   // static const std::string methodName = "\nReferenceBondIxn::getAngle";

   static const RealOpenMM zero        = 0.0;
   static const RealOpenMM one         = 1.0;

   // ---------------------------------------------------------------------------------------

   // get dot product betweenn vectors and then angle

   RealOpenMM dotProduct = getNormedDotProduct(vector1, vector2, hasREntry);

   RealOpenMM angle;
   if (dotProduct > (RealOpenMM) 0.99 || dotProduct < (RealOpenMM) -0.99) {
       // We're close to the singularity in acos(), so take the cross product and use asin() instead.

       RealOpenMM cross[3];
       SimTKOpenMMUtilities::crossProductVector3(vector1, vector2, cross);
       RealOpenMM scale = DOT3(vector1, vector1)*DOT3(vector2, vector2);
       angle = ASIN(SQRT(DOT3(cross, cross)/scale));
       if (dotProduct < zero)
           angle = (RealOpenMM) (M_PI-angle);
   } else {
      angle = ACOS(dotProduct);
   }

   if (outputDotProduct) {
      *outputDotProduct = dotProduct;
   }

   return angle;

}

/**---------------------------------------------------------------------------------------

   Get dihedral angle between three vectors

   @param  vector1            first vector
   @param  vector2            second vector
   @param  vector3            third vector
   @param  outputCrossProduct output cross product vectors
   @param  cosineOfAngle      cosine of angle (output)
   @param  signVector         vector to test sign (optional)
   @param  signOfAngle        sign of angle (output) (optional)
   @param  hasREntry          if set, then vector1[ReferenceForce::RIndex] = norm of vector
                              defaults to 0

   @return cosine of dihedral angle in radians

   --------------------------------------------------------------------------------------- */

RealOpenMM ReferenceBondIxn::getDihedralAngleBetweenThreeVectors(RealOpenMM*  vector1,
                                                                 RealOpenMM*  vector2, 
                                                                 RealOpenMM*  vector3, 
                                                                 RealOpenMM** outputCrossProduct  = NULL, 
                                                                 RealOpenMM*  cosineOfAngle       = NULL, 
                                                                 RealOpenMM*  signVector          = NULL, 
                                                                 RealOpenMM*  signOfAngle         = NULL, 
                                                                 int          hasREntry = 0) {

   // ---------------------------------------------------------------------------------------

   // static const std::string methodName = "\nReferenceBondIxn::getDihedralAngleBetweenThreeVectors";

   static const RealOpenMM zero        = 0.0;
   static const RealOpenMM one         = 1.0;

   RealOpenMM   tempVectors[6]         = { zero, zero, zero, zero, zero, zero };

   // ---------------------------------------------------------------------------------------

   // get cross products between vectors and then angle between cross product vectors

   RealOpenMM* crossProduct[2];
   if (outputCrossProduct) {
      crossProduct[0] = outputCrossProduct[0];
      crossProduct[1] = outputCrossProduct[1];
   } else {
      crossProduct[0] = tempVectors;
      crossProduct[1] = tempVectors + 3;
   }
   
   SimTKOpenMMUtilities::crossProductVector3(vector1, vector2, crossProduct[0]);
   SimTKOpenMMUtilities::crossProductVector3(vector2, vector3, crossProduct[1]);

   RealOpenMM angle         = getAngleBetweenTwoVectors(crossProduct[0], crossProduct[1], cosineOfAngle, 0);

   // take care of sign of angle

   if (signVector) {
      RealOpenMM dotProduct = DOT3(signVector, crossProduct[1]);
      RealOpenMM sign       = dotProduct < zero ? -one : one; 
      if (signOfAngle) {
         *signOfAngle = sign;
      }
      angle *= sign;
   }

   return angle;

}