//-----------------------------------------------------------------------------
// File:     UserForceAttraction.h
// Class:    UserForceAttraction
// Parent:   GeneralForceElements
// Children: None
// Purpose:  Applies attraction toward the binding site on actin, plus damping
// Author:   Mary Elting - May 30, 2007
//-----------------------------------------------------------------------------
#ifndef __USERFORCEATTRACTION_H__
#define __USERFORCEATTRACTION_H__
//-----------------------------------------------------------------------------
#include "StandardCppHeadersAndNamespace.h"
#include "SimTKsimbody.h"
using namespace SimTK;

//--------------------------------------------------------------------------
// User-defined classes for adding forces/torques are constructed as follows:
// 1. Create a constructor with whatever arguments make sense for the force or torque and copy the arguments into class data.
// 2. Create a clone method (all clone methods are identical except for the class name appearing after "new")
// 3. Create a calc  method (the arguments and return type for all calc methods are identical).
//    The code in the calc method is specific to the calculation of force or torque.
// Note: The set of all forces is replaced by an equivalent set, consisting of a torque
//       that is equal to the moment of the forces about the body's origin together
//       with the resultant of the forces applied at the body's origin.
//--------------------------------------------------------------------------
class UserForceAttraction : public GeneralForceElements::UserForce 
{
public:
   // Constructor is explicit
   explicit UserForceAttraction( BodyId bodyIdA, Vec3 actinBindingSite, Real c, Vec3 headInBody, Real maxForce, Real dampingCoeff)
   { 
      myBodyIdForApplyingForce = bodyIdA;
      
      //Vec3 to the binding site
      myActinBindingSite = actinBindingSite;

      //Attraction coeffiecient
      myC = c;

      myHeadInBody = headInBody;

      myMaxForce = maxForce;
      myDampingCoeff = dampingCoeff;
   } 

   // The clone method is used internally by Simbody (required by virtual parent class)
   UserForce*  clone() const  { return new UserForceAttraction(*this); }

   // The calc method is where forces or torques are calculated (required by virtual parent class)
   void  calc( const MatterSubsystem& matter,              // Input information (matter)
               const State&           state,               // Input information (current state)
               Vector_<SpatialVec>&   bodyForces,          // Forces and torques on bodies
               Vector_<Vec3>&         particleForces,      // Forces on particles (currently unused)
               Vector&                mobilityForces,      // Generalized forces 
               Real&                  pe ) const           // For forces with a potential energy
    {
       //Get the body origin location in ground
       Vec3 headInGround = matter.calcBodyOriginLocationInBody(state, myBodyIdForApplyingForce, GroundId);
       Vec3 vectorToBindingSite = myActinBindingSite - headInGround;

       Real distanceSquared = dot(vectorToBindingSite, vectorToBindingSite);

       // this gives a force that falls off as r^6
       Real distanceToSeven = pow( distanceSquared,3.5 );
       Vec3 myForce = myC * vectorToBindingSite / distanceToSeven;

       Real forceMag = myForce.norm();

       Real distance = vectorToBindingSite.norm();
       //don't exceed maxForce (prevent divide by zero problem)
       if(forceMag > myMaxForce){

          myForce = myMaxForce * vectorToBindingSite / (distance + 1E-300);//prevent divide by 0 problems
       }

       //calculate damping force
       Vec3 headVelocityInGround = matter.calcBodyOriginVelocityInBody(state, myBodyIdForApplyingForce, GroundId);
       Real distanceDotVelocity = dot(headVelocityInGround, vectorToBindingSite);

       Vec3 dampingForce = -myDampingCoeff*vectorToBindingSite*distanceDotVelocity/distanceToSeven;

       //Don't exceed maxForce (otherwise /0 problem)
       if(dampingForce.norm() > myMaxForce){
          dampingForce = -myMaxForce * vectorToBindingSite / (distance + 1E-300);//prevent divide by 0 problems
       }

       myForce += dampingForce;

        // Get the proper memory location to increment the force and/or torque.
        // bodiesForces is a Vec6 whose elements are two Vec3.
        // The elements of the first  Vec3 are Tx, Ty, Tz  (expressed in the ground's "x,y,z").
        // The elements of the second Vec3 are Fx, Fy, Fz  (expressed in the ground's "x,y,z").
        SpatialVec& bodiesForces = bodyForces[ myBodyIdForApplyingForce ];
        Vec3& torqueSum = bodiesForces[0];
        Vec3& forceSum  = bodiesForces[1];

        // Increment the sum of all forces on this body (other force subsystems may also add forces/torque)
        // torqueSum += torque;   // Increment torque in the ground's x,y,z direction.
        forceSum  += myForce;     // Increment force  in the ground's x,y,z direction.
    }

private:
    BodyId  myBodyIdForApplyingForce;
    Vec3    myActinBindingSite;
    Real    myC;
    Vec3    myHeadInBody;
    Real    myMaxForce;
    Real    myDampingCoeff;

};
//-----------------------------------------------------------------------------
#endif /* __USERFORCEATTRACTION_H__ */
//-----------------------------------------------------------------------------