// RBDL model for tip link to body chain
// Author: Max Schwarz <max.schwarz@uni-bonn.de>
//         Philipp Allgeuer <pallgeuer@ais.uni-bonn.de>

// Ensure header is only included once
#ifndef SINGLESUPPORTMODEL_H
#define SINGLESUPPORTMODEL_H

// Includes
#include <rbdl/rbdl_parser.h>
#include <boost/shared_ptr.hpp>
#include <robotcontrol/model/joint.h>

// Namespace aliases
namespace Math = RigidBodyDynamics::Math;

// Robotcontrol namespace
namespace robotcontrol
{

// Class forward declarations
class RobotModel;

/**
 * @brief RBDL model for tip link to body chain
 **/
class SingleSupportModel : public rbdl_parser::URDF_RBDL_Model
{
public:
	// Data source enumeration
	enum DataSource
	{
		MeasurementData,
		CommandData
	};

	// Constructor (link is the chosen URDF link to use as the root of the single support RBDL model)
	SingleSupportModel(RobotModel* model, const boost::shared_ptr<const urdf::Link>& link);

	// Initialisation function (root is normally link->name from the call to the constructor)
	virtual bool initFrom(const urdf::Model& model, const std::string& root = "");
	
	// Get function for URDF root link
	inline boost::shared_ptr<const urdf::Link> link() const { return m_link; }

	// Joint functions
	inline Joint::Ptr joint(int idx) { return m_joints[idx]; }
	void setJointCmd(int idx, double pos); // Sets the commanded position for a joint based on position only (see Joint::setFrom*() for alternatives)

	// Support coefficient functions
	inline void setCoeff(double coeff) { m_coeff = (coeff >= 0.0 ? coeff : 0.0); }
	inline void normCoeff(double coeffSum) { m_normedCoeff = (coeffSum <= 0.0 ? 0.0 : m_coeff / coeffSum); }
	inline double coeff() const { return m_coeff; }
	inline double normedCoeff() const { return m_normedCoeff; }

	// Update the internally stored joint states
	void updateRBDLJointPos(DataSource source);
	void updateMeasuredTorques();

	// Perform an inverse dynamics calculation on the single support model
	double doInverseDynamics(DataSource source, bool useCoeff, bool applyVelAcc, bool applyGrav);

	// Computation functions for calculated data
	void computeCoM();
	void computeZMP();

	// Get functions for calculated data
	inline Math::Vector3d centerOfMass() const { return m_com; }
	inline Math::Vector3d zeroMomentPoint() const { return m_zmp; }
	inline Math::Vector3d torqueZeroMomentPoint() const { return m_zmpTorque; }
	inline Math::Vector3d footForce() const { return m_footForce; }

	// Extra kinematics and dynamics functions
	Math::MatrixNd pointJacobian(unsigned int body, const Math::Vector3d& pos); //!< Calculate the jacobian matrix (3xNDof) for some point on a body
	Math::Vector3d estimateContactForce(unsigned int body, const Math::Vector3d& pos); //!< Estimate the force generated by joint torques

protected:
	// Set up a joint in the single support model (called by the base initFrom() implementation, overrides the implementation in rbdl_parser::URDF_RBDL_Model)
	virtual void setupJoint(unsigned int index, const urdf::Joint& urdf, bool reverse);
	
private:
	// Robot model and joints
	RobotModel* m_model; // Pointer to the applicable robot model
	boost::shared_ptr<const urdf::Link> m_link; // Shared pointer to the URDF link used as the root of this single support RBDL model
	std::vector<Joint::Ptr> m_joints; // List of joints in the model (in hierarchical tree order from the chosen root link onwards)
	
	// Support coefficients
	double m_coeff; // Coefficient as set from externally (must be non-negative)
	double m_normedCoeff; // Internally calculated normalised coefficient based on an externally provided coefficient sum for a particular robot model
	
	// Generalised vectors for passing, retrieving and storing joint positions, velocities, accelerations and torques
	Math::VectorNd m_q;
	Math::VectorNd m_qdot;
	Math::VectorNd m_qdotdot;
	Math::VectorNd m_tau;
	Math::VectorNd m_measuredTorque;

	// Extra vectors for calculated data
	Math::Vector3d m_com;
	Math::Vector3d m_zmp;
	Math::Vector3d m_zmpTorque;
	Math::Vector3d m_footForce;
};

}

#endif
//EOF