#ifndef _PIDCTRL_H_
#define _PIDCTRL_H_

/**
 * @brief PID楯饹
 * PID̤򻻽Фޤ
 *
 * I̤λФϾάƤޤ
 *
 * @author teshima
 */
class PIDCtrl
{
public:

	/**
	 * 󥹥ȥ饯
	 * @param tgtVal ɸ
	 * @param kp P
	 * @param ki I
	 * @param kd D
	 */
	PIDCtrl(float tgtVal, float kp, float ki, float kd)
	 : TGT_VAL(tgtVal), KP(kp) , KI(ki) , KD(kd) ,m_prevDiff(0) {}
 
	~PIDCtrl() {}

	/**
	 * PID̻
	 * @param curVal 
	 * @retval S16 PID
	 */
	S16 calcVal(S16 curVal) {
		float diff = TGT_VAL - curVal;		//к
		return (S16)(calcPVal(diff) + calcIVal(diff) + calcDVal(diff));
	}

private:
	const float TGT_VAL;	//ɸ
	const float KP;			//P
	const float KI;			//I
	const float KD;			//D
	float m_prevDiff;		//к

	/**
	 * P̻
	 * @param diff 
	 * @retval float P
	 */
	float calcPVal(float diff) {
		return KP * diff;
	}

	/**
	 * I̻
	 * @brief ʤ
	 */
	float calcIVal(float diff) {
		return 0;
	}
	
	/**
	 * D̻
	 * @param diff 
	 * @retval float D
	 */
	float calcDVal(float diff) {
		float dValue = KD * (diff - m_prevDiff);
		m_prevDiff = diff;
		return dValue;
	}
};
#endif
