/* ScummVM - Graphic Adventure Engine
 *
 * ScummVM is the legal property of its developers, whose names
 * are too numerous to list here. Please refer to the COPYRIGHT
 * file distributed with this source distribution.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 */

/*
 * Copyright (C) 2006-2010 - Frictional Games
 *
 * This file is part of HPL1 Engine.
 */

#ifndef HPL_PID_CONTROLLER_H
#define HPL_PID_CONTROLLER_H

#include "hpl1/algorithms.h"
#include "hpl1/engine/math/MathTypes.h"

namespace hpl {

template<class T>
class cPidController {
public:
	float p, i, d;
	//////////////////////////////////////////
	// Constructors
	/////////////////////////////////////////
	cPidController() {
		Reset();
	}
	cPidController(float afP, float afI, float afD, int alErrorNum) {
		p = afP;
		i = afI;
		d = afD;
		SetErrorNum(alErrorNum);
		Reset();
	}

	//////////////////////////////////////////
	// Public
	/////////////////////////////////////////

	//------------------------------------
	T Output(T aError, float afTimeStep) {
		mvErrors[mlErrorNum] = aError;
		mvTimeSteps[mlErrorNum] = afTimeStep;

		integral = 0;
		size_t lCount = mvErrors.size();
		for (size_t error = 0; error < lCount; ++error) {
			integral += mvErrors[error] * mvTimeSteps[error];
		}

		derivative = 0.0f;
		if (mlLastNum >= 0) {
			derivative = (mvErrors[mlErrorNum] - mvErrors[mlLastNum]) / afTimeStep;
		}

		mlLastNum = mlErrorNum;
		mlErrorNum++;
		if (mlErrorNum >= (int)mvErrors.size())
			mlErrorNum = 0;

		return mvErrors[mlLastNum] * p + integral * i + derivative * d;
	}

	//------------------------------------

	void SetErrorNum(int alErrorNum) {
		mvErrors.resize(alErrorNum);
		Hpl1::resizeAndFill(mvTimeSteps, alErrorNum, 0.f);
	}

	//------------------------------------

	void Reset() {
		mlErrorNum = 0;
		mlLastNum = -1;
		Common::fill(mvTimeSteps.begin(), mvTimeSteps.end(), 0);

		integral = 0;
		derivative = 0;
	}

	//------------------------------------

	T GetLastError() {
		if (mlLastNum >= 0)
			return mvErrors[mlLastNum];
		return 0;
	}

	T GetLastDerivative() {
		return derivative;
	}

	T GetLastIntegral() {
		return integral;
	}

	//------------------------------------

private:
	Common::Array<T> mvErrors;
	Common::Array<float> mvTimeSteps;

	T integral, derivative;

	int mlErrorNum;
	int mlLastNum;
};

//---------------------------------

typedef cPidController<float> cPidControllerf;
typedef cPidController<cVector3f> cPidControllerVec3;

} // namespace hpl

#endif // HPL_PID_CONTROLLER_H