local H=require(script.Parent.VectorUtil)local T={}T.__index=T function T.new(H,R,a,U,h)local t=setmetatable({},T)t._min=H or-math.huge t._max=R or math.huge t._kp=a t._kd=U t._ki=h t._preError=0 t._integral=0 t.VectorMode=false return t end function T.newVectorMode(H,R,a,U)local h=setmetatable({},T)h._max=H or math.huge h._kp=R h._kd=a h._ki=U h._preError=Vector3.new()h._integral=Vector3.new()h.VectorMode=true return h end function T.Reset(H)if not H.VectorMode then H._preError=0 H._integral=0 else H._preError=Vector3.new()H._integral=Vector3.new()end end function T.Calculate(a,H,T,R)local U=T-R local h=a._kp*U a._integral+=U*H local t=a._ki*a._integral local G=(U-a._preError)/H local n=a._kd*G local F=math.clamp((h+t)+n,a._min,a._max)a._preError=U return F end function T.CalculateVector(U,T,R,a)local h=R-a local t=U._kp*h U._integral+=h*T local G=U._ki*U._integral local n=(h-U._preError)/T local F=U._kd*n local e=(t+G)+F local N=H.ClampMagnitude(e,U._max)U._preError=h return N end function T.SetMinMax(R,H,T)R._min=H R._max=T end return T
-- PID
--From AeroGameFramework by sleitnick here for testing purposes
-- August 11, 2020

--[[

	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 VectorUtil = require(script.Parent.VectorUtil)

local PID = {}
PID.__index = PID

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

function PID.newVectorMode(maxMagnitude, kp, kd, ki)
	local self = setmetatable({}, PID)
	self._max = maxMagnitude or math.huge -- doesn't need min only clamps magnitude
	self._kp = kp
	self._kd = kd
	self._ki = ki
	self._preError = Vector3.new()
	self._integral = Vector3.new()
	self.VectorMode = true -- uses Vector3 values for integral so it doesn't error
	return self
end


function PID:Reset()
	if not self.VectorMode then
		self._preError = 0
		self._integral = 0
	else
		self._preError = Vector3.new()
		self._integral = Vector3.new()
	end
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:CalculateVector(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 outputPreClamp = pOut + iOut + dOut
	local output = VectorUtil.ClampMagnitude(outputPreClamp,self._max)
	self._preError = err
	return output
end



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


return PID