/*----------------------------------------------------------------------------*/
/* Copyright (c) 2019 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 <wpi/math>

#include "frc/controller/RamseteController.h"
#include "frc/trajectory/TrajectoryGenerator.h"
#include "gtest/gtest.h"

#define EXPECT_NEAR_UNITS(val1, val2, eps) \
  EXPECT_LE(units::math::abs(val1 - val2), eps)

static constexpr units::meter_t kTolerance{1 / 12.0};
static constexpr units::radian_t kAngularTolerance{2.0 * wpi::math::pi / 180.0};

units::radian_t boundRadians(units::radian_t value) {
  while (value > units::radian_t{wpi::math::pi}) {
    value -= units::radian_t{wpi::math::pi * 2};
  }
  while (value <= units::radian_t{-wpi::math::pi}) {
    value += units::radian_t{wpi::math::pi * 2};
  }
  return value;
}

TEST(RamseteControllerTest, ReachesReference) {
  frc::RamseteController controller{2.0, 0.7};
  frc::Pose2d robotPose{2.7_m, 23_m, frc::Rotation2d{0_deg}};

  auto waypoints = std::vector{frc::Pose2d{2.75_m, 22.521_m, 0_rad},
                               frc::Pose2d{24.73_m, 19.68_m, 5.846_rad}};
  auto trajectory = frc::TrajectoryGenerator::GenerateTrajectory(
      waypoints, {}, 0_mps, 0_mps, 8.8_mps, 0.1_mps_sq, false);

  constexpr auto kDt = 0.02_s;
  auto totalTime = trajectory.TotalTime();
  for (size_t i = 0; i < (totalTime / kDt).to<double>(); ++i) {
    auto state = trajectory.Sample(kDt * i);
    auto [vx, vy, omega] = controller.Calculate(robotPose, state);
    static_cast<void>(vy);

    robotPose = robotPose.Exp(frc::Twist2d{vx * kDt, 0_m, omega * kDt});
  }

  auto& endPose = trajectory.States().back().pose;
  EXPECT_NEAR_UNITS(endPose.Translation().X(), robotPose.Translation().X(),
                    kTolerance);
  EXPECT_NEAR_UNITS(endPose.Translation().Y(), robotPose.Translation().Y(),
                    kTolerance);
  EXPECT_NEAR_UNITS(boundRadians(endPose.Rotation().Radians() -
                                 robotPose.Rotation().Radians()),
                    0_rad, kAngularTolerance);
}