//-----------------------------------------------------------------------------
// File:     UserForceControlJointTorques.h
// Class:    UserForceControlHorizontal
// Parent:   GeneralForceElements
// Children: None
// Purpose:  Applies an air-resistance force on an object falling vertically.
// Author:   Paul Mitiguy - May 20, 2007
//-----------------------------------------------------------------------------
#ifndef __USERFORCEVCONTROLHORIZONTAL_H__
#define __USERFORCEVCONTROLHORIZONTAL_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 UserForceControlJointTorques : public GeneralForceElements::UserForce 
{
public:
   // Constructor is explicit
   explicit UserForceControlJointTorques( BodyId bodyIdA , BodyId bodyIdB, BodyId bodyIdC, Real damping, Real frequency)
   { 
      myShankId = bodyIdA;
      myThighId = bodyIdB;
      myHATId = bodyIdC;


      zeta = damping;
      omega = frequency;
   } 

   // The clone method is used internally by Simbody (required by virtual parent class)
   UserForce*  clone() const  { return new UserForceControlJointTorques(*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 );
        //const Vec3 bodyLocation = matter.calcBodyOriginLocationInBody( state, myBodyIdForApplyingForce, GroundId );
        //const Vec3 bodyAcceleration = matter.calcBodyOriginAccelerationInBody( state, myBodyIdForApplyingForce, GroundId );
        
		const Rotation shankRotationAboutGround = matter.calcBodyRotationFromBody(state, myShankId, GroundId);
		const Vec4 shankAngleAboutGround = shankRotationAboutGround.convertToAngleAxis();
		const Real shankAngleValueAboutGround = shankAngleAboutGround[0];
		const Vec3 shankAngVelVec = matter.calcBodyAngularVelocityInBody(state, myShankId, GroundId);
		const Real shankAngVel = sqrt(dot(shankAngVelVec,shankAngVelVec));

		const Rotation thighRotationAboutShank = matter.calcBodyRotationFromBody(state, myThighId, myShankId);
		const Vec4 thighAngleAboutShank = thighRotationAboutShank.convertToAngleAxis();
		const Real thighAngleValueAboutShank = thighAngleAboutShank[0];
		const Vec3 thighAngVelVec = matter.calcBodyAngularVelocityInBody(state, myThighId, myShankId);
		const Real thighAngVel = sqrt(dot(thighAngVelVec,thighAngVelVec));

		const Rotation HATRotationAboutThigh = matter.calcBodyRotationFromBody(state, myHATId, myThighId);
		const Vec4 HATAngleAboutThigh = HATRotationAboutThigh.convertToAngleAxis();
		const Real HATAngleValueAboutThigh = HATAngleAboutThigh[0];
		const Vec3 HATAngVelVec = matter.calcBodyAngularVelocityInBody(state, myHATId, myThighId);
		const Real HATAngVel = sqrt(dot(HATAngVelVec,HATAngVelVec));

		const Inertia shankInertia = matter.getBodyInertiaAboutBodyOrigin( state, myShankId );
		const Vec3 shankIMoments = shankInertia.getMoments()  ;
		const Real shankIzz = shankIMoments[2];

		const Inertia thighInertia = matter.getBodyInertiaAboutBodyOrigin( state, myThighId );
		const Vec3 thighIMoments = thighInertia.getMoments()  ;
		const Real thighIzz = thighIMoments[2];

		const Inertia HATInertia = matter.getBodyInertiaAboutBodyOrigin( state, myHATId );
		const Vec3 HATIMoments = HATInertia.getMoments()  ;
		const Real HATIzz = HATIMoments[2];
		
		//Real xActual = bodyLocation[0];
        //Real vActual = bodyVelocity[0];
        //Real aActual = bodyAcceleration[0];

        // Query the matter subsystem for the time in the simulation
        const Real time = state.getTime();

        // set desired position, velocity, and acceleration
        Real ankleAngleDes = -45*3.14/180;
		Real kneeAngleDes = 90*3.14/180;
		Real hipAngleDes = -45*3.14/180;

		Real ankleRotVelDes = 0.0;
		Real kneeRotVelDes = 0.0;
		Real hipRotVelDes = 0.0;

		Real ankleRotAccDes = 0.0;
		Real kneeRotAccDes = 0.0;
		Real hipRotAccDes = 0.0;


        // Set control constants
        Real kD = 2*omega;
        Real kP = omega*omega;

        // Define force according to control law
		Real ankleTorque = shankIzz*( ankleRotAccDes + kD*(ankleRotVelDes - shankAngVel)+ kP*(ankleAngleDes - shankAngleValueAboutGround));
		Real kneeTorque = thighIzz*( kneeRotAccDes + kD*(kneeRotVelDes - thighAngVel)+ kP*(kneeAngleDes - thighAngleValueAboutShank));
		Real hipTorque = HATIzz*( hipRotAccDes + kD*(hipRotVelDes - HATAngVel)+ kP*(hipAngleDes - HATAngleValueAboutThigh));


        // 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& shankBodiesForces = bodyForces[ myShankId ];
        Vec3& shankTorqueSum = shankBodiesForces[0];
        Vec3& shankForceSum  = shankBodiesForces[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.
        shankTorqueSum[2] += ankleTorque;       // Increment torque in the ground's z-direction.
        // forceSum[0]  += horizontalForce;       // Increment force  in the ground's x-direction.
        // forceSum[1]  += yForce;     // Increment force  in the ground's y-direction.
        // forceSum[0]  += 0;       // Increment force  in the ground's z-direction.

		SpatialVec& thighBodiesForces = bodyForces[ myThighId ];
        Vec3& thighTorqueSum = thighBodiesForces[0];
        Vec3& thighForceSum  = thighBodiesForces[1];
		thighTorqueSum[2] += kneeTorque; 


		SpatialVec& HATBodiesForces = bodyForces[ myHATId ];
        Vec3& HATTorqueSum = HATBodiesForces[0];
        Vec3& HATForceSum  = HATBodiesForces[1];
		HATTorqueSum[2] += hipTorque; 
   }

private:
    BodyId  myShankId;
	BodyId	myThighId;
	BodyId	myHATId;
    Real    zeta;
    Real    omega;

};
//-----------------------------------------------------------------------------
#endif /* __USERFORCEVCONTROLHORIZONTAL_H__ */
//-----------------------------------------------------------------------------