text: /*----------------------------------------------------------------------------*/
  /* Copyright (c) 2020 FIRST. All Rights Reserved.                             */
  /* Open Source Software - may be modified and shared by FRC teams. The code   */
  /* must be accompanied by the FIRST BSD license file in the root directory of */
  /* the project.                                                               */
  /*----------------------------------------------------------------------------*/
  
  #include <units/units.h>
  
  #include "frc/controller/ControllerUtil.h"
  #include "gtest/gtest.h"
  
  TEST(ControllerUtilTest, GetModulusError) {
    // Test symmetric range
    EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -180.0, 180.0));
    EXPECT_FLOAT_EQ(-20.0,
                    frc::GetModulusError(170.0 + 360.0, -170.0, -180.0, 180.0));
    EXPECT_FLOAT_EQ(-20.0,
                    frc::GetModulusError(170.0, -170.0 + 360.0, -180.0, 180.0));
  
    // Test range starting at zero
    EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, 190.0, 0.0, 360.0));
    EXPECT_FLOAT_EQ(-20.0,
                    frc::GetModulusError(170.0 + 360.0, 190.0, 0.0, 360.0));
    EXPECT_FLOAT_EQ(-20.0,
                    frc::GetModulusError(170.0, 190.0 + 360.0, 0.0, 360.0));
  
    // Test asymmetric range that doesn't start at zero
    EXPECT_FLOAT_EQ(-20.0, frc::GetModulusError(170.0, -170.0, -170.0, 190.0));
  
    // Test all supported types
    EXPECT_FLOAT_EQ(-20.0,
                    frc::GetModulusError<double>(170.0, -170.0, -170.0, 190.0));
    EXPECT_EQ(-20, frc::GetModulusError<int>(170, -170, -170, 190));
    EXPECT_EQ(-20_deg, frc::GetModulusError<units::degree_t>(170_deg, -170_deg,
                                                             -170_deg, 190_deg));
  }
  