--This work is a derivative of
--https://github.com/OpenPrograms/Kubuxu-Programs/tree/master/pid
local pid = {}
pid.__index = pid

local function clamp(x, min, max)
  if x > max then
    return max
  elseif x < min then
    return min
  else
    return x
  end
end

-- Creates a new PID controller.
function pid:new(kp, ki, kd)
  local this = {}
  
  setmetatable(this, pid)
  
  this.kp = kp
  this.ki = ki
  this.kd = kd
  
  this.target = nil
  this.minout = -math.huge
  this.maxout = math.huge
  
  this._Iterm = 0
  this._lastinput = 0
  return this
end

function pid:get(value, dtime)
  assert(value and self.target, "You have to sepecify current input and target before running compute()")
  
  -- reset values if PID was paused for prolonegd period of time
  local err = self.target - value
  
  if dtime == 0 then
    return
  end

  self._Iterm = self._Iterm + self.ki * err * dtime
  self._Iterm = clamp(self._Iterm, self.minout, self.maxout)
  
  local dinput = (value - self._lastinput) / dtime
  
  local output = self.kp * err + self._Iterm - self.kd * dinput
  output = clamp(output, self.minout, self.maxout)
  
  self._lastinput = value
  
  return output
end

return pid