/*
 * CopyPose.h
 *
 *  Created on: Mar 17, 2009
 *      Author: benoit bolsee
 */

#ifndef COPYPOSE_H_
#define COPYPOSE_H_

#include "ConstraintSet.hpp"
namespace iTaSC{

using namespace KDL;

class CopyPose: public iTaSC::ConstraintSet
{
protected:
    virtual void updateKinematics(const Timestamp& timestamp);
    virtual void pushCache(const Timestamp& timestamp);
    virtual void updateJacobian();
    virtual bool initialise(Frame& init_pose);
    virtual void initCache(Cache *_cache);
    virtual void updateControlOutput(const Timestamp& timestamp);
	virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
	virtual double getMaxTimestep(double& timestep);

public:
    enum ID {		// constraint ID in callback and setControlParameter
		ID_POSITION=0,
        ID_POSITIONX=1,
        ID_POSITIONY=2,
        ID_POSITIONZ=3,
		ID_ROTATION=4,
        ID_ROTATIONX=5,
        ID_ROTATIONY=6,
        ID_ROTATIONZ=7,
    };
	enum CTL {		// control ID in constructor to specify which output is constrainted
		CTL_NONE=0x00,
        CTL_POSITIONX=0x01,		// the bit order is important: it matches the y output order
        CTL_POSITIONY=0x02,
        CTL_POSITIONZ=0x04,
		CTL_POSITION=0x07,
        CTL_ROTATIONX=0x08,
        CTL_ROTATIONY=0x10,
        CTL_ROTATIONZ=0x20,
		CTL_ROTATION=0x38,
		CTL_ALL=0x3F,
	};

	// use a combination of CTL_.. in control_output to specify which 
    CopyPose(unsigned int control_output=CTL_ALL, unsigned int dynamic_output=CTL_NONE, double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100);
    virtual ~CopyPose();

    virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
    virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);

private:
    struct ConstraintSingleValue m_posData[3];	// index = controlled output in X,Y,Z order
    struct ConstraintSingleValue m_rotData[3];
    struct ConstraintValues m_values[2];		// index = group of controlled output, in position, rotation order
    Cache* m_cache;
    int m_poseCCh;
    CacheTS m_poseCTs;
	unsigned int m_poseCacheSize;
	unsigned int m_outputDynamic;	// combination of CTL_... determine which variables are dynamically controlled by the application
	unsigned int m_outputControl;	// combination of CTL_... determine which output are constrained
	unsigned int m_nvalues;		// number of elements used in m_values[]
	double m_maxerror;

	struct ControlState {
		int firsty;			// first y index
		int ny;				// number of y in output
		double alpha;
		double K;
		double tolerance;
		struct ControlValue {
			double yddot;
			double yd;
			double nextyd;
			double nextyddot;
		} output[3];		// inded numbex = same as m_rotData
	} m_rot, m_pos;

    void pushPose(CacheTS timestamp);
    bool popPose(CacheTS timestamp);
	int nBitsOn(unsigned int v)
		{ int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
	double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
	double* pushValues(double* item, ControlState* _state, unsigned int mask);
	void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep);
	void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask);
	void updateOutput(Vector& vel, ControlState* _state, unsigned int mask);
	void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp);

};
}
#endif /* COPYROTATION_H_ */