/*
 * Copyright (C) 2019 Open Source Robotics Foundation
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
*/

#include <gtest/gtest.h>
#include <thread>

#include "gz/math/Angle.hh"
#include "gz/math/Helpers.hh"
#include "gz/math/MecanumDriveOdometry.hh"

using namespace gz;

/////////////////////////////////////////////////
TEST(MecanumDriveOdometryTest, MecanumDriveOdometry)
{
  math::MecanumDriveOdometry odom;
  EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
  EXPECT_DOUBLE_EQ(0.0, odom.X());
  EXPECT_DOUBLE_EQ(0.0, odom.Y());
  EXPECT_DOUBLE_EQ(0.0, odom.LinearVelocity());
  EXPECT_DOUBLE_EQ(0.0, odom.LateralVelocity());
  EXPECT_DOUBLE_EQ(0.0, *odom.AngularVelocity());

  double wheelSeparation = 2.0;
  double wheelRadius = 0.5;
  double wheelCircumference = 2 * GZ_PI * wheelRadius;

  // This is the linear distance traveled per degree of wheel rotation.
  double distPerDegree = wheelCircumference / 360.0;

  // Setup the wheel parameters, and initialize
  odom.SetWheelParams(wheelSeparation, wheelRadius, wheelRadius, wheelRadius);
  auto startTime = std::chrono::steady_clock::now();
  odom.Init(startTime);

  // Sleep for a little while, then update the odometry with the new wheel
  // position.
  auto time1 = startTime + std::chrono::milliseconds(100);
  odom.Update(GZ_DTOR(1.0), GZ_DTOR(1.0), GZ_DTOR(1.0), GZ_DTOR(1.0), time1);
  EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
  EXPECT_DOUBLE_EQ(distPerDegree, odom.X());
  EXPECT_DOUBLE_EQ(0.0, odom.Y());
  // Linear velocity should be dist_traveled / time_elapsed.
  EXPECT_NEAR(distPerDegree / 0.1, odom.LinearVelocity(), 1e-3);
  // Angular velocity should be zero since the "robot" is traveling in a
  // straight line.
  EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3);

  // Sleep again, then update the odometry with the new wheel position.
  auto time2 = time1 + std::chrono::milliseconds(100);
  odom.Update(GZ_DTOR(2.0), GZ_DTOR(2.0), GZ_DTOR(2.0), GZ_DTOR(2.0), time2);
  EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
  EXPECT_NEAR(distPerDegree * 2.0, odom.X(), 3e-6);
  EXPECT_DOUBLE_EQ(0.0, odom.Y());
  // Linear velocity should be dist_traveled / time_elapsed.
  EXPECT_NEAR(distPerDegree / 0.1, odom.LinearVelocity(), 1e-3);
  // Angular velocity should be zero since the "robot" is traveling in a
  // straight line.
  EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3);

  // Initialize again, and odom values should be reset.
  startTime = std::chrono::steady_clock::now();
  odom.Init(startTime);
  EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
  EXPECT_DOUBLE_EQ(0.0, odom.X());
  EXPECT_DOUBLE_EQ(0.0, odom.Y());
  EXPECT_DOUBLE_EQ(0.0, odom.LinearVelocity());
  EXPECT_DOUBLE_EQ(0.0, *odom.AngularVelocity());

  // Sleep again, this time move 2 degrees in 100ms.
  time1 = startTime + std::chrono::milliseconds(100);
  odom.Update(GZ_DTOR(2.0), GZ_DTOR(2.0), GZ_DTOR(2.0), GZ_DTOR(2.0), time1);
  EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
  EXPECT_NEAR(distPerDegree * 2.0, odom.X(), 3e-6);
  EXPECT_DOUBLE_EQ(0.0, odom.Y());
  // Linear velocity should be dist_traveled / time_elapsed.
  EXPECT_NEAR(distPerDegree * 2 / 0.1, odom.LinearVelocity(), 1e-3);
  // Angular velocity should be zero since the "robot" is traveling in a
  // straight line.
  EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3);

  // Sleep again, this time move 2 degrees in 100ms.
  odom.Init(startTime);
  time1 = startTime + std::chrono::milliseconds(100);
  odom.Update(GZ_DTOR(-2.0), GZ_DTOR(2.0), GZ_DTOR(2.0), GZ_DTOR(-2.0), time1);
  EXPECT_DOUBLE_EQ(0.0, *odom.Heading());
  EXPECT_NEAR(distPerDegree * 2.0, odom.Y(), 3e-6);
  // EXPECT_DOUBLE_EQ(0.0, odom.Y());
  // Linear velocity should be dist_traveled / time_elapsed.
  EXPECT_NEAR(distPerDegree * 2 / 0.1, odom.LateralVelocity(), 1e-3);
  // Angular velocity should be zero since the "robot" is traveling in a
  // straight line.
  EXPECT_NEAR(0.0, *odom.AngularVelocity(), 1e-3);
}
