// Servo command generator
// Author: Max Schwarz <max.schwarz@uni-bonn.de>

#ifndef SERVOCOMMANDGENERATOR_H
#define SERVOCOMMANDGENERATOR_H

#include <Eigen/Core>

class ServoCommandGenerator
{
public:
	ServoCommandGenerator();

	//! Get current tick <-> voltage factor
	double currentFactor() const;

	//! Get separate command parts as a vector
	Eigen::VectorXd commandPartsFor(double pos, double vel, double acc, double outsideTorque) const;

	/**
	 * @brief Get servo command for desired action
	 *
	 * @param pos Servo position in radians
	 * @param vel Goal velocity in rad/s
	 * @param acc Goal acceleration in rad/s^2
	 * @param outsideTorque Torque generated by outside forces (e.g. gravity, inertia)
	 **/
	double servoCommandFor(double pos, double vel, double acc, double outsideTorque) const;

	/**
	 * @brief Estimate servo torque from position displacement
	 *
	 * @param pos_cmd Servo position command
	 * @param pos_cur Servo position (measured)
	 * @param vel Servo velocity (for friction torque estimation)
	 **/
	double servoTorqueFromCommand(double pos_cmd, double pos_cur, double vel) const;

	//! Set coefficients as vector
	void setCoefficients(const Eigen::VectorXd& coeff);

	std::string serializeCoefficients();

	//! @return true on success
	bool deserializeCoefficients(const std::string& coeff);

	void setKM(double val);
	void setViscousFriction(double val);
	void setStribeckOne(double val);
	void setStribeckTwo(double val);
	void setTicksPerRev(double val);
	void setMinTickValue(int val);
	void setMaxTickValue(int val);
	void setPValue(int pValue);
	void setLatency(double val);
	void setVoltage(double volt);

	inline double km() const
	{ return m_coeff(0); }

	inline double viscousFriction() const
	{ return m_coeff(1); }

	inline double stribeckOne() const
	{ return m_coeff(2); }

	inline double stribeckTwo() const
	{ return m_coeff(3); }

	inline double ticksPerRev() const
	{ return m_ticksPerRev; }
	inline double ticksPerRad() const
	{ return m_ticksPerRev * 0.5 * M_1_PI; }

	inline int minTickValue() const
	{ return m_minTickValue; }
	inline int maxTickValue() const
	{ return m_maxTickValue; }

	inline double pValue() const
	{ return m_pValue; }

	inline double latency() const
	{ return m_latency; }

	inline double voltage() const
	{ return m_voltage; }

	static const int DefaultTicksPerRev;

protected:
	virtual void update();

private:
	Eigen::VectorXd m_coeff;
	double m_ticksPerRev;
	int m_minTickValue;
	int m_maxTickValue;
	double m_pValue;
	double m_latency;
	double m_voltage;
};

#endif
