/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Locus Robotics
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the copyright holder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */
#include <gtest/gtest.h>
#include <dwb_plugins/standard_traj_generator.h>
#include <dwb_plugins/limited_accel_generator.h>
#include <nav_core2/exceptions.h>
#include <vector>
#include <algorithm>

using dwb_plugins::StandardTrajectoryGenerator;

geometry_msgs::Pose2D origin;
nav_2d_msgs::Twist2D zero;
nav_2d_msgs::Twist2D forward;

void checkLimits(const std::vector<nav_2d_msgs::Twist2D>& twists,
                 double exp_min_x, double exp_max_x, double exp_min_y, double exp_max_y,
                 double exp_min_theta, double exp_max_theta,
                 double exp_max_xy = -1.0,
                 double exp_min_xy = -1.0, double exp_min_speed_theta = -1.0)
{
  ASSERT_GT(twists.size(), 0U);
  nav_2d_msgs::Twist2D first = twists[0];

  double min_x = first.x, max_x = first.x, min_y = first.y, max_y = first.y;
  double min_theta = first.theta, max_theta = first.theta;
  double max_xy = hypot(first.x, first.y);

  for (nav_2d_msgs::Twist2D twist : twists)
  {
    min_x = std::min(min_x, twist.x);
    min_y = std::min(min_y, twist.y);
    min_theta = std::min(min_theta, twist.theta);
    max_x = std::max(max_x, twist.x);
    max_y = std::max(max_y, twist.y);
    max_theta = std::max(max_theta, twist.theta);
    double hyp = hypot(twist.x, twist.y);
    max_xy = std::max(max_xy, hyp);

    if (exp_min_xy >= 0 && exp_min_speed_theta >= 0)
    {
      EXPECT_TRUE(fabs(twist.theta) >= exp_min_speed_theta || hyp >= exp_min_xy);
    }
  }
  EXPECT_DOUBLE_EQ(min_x, exp_min_x);
  EXPECT_DOUBLE_EQ(max_x, exp_max_x);
  EXPECT_DOUBLE_EQ(min_y, exp_min_y);
  EXPECT_DOUBLE_EQ(max_y, exp_max_y);
  EXPECT_DOUBLE_EQ(min_theta, exp_min_theta);
  EXPECT_DOUBLE_EQ(max_theta, exp_max_theta);
  if (exp_max_xy >= 0)
  {
    EXPECT_DOUBLE_EQ(max_xy, exp_max_xy);
  }
}

TEST(VelocityIterator, standard_gen)
{
  ros::NodeHandle nh("st_gen");
  StandardTrajectoryGenerator gen;
  gen.initialize(nh);
  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
  EXPECT_EQ(twists.size(), 1926U);
  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, 0.55, 0.1, 0.4);
}

TEST(VelocityIterator, max_xy)
{
  ros::NodeHandle nh("max_xy");
  nh.setParam("max_speed_xy", 1.0);
  StandardTrajectoryGenerator gen;
  gen.initialize(nh);

  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
  // Expect more twists since max_speed_xy is now beyond feasible limits
  EXPECT_EQ(twists.size(), 2010U);
  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1));
}

TEST(VelocityIterator, min_xy)
{
  ros::NodeHandle nh("min_xy");
  nh.setParam("min_speed_xy", -1);
  StandardTrajectoryGenerator gen;
  gen.initialize(nh);
  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
  // Expect even more since theres no min_speed_xy
  EXPECT_EQ(twists.size(), 2015U);
  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
}

TEST(VelocityIterator, min_theta)
{
  ros::NodeHandle nh("min_theta");
  nh.setParam("min_speed_theta", -1);
  StandardTrajectoryGenerator gen;
  gen.initialize(nh);
  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
  // Expect even more since theres no min_speed_xy
  EXPECT_EQ(twists.size(), 2015U);
  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0);
}

TEST(VelocityIterator, no_limits)
{
  ros::NodeHandle nh("no_limits");
  nh.setParam("max_speed_xy", -1.0);
  nh.setParam("min_speed_xy", -1);
  nh.setParam("min_speed_theta", -1);
  StandardTrajectoryGenerator gen;
  gen.initialize(nh);
  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
  // vx_samples * vtheta_samples * vy_samples + added zero theta samples - (0,0,0)
  EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
}

TEST(VelocityIterator, no_limits_samples)
{
  ros::NodeHandle nh("no_limits_samples");
  nh.setParam("max_speed_xy", -1.0);
  nh.setParam("min_speed_xy", -1);
  nh.setParam("min_speed_theta", -1);
  int x_samples = 10, y_samples = 3, theta_samples = 5;
  nh.setParam("vx_samples", x_samples);
  nh.setParam("vy_samples", y_samples);
  nh.setParam("vtheta_samples", theta_samples);
  StandardTrajectoryGenerator gen;
  gen.initialize(nh);
  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
  EXPECT_EQ(twists.size(), static_cast<unsigned int>(x_samples * y_samples * theta_samples - 1));
  checkLimits(twists, 0.0, 0.55, -0.1, 0.1, -1.0, 1.0, hypot(0.55, 0.1), 0.0, 0.0);
}

TEST(VelocityIterator, dwa_gen_exception)
{
  ros::NodeHandle nh("dwa_gen_exception");
  nh.setParam("use_dwa", true);
  StandardTrajectoryGenerator gen;
  EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
}

TEST(VelocityIterator, no_dwa_gen_exception)
{
  ros::NodeHandle nh("no_dwa_gen_exception");
  nh.setParam("use_dwa", false);
  dwb_plugins::LimitedAccelGenerator gen;
  EXPECT_THROW(gen.initialize(nh), nav_core2::PlannerException);
}

TEST(VelocityIterator, dwa_gen)
{
  ros::NodeHandle nh("dwa_gen");
  nh.setParam("use_dwa", true);
  nh.setParam("min_speed_theta", -1);
  dwb_plugins::LimitedAccelGenerator gen;
  gen.initialize(nh);
  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
  // Same as no-limits since everything is within our velocity limits
  EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
  checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
}

TEST(VelocityIterator, dwa_gen_no_param)
{
  ros::NodeHandle nh("dwa_gen_no_param");
  nh.setParam("min_speed_theta", -1);
  dwb_plugins::LimitedAccelGenerator gen;
  gen.initialize(nh);
  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(zero);
  EXPECT_EQ(twists.size(), static_cast<unsigned int>(20 * 20 * 5 + 100 - 1));
  checkLimits(twists, 0.0, 0.125, -0.1, 0.1, -0.16, 0.16, hypot(0.125, 0.1), 0.0, 0.1);
}

TEST(VelocityIterator, nonzero)
{
  ros::NodeHandle nh("nonzero");
  nh.setParam("use_dwa", true);
  nh.setParam("min_speed_theta", -1);
  dwb_plugins::LimitedAccelGenerator gen;
  gen.initialize(nh);
  nav_2d_msgs::Twist2D initial;
  initial.x = 0.1;
  initial.y = -0.08;
  initial.theta = 0.05;
  std::vector<nav_2d_msgs::Twist2D> twists = gen.getTwists(initial);
  EXPECT_EQ(twists.size(), 2519U);
  checkLimits(twists, 0.0, 0.225, -0.1, 0.045, -0.11000000000000003, 0.21,
                      0.24622144504490268, 0.0, 0.1);
}

void matchPose(const geometry_msgs::Pose2D& a, const geometry_msgs::Pose2D& b)
{
  EXPECT_DOUBLE_EQ(a.x, b.x);
  EXPECT_DOUBLE_EQ(a.y, b.y);
  EXPECT_DOUBLE_EQ(a.theta, b.theta);
}

void matchPose(const geometry_msgs::Pose2D& a, const double x, const double y, const double theta)
{
  EXPECT_DOUBLE_EQ(a.x, x);
  EXPECT_DOUBLE_EQ(a.y, y);
  EXPECT_DOUBLE_EQ(a.theta, theta);
}

void matchTwist(const nav_2d_msgs::Twist2D& a, const nav_2d_msgs::Twist2D& b)
{
  EXPECT_DOUBLE_EQ(a.x, b.x);
  EXPECT_DOUBLE_EQ(a.y, b.y);
  EXPECT_DOUBLE_EQ(a.theta, b.theta);
}

void matchTwist(const nav_2d_msgs::Twist2D& a, const double x, const double y, const double theta)
{
  EXPECT_DOUBLE_EQ(a.x, x);
  EXPECT_DOUBLE_EQ(a.y, y);
  EXPECT_DOUBLE_EQ(a.theta, theta);
}

const double DEFAULT_SIM_TIME = 1.7;

TEST(TrajectoryGenerator, basic)
{
  ros::NodeHandle nh("basic");
  StandardTrajectoryGenerator gen;
  nh.setParam("linear_granularity", 0.5);
  gen.initialize(nh);
  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
  matchTwist(res.velocity, forward);
  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
  int n = res.poses.size();
  EXPECT_EQ(n, 3);
  ASSERT_GT(n, 0);

  matchPose(res.poses[0], origin);
  matchPose(res.poses[n - 1], DEFAULT_SIM_TIME * forward.x, 0, 0);
}

TEST(TrajectoryGenerator, basic_no_last_point)
{
  ros::NodeHandle nh("basic_no_last_point");
  StandardTrajectoryGenerator gen;
  nh.setParam("include_last_point", false);
  nh.setParam("linear_granularity", 0.5);
  gen.initialize(nh);
  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
  matchTwist(res.velocity, forward);
  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME / 2);
  int n = res.poses.size();
  EXPECT_EQ(n, 2);
  ASSERT_GT(n, 0);

  matchPose(res.poses[0], origin);
  matchPose(res.poses[n - 1], 0.255, 0, 0);
}

TEST(TrajectoryGenerator, too_slow)
{
  ros::NodeHandle nh("too_slow");
  StandardTrajectoryGenerator gen;
  nh.setParam("linear_granularity", 0.5);
  gen.initialize(nh);
  nav_2d_msgs::Twist2D cmd;
  cmd.x = 0.2;
  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
  matchTwist(res.velocity, cmd);
  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
  int n = res.poses.size();
  EXPECT_EQ(n, 2);
  ASSERT_GT(n, 0);

  matchPose(res.poses[0], origin);
}

TEST(TrajectoryGenerator, holonomic)
{
  ros::NodeHandle nh("holonomic");
  StandardTrajectoryGenerator gen;
  nh.setParam("linear_granularity", 0.5);
  gen.initialize(nh);
  nav_2d_msgs::Twist2D cmd;
  cmd.x = 0.3;
  cmd.y = 0.2;
  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
  matchTwist(res.velocity, cmd);
  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME);
  int n = res.poses.size();
  EXPECT_EQ(n, 3);
  ASSERT_GT(n, 0);

  matchPose(res.poses[0], origin);
  matchPose(res.poses[n - 1], cmd.x*DEFAULT_SIM_TIME, cmd.y*DEFAULT_SIM_TIME, 0);
}

TEST(TrajectoryGenerator, twisty)
{
  ros::NodeHandle nh("twisty");
  StandardTrajectoryGenerator gen;
  nh.setParam("linear_granularity", 0.5);
  nh.setParam("angular_granularity", 0.025);
  gen.initialize(nh);
  nav_2d_msgs::Twist2D cmd;
  cmd.x = 0.3;
  cmd.y = -0.2;
  cmd.theta = 0.111;
  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, cmd, cmd);
  matchTwist(res.velocity, cmd);
  EXPECT_NEAR(res.time_offsets.back().toSec(), DEFAULT_SIM_TIME, 1.0E-5);
              // Given the number of poses, the resulting time is slightly less precise
  int n = res.poses.size();
  EXPECT_EQ(n, 9);
  ASSERT_GT(n, 0);

  matchPose(res.poses[0], origin);
  matchPose(res.poses[n - 1], 0.5355173615993063, -0.29635287789821596, cmd.theta * DEFAULT_SIM_TIME);
}

TEST(TrajectoryGenerator, sim_time)
{
  ros::NodeHandle nh("sim_time");
  const double sim_time = 2.5;
  nh.setParam("sim_time", sim_time);
  nh.setParam("linear_granularity", 0.5);
  StandardTrajectoryGenerator gen;
  gen.initialize(nh);
  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, forward, forward);
  matchTwist(res.velocity, forward);
  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), sim_time);
  int n = res.poses.size();
  EXPECT_EQ(n, 3);
  ASSERT_GT(n, 0);

  matchPose(res.poses[0], origin);
  matchPose(res.poses[n - 1], sim_time * forward.x, 0, 0);
}

TEST(TrajectoryGenerator, accel)
{
  ros::NodeHandle nh("accel");
  nh.setParam("sim_time", 5.0);
  nh.setParam("discretize_by_time", true);
  nh.setParam("sim_granularity", 1.0);
  nh.setParam("acc_lim_x", 0.1);
  nh.setParam("min_speed_xy", -1.0);
  StandardTrajectoryGenerator gen;
  gen.initialize(nh);

  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
  matchTwist(res.velocity, forward);
  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), 5.0);
  ASSERT_EQ(res.poses.size(), 6U);
  matchPose(res.poses[0], origin);
  matchPose(res.poses[1], 0.1, 0, 0);
  matchPose(res.poses[2], 0.3, 0, 0);
  matchPose(res.poses[3], 0.6, 0, 0);
  matchPose(res.poses[4], 0.9, 0, 0);
  matchPose(res.poses[5], 1.2, 0, 0);
}

TEST(TrajectoryGenerator, dwa)
{
  ros::NodeHandle nh("dwa");
  nh.setParam("use_dwa", true);
  nh.setParam("sim_period", 1.0);
  nh.setParam("sim_time", 5.0);
  nh.setParam("discretize_by_time", true);
  nh.setParam("sim_granularity", 1.0);
  nh.setParam("acc_lim_x", 0.1);
  nh.setParam("min_speed_xy", -1.0);
  dwb_plugins::LimitedAccelGenerator gen;
  gen.initialize(nh);

  dwb_msgs::Trajectory2D res = gen.generateTrajectory(origin, zero, forward);
  matchTwist(res.velocity, forward);
  EXPECT_DOUBLE_EQ(res.time_offsets.back().toSec(), 5.0);
  ASSERT_EQ(res.poses.size(), 6U);
  matchPose(res.poses[0], origin);
  matchPose(res.poses[1], 0.3, 0, 0);
  matchPose(res.poses[2], 0.6, 0, 0);
  matchPose(res.poses[3], 0.9, 0, 0);
  matchPose(res.poses[4], 1.2, 0, 0);
  matchPose(res.poses[5], 1.5, 0, 0);
}

int main(int argc, char **argv)
{
  forward.x = 0.3;
  ros::init(argc, argv, "twist_gen");
  testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}