text: // Copyright (c) FIRST and other WPILib contributors.
  // Open Source Software; you can modify and/or share it under the terms of
  // the WPILib BSD license file in the root directory of this project.
  
  #pragma once
  
  #include <cmath>
  #include <type_traits>
  
  #include <units/math.h>
  
  namespace frc {
  
  /**
   * Returns modulus of error where error is the difference between the reference
   * and a measurement.
   *
   * @param reference Reference input of a controller.
   * @param measurement The current measurement.
   * @param minimumInput The minimum value expected from the input.
   * @param maximumInput The maximum value expected from the input.
   */
  template <typename T>
  T GetModulusError(T reference, T measurement, T minimumInput, T maximumInput) {
    T error = reference - measurement;
    T modulus = maximumInput - minimumInput;
  
    // Wrap error above maximum input
    int numMax = (error + maximumInput) / modulus;
    error -= numMax * modulus;
  
    // Wrap error below minimum input
    int numMin = (error + minimumInput) / modulus;
    error -= numMin * modulus;
  
    return error;
  }
  
  }  // namespace frc
  