//-----------------------------------------------------------------------------
// File:     UserForceControlHorizontal.h
// Class:    UserForceControlHorizontal
// Parent:   GeneralForceElements
// Children: None
// Purpose:  Applies an air-resistance force on an object falling vertically.
// Author:   Paul Mitiguy - May 20, 2007
// Modified: MDF 5/30/07
//-----------------------------------------------------------------------------
#ifndef __USERFORCECONTROLHORIZONTAL_H__
#define __USERFORCECONTROLHORIZONTAL_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 UserForceControlHorizontal : public GeneralForceElements::UserForce 
{
public:
   // Constructor is explicit
   explicit UserForceControlHorizontal( BodyId bodyIdA, Real massOfObject, Real dampingRatio, Real naturalFrequency )
   { 
      myBodyIdForApplyingForce = bodyIdA;
      myMassOfObject = massOfObject;
      myDampingRatio = dampingRatio;
      myNaturalFrequency = naturalFrequency;
   } 

   // The clone method is used internally by Simbody (required by virtual parent class)
   UserForce*  clone() const  { return new UserForceControlHorizontal(*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
    {
        // Query the matter subsystem for the body's origin velocity in ground.
        // This vector is expressed in the ground's "x,y,z" unit vectors.
        const Vec3 bodyVelocity = matter.calcBodyOriginVelocityInBody( state, myBodyIdForApplyingForce, GroundId );
        Real xVelocity = bodyVelocity[0];

        // Query the matter subsystem for the body's origin location in ground.
        // This vector is expressed in the ground's "x,y,z" unit vectors.
        const Vec3 bodyLocation = matter.calcBodyOriginLocationInBody( state, myBodyIdForApplyingForce, GroundId );
        Real xLocation = bodyLocation[0];

        // Get time of current state.
        const Real& myTime = state.getTime();

        // Calculate desired location, velocity, and acceleration.
        Real xLocationDesired = 2 + sin(myTime);
        Real xVelocityDesired = cos(myTime);
        Real xAccelerationDesired = -sin(myTime);

        // Calculate constant gains kd and kp.
        Real kd = 2 * myNaturalFrequency;
        Real kp = pow(myNaturalFrequency, 2);

        // Calculate magnitude of force to be applied.
        Real xForce = myMassOfObject * ( xAccelerationDesired + kd*(xVelocityDesired-xVelocity) + kp*(xLocationDesired-xLocation) );

        // 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[0] += 0;       // Increment torque in the ground's x-direction.
        // torqueSum[1] += 0;       // Increment torque in the ground's y-direction.
        // torqueSum[2] += 0;       // Increment torque in the ground's z-direction.
        forceSum[0]  += xForce;       // Increment force  in the ground's x-direction.
        // forceSum[1]  += yForce;     // Increment force  in the ground's y-direction.
        // forceSum[2]  += 0;       // Increment force  in the ground's z-direction.
    }

private:
    BodyId  myBodyIdForApplyingForce;
    Real myMassOfObject;
    Real myDampingRatio;
    Real myNaturalFrequency;
};
//-----------------------------------------------------------------------------
#endif /* __USERFORCECONTROLHORIZONTAL_H__ */
//-----------------------------------------------------------------------------