local R={}R.__index=R function R.new(l,a,M,U,E)local O=setmetatable({},R)O._min=l O._max=a O._kp=M O._kd=U O._ki=E O._preError=0 O._integral=0 return O end function R.Reset(R)R._preError=0 R._integral=0 end function R.Calculate(M,R,l,a)local U=l-a local E=M._kp*U M._integral+=U*R local O=M._ki*M._integral local x=(U-M._preError)/R local p=M._kd*x local C=math.clamp((E+O)+p,M._min,M._max)M._preError=U return C end function R.SetMinMax(a,R,l)a._min=R a._max=l end return R
--[[

	PID stands for Proportional-Integral-Derivative. One example of PID controllers in
	real-life is to control the input to each motor of a drone to keep it stabilized.
	Another example is cruise-control on a car.

	-----------------------------------------------

	Constructor:

		pid = PID.new(min, max, kP, kD, kI)


	Methods:

		pid:Calculate(dt, setpoint, pv)
		> Calculates and returns the new value
			> dt: DeltaTime
			> setpoint: The current point
			> pv: The process variable (i.e. goal)

		pid:Reset()
		> Resets the PID

	-----------------------------------------------

--]]

local PID = {}
PID.__index = PID

function PID.new(min, max, kp, kd, ki)
	local self = setmetatable({}, PID)
	self._min = min
	self._max = max
	self._kp = kp
	self._kd = kd
	self._ki = ki
	self._preError = 0
	self._integral = 0
	return self
end

function PID:Reset()
	self._preError = 0
	self._integral = 0
end

function PID:Calculate(dt, setpoint, pv)
	local err = (setpoint - pv)
	local pOut = (self._kp * err)
	self._integral += (err * dt)
	local iOut = (self._ki * self._integral)
	local deriv = ((err - self._preError) / dt)
	local dOut = (self._kd * deriv)
	local output = math.clamp((pOut + iOut + dOut), self._min, self._max)
	self._preError = err
	return output
end

function PID:SetMinMax(min, max)
	self._min = min
	self._max = max
end

return PID
