defmodule PIDControl do
  @moduledoc """
  A discrete implementation of a [PID controller](https://en.wikipedia.org/wiki/PID_controller) for building
  closed-loop control into embedded systems.

  ## Usage

  The `PIDControl.new/1` function returns an initialized `PIDControl` struct with the given configuration:

  ```elixir
  iex> pid = PIDControl.new(kp: 0.5, kd: 0.2, ki: 0.03)
  %PIDControl{...}
  ```

  Calling `PIDControl.step/3` performs one discrete cycle of the PID loop for the given `set_point` and `measurement`
  values.  The new PIDControl state is returned and the output can be accessed via the `output` key in the struct.

  ```elixir
  iex> pid = PIDControl.step(pid, 0.3, 0.312)
  %PIDControl{
  output: -0.11448221
  #...
  }
  ```

  The `step` function can be called in each successive input-output cycle of the underling sensor/actuator.

  ## Example 

  ```elixir
  defmodule Controller do
  use GenServer
  # ...

  def init(_) do
    Sensor.subcribe()
    pid = PIDControl.new(kp: 0.5, kd: 0.2, ki: 0.03)
    {:ok, pid}
  end

  def handle_info({:sensor, measurment}, pid) do
    pid = PIDControl.step(pid, @set_point, measurment)
    Actuator.command(pid.output)
    {:noreply, pid}
  end
  end
  ```

  ## Telemetry

  If `telemetry` is set to `true`, telemetry for the PID state will be emitted each time the `step` function is called.
  By default, the event name will be `[pid_control]` and will contain the following measurements.

  * `set_point` - Current set point
  * `measurement` - Most recent measurement
  * `error` - Current error that is being fed to the PID
  * `p` - Proportional component of the output
  * `i` - Integral component of the output
  * `d` - Derivative component of the output
  * `t` - The time value used for the step function.  This will always be the configured value `t` unless `use_system_t` is set to `true`.
  * `output` - Most recent output (which will be the sum of `p`, `i`, and `d` values)

  This is really useful for manual tuning when combined with something like `PheonixLiveDashboard`.

  """
  defstruct p: 0.0,
            i: 0.0,
            d: 0.0,
            t: 0.0,
            e0: nil,
            measurement_prev: 0.0,
            set_point_prev: 0.0,
            output: 0.0,
            time_prev: nil,
            config: %{}

  @default_config %{
    kp: 0.0,
    ki: 0.0,
    kd: 0.0,
    tau: 1.0,
    t: 1.0,
    output_min: -1.0,
    output_max: 1.0,
    use_system_t: false,
    zero_d_on_set_point_change: false,
    telemetry: false,
    telemetry_event_name: [:pid_control]
  }

  @type config() :: %{
          kp: float(),
          ki: float(),
          kd: float(),
          tau: float(),
          t: float(),
          output_min: float(),
          output_max: float(),
          use_system_t: boolean(),
          zero_d_on_set_point_change: boolean(),
          telemetry: boolean(),
          telemetry_event_name: list(atom())
        }
  @type t() :: %__MODULE__{
          p: float(),
          i: float(),
          d: float(),
          t: float(),
          e0: float() | nil,
          measurement_prev: float(),
          set_point_prev: float(),
          output: float(),
          time_prev: integer() | nil,
          config: config()
        }

  @doc """
  Creates a new PID controller.

  Config Options:

  * **`kp`, `ki`, `kp`** - Parameters for each term in the PID.  Any left blank will be set to `0`
  * **`tau`** - Low-pass filter parameter for the calculated `d` term.  A value of `1` will bypass the filtering.
    A value between `0` and `1` will filter out high-frequency noise in the derivative term.
  * **`t`** - Time factor for calculating the `i` and `d` term of the PID.  A value of `1` will ignore
    any time parameters and treat each `step` as a single time unit.  A value of `0.1`, for example,
    will treat each `step` as a tenth of a time unit.  If `use_system_t` is set to `true`, this value
    is ignored in favor of the system time (in seconds) between calls to `step`.
  * **`output_min` and `output_max`** - Minimum and maximum values that will be output from the PID. Any other
    values outside of this range will be clamped. These same values are used for anti-windup of the `i` term.
    Default is `-1` and `1`, respectively.
  * **`use_system_t`** - When set to `true`, the time used to calculate `d` and `i` terms of the PID will be
    derived from the measured time since the last call to `step` (using `System.monotonic_time/0`). When set
    to false, the configured value of `t` will be used. Defaults to `false`.
  * **`zero_d_on_set_point_change`** - When the set point changes rapidly, the d-term will suddenly spike, causing
    a sudden change in the PID output. Setting `tau` to a lower value will lessen this effect, but setting 
    `zero_d_on_set_point_change` will force the d-term to zero for the step when the set_point changes.  This is 
    useful for situations when the set point changes suddenly and occasionally, as opposed to situations where 
    the set_point is gradually changes (as when following a profile curve).  Defaults to `false`.
  * **`telemetry`** - When set to `true`, the PID will automatically emit its own telemetry after each `step`.
    Defaults to `false`.
  * **`telemetry_prefix`** - If `telemetry` is set to `true`, this value defines the name that the event is published
    under.  Defaults to `[:pid_control]`
  """
  @spec new(keyword()) :: t()
  def new(config \\ []) do
    %__MODULE__{config: @default_config} |> apply_config(config)
  end

  defp apply_config(%__MODULE__{} = pid, config) do
    pid |> Map.put(:config, Map.merge(pid.config, Enum.into(config, %{})))
  end

  @doc """
  Performs one step in the control loop.  Given the set point and measurement, the PID state
  is advanced and the output of the PID is set to the `output` field of the `PIDControl` struct.
  """
  @spec step(t(), float(), float()) :: t()
  def step(pid, set_point, measurement) do
    time = System.monotonic_time()

    t =
      if pid.config.use_system_t do
        t_actual(pid, time)
      else
        pid.config.t
      end

    e0 = set_point - measurement

    # P term is simply linear with the error
    p = pid.config.kp * e0

    # D term is linear on the change in error, passed through a low-pass filter
    last_error =
      cond do
        pid.e0 == nil -> e0
        set_point != pid.set_point_prev and pid.config.zero_d_on_set_point_change -> e0
        true -> pid.e0
      end

    d_raw = pid.config.kd * (e0 - last_error) / max(t, 0.001)
    d = pid.d + pid.config.tau * (d_raw - pid.d)

    # I term is linear to the sum of all previous errors, with anti-windup
    i_raw = pid.i + pid.config.ki * e0 * t
    i = clamp(i_raw, pid.config.output_min, pid.config.output_max)

    output = clamp(p + d + i, pid.config.output_min, pid.config.output_max)

    %{
      pid
      | p: p,
        i: i,
        d: d,
        t: t,
        e0: e0,
        measurement_prev: measurement,
        set_point_prev: set_point,
        output: output,
        time_prev: time
    }
    |> emit_telemetry()
  end

  defp t_actual(pid, time) do
    System.convert_time_unit(
      time - (Map.get(pid, :time_prev) || time),
      :native,
      :microsecond
    ) / 1_000_000.0
  end

  defp emit_telemetry(%__MODULE__{config: %{telemetry: true}} = pid) do
    :telemetry.execute(
      pid.config.telemetry_event_name,
      %{
        set_point: pid.set_point_prev,
        measurement: pid.measurement_prev,
        error: pid.e0,
        output: pid.output,
        p: pid.p,
        i: pid.i,
        d: pid.d,
        t: pid.t
      }
    )

    pid
  end

  defp emit_telemetry(pid), do: pid

  defp clamp(n, a, b) when a > b, do: clamp(n, b, a)

  defp clamp(n, a, b) when is_float(n) or is_float(a) or is_float(b),
    do: (n / 1) |> max(a / 1) |> min(b / 1)

  defp clamp(n, a, b), do: n |> max(a) |> min(b)
end