/***************************************************************************
 *   Copyright (C) 2005 by Robot Group Leipzig                             *
 *    martius@informatik.uni-leipzig.de                                    *
 *    fhesse@informatik.uni-leipzig.de                                     *
 *    der@informatik.uni-leipzig.de                                        *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************
 *                                                                         *
 *  Joint wrapper to ba able to draw joints and abstract from ode details  *
 *                                                                         *
 *   $Log: joint.h,v $
 *   Revision 1.7  2007/07/03 13:03:07  martius
 *   assert for parts
 *   getPartX also as non-Const
 *
 *   Revision 1.6  2007/03/16 11:00:41  martius
 *   registration of ignored geoms
 *
 *   Revision 1.5  2007/01/26 12:05:36  martius
 *   joint support forces in uniform manner
 *
 *   Revision 1.4  2006/08/02 10:11:27  martius
 *   getNumberAxes was mistyped
 *
 *   Revision 1.3  2006/07/26 10:36:05  martius
 *   joints support getPositions and getNumberAxes
 *
 *   Revision 1.2  2006/07/14 12:23:35  martius
 *   selforg becomes HEAD
 *
 *   Revision 1.1.2.12  2006/06/29 16:35:56  robot3
 *   includes cleared up
 *
 *   Revision 1.1.2.11  2006/03/29 15:05:57  martius
 *   fixed joint
 *
 *   Revision 1.1.2.10  2006/02/01 18:34:03  martius
 *   use Axis type for Joint axis. very important, since otherwise Vec3 * pose is not the right direction vector anymore
 *
 *   Revision 1.1.2.9  2006/01/12 22:19:08  martius
 *   bugfix in TwoAxisJoint Constructor (axis and anchor swapped)
 *
 *   Revision 1.1.2.8  2006/01/11 14:11:06  fhesse
 *   moved anchor up into Joint and introduced getAnchor()
 *
 *   Revision 1.1.2.7  2005/12/21 15:39:03  martius
 *   OneAxisJoint and TwoAxisJoint as superclasses
 *
 *   Revision 1.1.2.6  2005/12/19 16:34:18  martius
 *   added Ball and Universal joint
 *
 *   Revision 1.1.2.5  2005/12/15 17:03:42  martius
 *   cameramanupulator setPose is working
 *   joints have setter and getter parameters
 *   Primitives are not longer inherited from OSGPrimitive, moreover
 *   they aggregate them
 *
 *   Revision 1.1.2.4  2005/12/14 15:36:45  martius
 *   joints are visible now
 *
 *   Revision 1.1.2.3  2005/12/13 18:11:13  martius
 *   transform primitive added, some joints stuff done, forward declaration
 *
 *   Revision 1.1.2.2  2005/12/12 23:40:22  martius
 *   hinge2joint started
 *
 *   Revision 1.1.2.1  2005/12/12 22:25:16  martius
 *   added joint
 *
 *
 *                                                                 *
 ***************************************************************************/
#ifndef __JOINT_H
#define __JOINT_H

#include <assert.h>
#include <list>

#include "primitive.h"
#include "osgforwarddecl.h"
#include "axis.h"
#include "osghandle.h"
#include "odehandle.h"

namespace lpzrobots {


  /***************************************************************************/

  class Joint {
  public: 
    Joint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor) 
      : joint(0), part1(part1), part2(part2), anchor(anchor) {
      assert(part1 && part2);
    }
    virtual ~Joint();
    /** initialises (and creates) the joint. If visual is true then the joints is
	also drawn. visualSize is the size of the visual representation.
	Also member variable odeHandle should be set.
	(To be overloaded, but this init should be called always from children!)
    */
    virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
		      bool withVisual = true, double visualSize = 0.2);
    
    /// should syncronise the Ode stuff and the OSG notes (if any)
    virtual void update() = 0;
    /// sets the ODE joint parameter (see ODE manual)
    virtual void setParam(int parameter, double value) = 0;
    /// return the ODE joint parameter (see ODE manual)
    virtual double getParam(int parameter) const = 0;
    
    dJointID getJoint() const  { return joint; }
    const Primitive* getPart1() const { return part1; }
    Primitive* getPart1() { return part1; }
    const Primitive* getPart2() const { return part2; } 
    Primitive* getPart2() { return part2; } 
    const osg::Vec3 getAnchor() const { return anchor; }

    /// returns the number of Axes
    virtual int getNumberAxes() const = 0;
    /// returns the positions of all Axes
    virtual std::list<double> getPositions() const { return std::list<double>(); }
    /// returns the position rates of all Axes
    virtual std::list<double> getPositionRates() const { return std::list<double>(); }
    /// stores the positions of all Axes into sensorarray and returns the number of written entries
    virtual int getPositions(double* sensorarray) const { return 0; }
    /** stores the position rates of all Axes into sensorarray and 
	returns the number of written entries
     */
    virtual int getPositionRates(double* sensorarray) const { return 0; }
    
    static osg::Matrix anchorAxisPose(const osg::Vec3& anchor, const Axis& axis);
  protected:
    dJointID joint;
    Primitive* part1;
    Primitive* part2;    
    osg::Vec3 anchor;
  public:
    OdeHandle odeHandle;
  };

  class OneAxisJoint : public Joint {
  public:
    OneAxisJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor, const Axis axis1) 
      : Joint(part1, part2, anchor), axis1(axis1) {}
    virtual Axis getAxis1() const { return axis1; };
    
    virtual double getPosition1() const = 0;
    virtual double getPosition1Rate() const = 0;
    virtual void addForce1(double force) = 0;    

    virtual int getNumberAxes() const { return 1;};
    virtual std::list<double> getPositions() const;
    virtual std::list<double> getPositionRates() const;
    virtual int getPositions(double* sensorarray) const;
    virtual int getPositionRates(double* sensorarray) const;            
  protected:
    Axis axis1;
  };

  class TwoAxisJoint : public OneAxisJoint {
  public:
    TwoAxisJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor, const Axis axis1, 
		 const Axis axis2 ) 
      : OneAxisJoint(part1, part2, anchor, axis1), axis2(axis2) {}
    virtual Axis getAxis2() const { return axis2; };

    virtual double getPosition2() const = 0;
    virtual double getPosition2Rate() const = 0;
    virtual void addForce2(double force) = 0;
    void addForces(double force1,double force2){
      addForce1(force1); addForce2(force2); 
    }

    virtual int getNumberAxes() const { return 2;};
    virtual std::list<double> getPositions() const;
    virtual std::list<double> getPositionRates() const;
    virtual int getPositions(double* sensorarray) const;
    virtual int getPositionRates(double* sensorarray) const;

  protected:
    Axis  axis2;
  };

  /***************************************************************************/

  class FixedJoint : public Joint {
  public:
    FixedJoint(Primitive* part1, Primitive* part2);

    virtual ~FixedJoint();

    /** initialises (and creates) the joint. 
    */
    virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
		      bool withVisual = true, double visualSize = 0.2);
    
    virtual void update();    
    virtual void setParam(int parameter, double value);
    virtual double getParam(int parameter) const;

    virtual int getNumberAxes() const { return 0; }

  };


  /***************************************************************************/

  class HingeJoint : public OneAxisJoint {
  public:
    HingeJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor, 
		const Axis& axis1);

    virtual ~HingeJoint();

    /** initialises (and creates) the joint. If visual is true then the axis of the joints is
	also drawn as a slim cylinder. visualSize is the length of the cylinder.
    */
    virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
		      bool withVisual = true, double visualSize = 0.2);
    
    virtual void update();    

    virtual void addForce1(double t);
    virtual double getPosition1() const;
    virtual double getPosition1Rate() const;
    virtual void setParam(int parameter, double value);
    virtual double getParam(int parameter) const;
    
  protected:
    OSGPrimitive* visual;
  };

  /***************************************************************************/
  
  class Hinge2Joint : public TwoAxisJoint {
  public:
    Hinge2Joint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor, 
		const Axis& axis1, const Axis& axis2);

    virtual ~Hinge2Joint();

    /** initialises (and creates) the joint. If visual is true then axis2 of the joints is
	also drawn as a slim cylinder. visualSize is the length of the cylinder.
    */
    virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
		      bool withVisual = true, double visualSize = 0.2);
    
    virtual void update();    

    /// adds torques to axis 1 and 2
    virtual void addForce1(double t1);
    virtual void addForce2(double t2);
    virtual double getPosition1() const;
    virtual double getPosition2() const; /// This is not supported by the joint!
    virtual double getPosition1Rate() const;
    virtual double getPosition2Rate() const;
    virtual void setParam(int parameter, double value);
    virtual double getParam(int parameter) const;
    
  protected:
    OSGPrimitive* visual;
  };

  /***************************************************************************/

  class UniversalJoint : public TwoAxisJoint {
  public:
    UniversalJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor, 
		const Axis& axis1, const Axis& axis2);

    virtual ~UniversalJoint();

    /** initialises (and creates) the joint. If visual is true then axix1 and axis2 of the joints is
	also drawn as a slim cylinder. visualSize is the length of the cylinder.
    */
    virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
		      bool withVisual = true, double visualSize = 0.2);
    
    virtual void update();    

    /// adds torques to axis 1 and 2
    virtual void addForce1(double t1);
    virtual void addForce2(double t2);
    virtual double getPosition1() const;
    virtual double getPosition2() const;
    virtual double getPosition1Rate() const;
    virtual double getPosition2Rate() const;

    virtual void setParam(int parameter, double value);
    virtual double getParam(int parameter) const;
    
  protected:
    OSGPrimitive* visual1;
    OSGPrimitive* visual2;
  };

  /***************************************************************************/

  class BallJoint : public Joint {
  public:
    BallJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor);

    virtual ~BallJoint();

    /** initialises (and creates) the joint. 
	If visual is true then ball is drawn as a sphere with radius of visualSize.
    */
    virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
		      bool withVisual = true, double visualSize = 0.2);
    
    virtual void update();    

    virtual int getNumberAxes() const { return 0; }
    // Ball and Socket has no parameter
    virtual void setParam(int parameter, double value);
    virtual double getParam(int parameter) const;
    
  protected:
    OSGPrimitive* visual;
  };

  /***************************************************************************/

  class SliderJoint : public OneAxisJoint {
  public:
    SliderJoint(Primitive* part1, Primitive* part2, const osg::Vec3& anchor, 
		const Axis& axis1);

    virtual ~SliderJoint();

    /** initialises (and creates) the joint. If visual is true then the axis of the joints is
	also drawn as a slim cylinder. VisualSize is added to the lenght of the slider and is used
	for the length of the cylinder. The radius is visualSize/10
    */
    virtual void init(const OdeHandle& odeHandle, const OsgHandle& osgHandle,
		      bool withVisual = true, double visualSize = 0.1);
    
    virtual void update();    

    virtual void addForce1(double t);
    virtual double getPosition1() const;
    virtual double getPosition1Rate() const;
    virtual void setParam(int parameter, double value);
    virtual double getParam(int parameter) const;
    
  protected:
    OSGPrimitive* visual;
    double visualSize;
    OsgHandle osgHandle;
  };

  /***************************************************************************/



}

#endif
