/*****************************************************************************
 * \file
 *      This file contains the definition of classes for a
 *      Rall Algebra of (subset of) the classes defined in frames,
 *      i.e. classes that contain a pair (value,derivative) and define operations on that pair
 *      this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
 *      Defines VectorVel, RotationVel, FrameVel.  Look at Frames.h for details on how to work
 *      with Frame objects.
 *  \author
 *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
 *
 *  \version
 *      ORO_Geometry V0.2
 *
 *  \par History
 *      - $log$
 *
 *  \par Release
 *      $Id: framevel.hpp 19905 2009-04-23 13:29:54Z ben2610 $
 *      $Name:  $
 ****************************************************************************/

#ifndef KDL_FRAMEVEL_H
#define KDL_FRAMEVEL_H

#include "utilities/utility.h"
#include "utilities/rall1d.h"
#include "utilities/traits.h"

#include "frames.hpp"



namespace KDL {

typedef Rall1d<double> doubleVel;

IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
	return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
}

IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
	return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
}

IMETHOD void random(doubleVel& F) {
	random(F.t);
	random(F.grad);
}
IMETHOD void posrandom(doubleVel& F) {
	posrandom(F.t);
	posrandom(F.grad);
}

}

template <>
struct Traits<KDL::doubleVel> {
	typedef double valueType;
	typedef KDL::doubleVel derivType;
};

namespace KDL {

class TwistVel;
class VectorVel;
class FrameVel;
class RotationVel;

class VectorVel
// = TITLE
//     An VectorVel is a Vector and its first derivative
// = CLASS TYPE
//     Concrete
{
public:
    Vector p;       // position vector
    Vector v;       // velocity vector
public:
    VectorVel():p(),v(){}
    VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
    explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}

    Vector value() const { return p;}
    Vector deriv() const { return v;}

    IMETHOD VectorVel& operator = (const VectorVel& arg);
    IMETHOD VectorVel& operator = (const Vector& arg);
    IMETHOD VectorVel& operator += (const VectorVel& arg);
    IMETHOD VectorVel& operator -= (const VectorVel& arg);
    IMETHOD static VectorVel Zero();
    IMETHOD void ReverseSign();
    IMETHOD doubleVel Norm() const;
    IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
    IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
    IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
    IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
    IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
    IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
    IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
    IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
    IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
    IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
    IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
    IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
    IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
    IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);

    IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
    IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
    IMETHOD friend void SetToZero(VectorVel& v);


    IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
    IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
    IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
    IMETHOD friend VectorVel operator - (const VectorVel& r);
    IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
    IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
    IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
};



class RotationVel
// = TITLE
//     An RotationVel is a Rotation and its first derivative, a rotation vector
// = CLASS TYPE
//     Concrete
{
public:
    Rotation R; // Rotation matrix
    Vector   w; // rotation vector
public:
    RotationVel():R(),w() {}
    explicit RotationVel(const Rotation& R_):R(R_),w(Vector::Zero()){}
    RotationVel(const Rotation& R_,const Vector& _w):R(R_),w(_w){}


    Rotation value() const { return R;}
    Vector   deriv() const { return w;}


    IMETHOD RotationVel& operator = (const RotationVel& arg);
    IMETHOD RotationVel& operator = (const Rotation& arg);
	IMETHOD VectorVel UnitX() const;
	IMETHOD VectorVel UnitY() const;
	IMETHOD VectorVel UnitZ() const;
    IMETHOD static RotationVel Identity();
    IMETHOD RotationVel Inverse() const;
    IMETHOD VectorVel Inverse(const VectorVel& arg) const;
    IMETHOD VectorVel Inverse(const Vector& arg) const;
    IMETHOD VectorVel operator*(const VectorVel& arg) const;
    IMETHOD VectorVel operator*(const Vector& arg) const;
    IMETHOD void DoRotX(const doubleVel& angle);
    IMETHOD void DoRotY(const doubleVel& angle);
    IMETHOD void DoRotZ(const doubleVel& angle);
    IMETHOD static RotationVel RotX(const doubleVel& angle);
    IMETHOD static RotationVel RotY(const doubleVel& angle);
    IMETHOD static RotationVel RotZ(const doubleVel& angle);
    IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
    // rotvec has arbitrary norm
    // rotation around a constant vector !
    IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
    // rotvec is normalized.
    // rotation around a constant vector !
    IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
    IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
    IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
    IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
    IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
    IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);

    IMETHOD TwistVel Inverse(const TwistVel& arg) const;
    IMETHOD TwistVel Inverse(const Twist& arg) const;
    IMETHOD TwistVel operator * (const TwistVel& arg) const;
    IMETHOD TwistVel operator * (const Twist& arg) const;
};




class FrameVel
// = TITLE
//     An FrameVel is a Frame and its first derivative, a Twist vector
// = CLASS TYPE
//     Concrete
// = CAVEATS
//
{
public:
    RotationVel M;
    VectorVel   p;
public:
    FrameVel(){}

    explicit FrameVel(const Frame& T_):
        M(T_.M),p(T_.p) {}

    FrameVel(const Frame& T_,const Twist& _t):
        M(T_.M,_t.rot),p(T_.p,_t.vel) {}

    FrameVel(const RotationVel& _M,const VectorVel& _p):
        M(_M),p(_p) {}


    Frame value() const { return Frame(M.value(),p.value());}
    Twist deriv() const { return Twist(p.deriv(),M.deriv());}


    IMETHOD FrameVel& operator = (const Frame& arg);
    IMETHOD FrameVel& operator = (const FrameVel& arg);
    IMETHOD static FrameVel Identity();
    IMETHOD FrameVel Inverse() const;
    IMETHOD VectorVel Inverse(const VectorVel& arg) const;
    IMETHOD VectorVel operator*(const VectorVel& arg) const;
    IMETHOD VectorVel operator*(const Vector& arg) const;
    IMETHOD VectorVel Inverse(const Vector& arg) const;
    IMETHOD Frame GetFrame() const;
    IMETHOD Twist GetTwist() const;
    IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
    IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
    IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
    IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
    IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
    IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);

    IMETHOD TwistVel  Inverse(const TwistVel& arg) const;
    IMETHOD TwistVel  Inverse(const Twist& arg) const;
    IMETHOD TwistVel operator * (const TwistVel& arg) const;
    IMETHOD TwistVel operator * (const Twist& arg) const;
};





//very similar to Wrench class.
class TwistVel
// = TITLE
// This class represents a TwistVel. This is a velocity and rotational velocity together
{
public:
    VectorVel vel;
    VectorVel rot;
public:

// = Constructors
    TwistVel():vel(),rot() {};
    TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
    TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
    TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};

    Twist value() const {
        return Twist(vel.value(),rot.value());
    }
    Twist deriv() const {
        return Twist(vel.deriv(),rot.deriv());
    }
// = Operators
     IMETHOD TwistVel& operator-=(const TwistVel& arg);
     IMETHOD TwistVel& operator+=(const TwistVel& arg);

// = External operators
     IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
     IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
     IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);

     IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
     IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
     IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);

     IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
     IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
     IMETHOD friend TwistVel operator-(const TwistVel& arg);
     IMETHOD friend void SetToZero(TwistVel& v);


// = Zero
     static IMETHOD TwistVel Zero();

// = Reverse Sign
     IMETHOD void ReverseSign();

// = Change Reference point
     IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
     // Changes the reference point of the TwistVel.
     // The VectorVel v_base_AB is expressed in the same base as the TwistVel
     // The VectorVel v_base_AB is a VectorVel from the old point to
     // the new point.
     // Complexity : 6M+6A

     // = Equality operators
     // do not use operator == because the definition of Equal(.,.) is slightly
     // different.  It compares whether the 2 arguments are equal in an eps-interval
     IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
     IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
     IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);

// = Conversion to other entities
     IMETHOD Twist GetTwist() const;
     IMETHOD Twist GetTwistDot() const;
// = Friends
    friend class RotationVel;
    friend class FrameVel;

};

IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
	return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
}

IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
	return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
}
IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
	return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
}

IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
	return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
}

IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
	return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
}

IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
	return FrameVel(
			addDelta(a.M,da.rot,dt),
			addDelta(a.p,da.vel,dt)
		   );
}

IMETHOD void random(VectorVel& a) {
	random(a.p);
	random(a.v);
}
IMETHOD void random(TwistVel& a) {
	random(a.vel);
	random(a.rot);
}

IMETHOD void random(RotationVel& R) {
	random(R.R);
	random(R.w);
}

IMETHOD void random(FrameVel& F) {
	random(F.M);
	random(F.p);
}
IMETHOD void posrandom(VectorVel& a) {
	posrandom(a.p);
	posrandom(a.v);
}
IMETHOD void posrandom(TwistVel& a) {
	posrandom(a.vel);
	posrandom(a.rot);
}

IMETHOD void posrandom(RotationVel& R) {
	posrandom(R.R);
	posrandom(R.w);
}

IMETHOD void posrandom(FrameVel& F) {
	posrandom(F.M);
	posrandom(F.p);
}

#ifdef KDL_INLINE
#include "framevel.inl"
#endif

} // namespace

#endif