#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
