class PidController

# File lib/pid_controller.rb, line 7
def initialize(
  setpoint:,
  kp: 1.0,
  ki: 1.0,
  kd: 1.0,
  integral_min: nil,
  integral_max: nil,
  output_min: nil,
  output_max: nil
)
  @setpoint = setpoint
  @kp = kp
  @ki = ki
  @kd = kd
  # Prevents https://en.wikipedia.org/wiki/Integral_windup via bounds
  @integral_min = integral_min || -Float::INFINITY
  @integral_max = integral_max || Float::INFINITY
  @output_min = output_min || -Float::INFINITY
  @output_max = output_max || Float::INFINITY
  @integral   = 0.0
  @derivative = 0.0
  @last_error  = nil
  @last_update = nil
end

# File lib/pid_controller.rb, line 88
def clock_time
  Process.clock_gettime(Process::CLOCK_MONOTONIC)
end

# File lib/pid_controller.rb, line 83
def d_term
  kd * @derivative
end

# File lib/pid_controller.rb, line 79
def i_term
  ki * @integral
end

# File lib/pid_controller.rb, line 71
def output
  (p_term + i_term + d_term).clamp(@output_min, @output_max)
end

# File lib/pid_controller.rb, line 75
def p_term
  kp * (@last_error || 0.0)
end

# File lib/pid_controller.rb, line 45
def update(measurement)
  now = clock_time
  dt = if @last_update
         now - @last_update
       else
         0.0
       end
  @last_update = now
  update_with_duration(measurement, dt)
end

# File lib/pid_controller.rb, line 58
def update_with_duration(measurement, dt)
  error = setpoint - measurement.to_f

  if dt > 0.0
    @integral = (@integral + error * dt).clamp(@integral_min, @integral_max)
    @derivative = (error - @last_error) / dt if @last_error
  end

  @last_error = error

  output
end