/***************************************************************
 * PrLink.h
 *
 * This class provides a robot link.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 03/19/98: K.C. Chang: added homeT_.
 * 11/07/97: K.C. Chang: created.
 */

#ifndef _PrLink_h
#define _PrLink_h

#include "PrGlobalDefn.h"
#include "matrix/PrTransform.h"
#include "matrix/PrVector3.h"
#include "matrix/PrVector6.h"
#include "matrix/PrMatrix3.h"
#include "matrix/PrMatrix6.h"

#define PR_LINK_OK 1
#define PR_LINK_ERROR 0

#define PR_LINK_JOINT_PRISMATIC 0
#define PR_LINK_JOINT_REVOLUTE  1

#define PR_LINK_INV_DYNAMICS 0
#define PR_LINK_FWD_DYNAMICS 1

#define PR_LINK_AXIS_X 0
#define PR_LINK_AXIS_Y 1 
#define PR_LINK_AXIS_Z 2

class PrLink
{
  public:
    PrLink(int jointType=PR_LINK_JOINT_REVOLUTE,int jointAxis=PR_LINK_AXIS_Z);
    ~PrLink() {}

    void homePosition(Float alpha, Float a, Float d=0.0, Float theata=0.0);
    void homePosition(const PrTransform &home, Float d=0.0, Float theta=0.0);

    void state(Float q,Float dq,int dynamicsType=PR_LINK_INV_DYNAMICS);
    void state(Float q) { Position(q); }
    void invDynamics(Float ddq);
    void fwdDynamics(Float tau);

    Float jointForce();
    Float jointAcceleration();

    PrTransform &localTransform() { return localT_; }
    PrTransform &globalTransform() { return globalT_; }
    PrVector6 &globalJacobian(const PrVector3 &globalP);
    PrVector6 &globalJacobian() { return J_; }
    PrVector6 &A() { return A_; }
    PrVector6 &F() { return F_; }
    PrMatrix6 &Lambda() { return L_; }
  PrMatrix6 &I() { return I_; }

    void parent(PrLink *link) { parent_ = link; }
    PrLink *parent() { return parent_; }
    void child(PrLink *link) { child_ = link; }
    PrLink *child() { return child_; }
    void friction(Float viscousFriction,
		  Float dynamicCoulombFriction, Float staticCoulombFriction)
      { friction_.values(viscousFriction,
			 dynamicCoulombFriction,staticCoulombFriction); }

    void id(int num) { id_ = num; }
    int id() { return id_; }

    void articulatedOSIM();

    void display();

  void offset(const PrVector3 &os) { offset_ = os; }
  PrVector3 &offset() { return offset_; }
    void centerOfMass(const PrVector3 &com);
    void mass(Float m) { mass_ = m; }
    Float mass() { return mass_; }
    void inertia(Float Ixx,Float Iyy,Float Izz);
  PrVector3 &inertia() { return Inertia_; }
    void qMin(Float q) { qMin_ = q; }
    void qMax(Float q) { qMax_ = q; }
    Float qMin() const { return qMin_; }
    Float qMax() const { return qMax_; }
    void initInertiaMatrix();

    void similarityXform6(const PrMatrix6 &I,PrMatrix6 &XIXt);
    void similarityXform6inv(const PrMatrix6 &L,PrMatrix6 &XinvLXinvt);

  private:
    const int jointType_;
    const int jointAxis_;
    int id_;
    PrLink *parent_;
    PrLink *child_;
//    Float sinAlpha_,cosAlpha_,sinAlpha_2_,cosAlpha_2_;
    PrTransform homeT_;
    PrTransform localT_;
    PrTransform globalT_;

    int inertiaMatrixFlag_;
    PrVector3 offset_;
    PrVector3 friction_;
    PrVector3 COM_;
    Float prismaticCOM_;
    PrVector3 Inertia_;
    Float mass_;
    Float tau_;
    Float qMax_;
    Float qMin_;

    PrVector6 J_;    // global jacobian
    PrVector6 V_;
    PrVector6 A_;
    PrVector6 C_;
    PrVector6 Z_;    // bias force
    PrVector6 Fnet_;
    PrVector6 F_;

    PrVector3 Sdq_;
    PrVector3 Sddq_;

    PrMatrix6 L_;    // OSIM
    PrMatrix6 I_;
    PrMatrix6 Ia_;
    PrVector6 Za_;

    Float St_Za_plus_Ia_C_;
    Float inv_St_Ia_S_;
    PrVector6 Ia_S_;

    PrMatrix3 R_,Rt_,Px_;
    PrVector3 tmpV31,tmpV32,tmpV33;
    PrMatrix3 tmpM31,tmpM32;
    PrVector6 tmpV61,tmpV62;
    PrMatrix6 tmpM61,tmpM62;

    inline void UpdateLocalTranslation(Float q);
    inline void UpdateLocalRotation(Float q);
    inline void UpdateGlobalTransformation();

    void Position(Float q);
    void Velocity();
    void CoriolisCentrifugal();
    void BiasForce();
    void InitFwdDynamics();
    void Acceleration();
    void NetForce();
    void TotalForce();
    Float GetJointAcceleration();
    Float Friction();

    void ArticulatedInertiaMatrix();
    void ArticulatedBiasForce();

    void Xform6T(const PrVector6 &v6,PrVector6 &dest);
    void Xform6(const PrVector6 &f6,PrVector6 &dest);

    // copy is not allowed
    PrLink(const PrLink &) : jointType_(PR_LINK_JOINT_REVOLUTE), jointAxis_(PR_LINK_AXIS_Z) {}
};

#endif // _PrLink_h






