/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2019, 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.
 */

// Tests ripped from https://github.com/locusrobotics/robot_navigation/blob/master/nav_grid/test/utest.cpp

#include <gtest/gtest.h>
#include <robot_costmap_2d/costmap_2d.h>
#include <robot_costmap_2d/cost_values.h>
#include <robot_costmap_2d/inflation_layer.h>
#include <robot_costmap_2d/layered_costmap.h>
#include <robot_costmap_2d/observation_buffer.h>
#include <robot_costmap_2d/voxel_layer.h>

#include <boost/make_shared.hpp>
#include <cstdlib>

using namespace robot_costmap_2d;

TEST(CostmapCoordinates, easy_coordinates_test)
{
  Costmap2D costmap(2, 3, 1.0, 0, 0);

  double wx, wy;
  costmap.mapToWorld(0u, 0u, wx, wy);
  EXPECT_DOUBLE_EQ(wx, 0.5);
  EXPECT_DOUBLE_EQ(wy, 0.5);
  costmap.mapToWorld(1u, 2u, wx, wy);
  EXPECT_DOUBLE_EQ(wx, 1.5);
  EXPECT_DOUBLE_EQ(wy, 2.5);

  unsigned int umx, umy;
  int mx, my;
  ASSERT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
  EXPECT_EQ(umx, 1);
  EXPECT_EQ(umy, 2);
  costmap.worldToMapNoBounds(wx, wy, mx, my);
  EXPECT_EQ(mx, 1);
  EXPECT_EQ(my, 2);

  // Invalid Coordinate
  wx = 2.5;
  EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
  costmap.worldToMapEnforceBounds(wx, wy, mx, my);
  EXPECT_EQ(mx, 1);
  EXPECT_EQ(my, 2);
  costmap.worldToMapNoBounds(wx, wy, mx, my);
  EXPECT_EQ(mx, 2);
  EXPECT_EQ(my, 2);

  // Border Cases
  EXPECT_TRUE(costmap.worldToMap(0.0, wy, umx, umy));
  EXPECT_EQ(umx, 0);
  EXPECT_TRUE(costmap.worldToMap(0.25, wy, umx, umy));
  EXPECT_EQ(umx, 0);
  EXPECT_TRUE(costmap.worldToMap(0.75, wy, umx, umy));
  EXPECT_EQ(umx, 0);
  EXPECT_TRUE(costmap.worldToMap(0.9999, wy, umx, umy));
  EXPECT_EQ(umx, 0);
  EXPECT_TRUE(costmap.worldToMap(1.0, wy, umx, umy));
  EXPECT_EQ(umx, 1);
  EXPECT_TRUE(costmap.worldToMap(1.25, wy, umx, umy));
  EXPECT_EQ(umx, 1);
  EXPECT_TRUE(costmap.worldToMap(1.75, wy, umx, umy));
  EXPECT_EQ(umx, 1);
  EXPECT_TRUE(costmap.worldToMap(1.9999, wy, umx, umy));
  EXPECT_EQ(umx, 1);
  EXPECT_FALSE(costmap.worldToMap(2.0, wy, umx, umy));
  costmap.worldToMapEnforceBounds(2.0, wy, mx, my);
  EXPECT_EQ(mx, 1);
}

TEST(CostmapCoordinates, hard_coordinates_test)
{
  Costmap2D costmap(2, 3, 0.1, -0.2, 0.2);

  double wx, wy;
  costmap.mapToWorld(0, 0, wx, wy);
  EXPECT_DOUBLE_EQ(wx, -0.15);
  EXPECT_DOUBLE_EQ(wy, 0.25);
  costmap.mapToWorld(1, 2, wx, wy);
  EXPECT_DOUBLE_EQ(wx, -0.05);
  EXPECT_DOUBLE_EQ(wy, 0.45);

  unsigned int umx, umy;
  int mx, my;
  EXPECT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
  EXPECT_EQ(umx, 1);
  EXPECT_EQ(umy, 2);
  costmap.worldToMapNoBounds(wx, wy, mx, my);
  EXPECT_EQ(mx, 1);
  EXPECT_EQ(my, 2);

  // Invalid Coordinate
  wx = 2.5;
  EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
  costmap.worldToMapEnforceBounds(wx, wy, mx, my);
  EXPECT_EQ(mx, 1);
  EXPECT_EQ(my, 2);
  costmap.worldToMapNoBounds(wx, wy, mx, my);
  EXPECT_EQ(mx, 27);
  EXPECT_EQ(my, 2);
}

TEST(CostmapPerformanceRegression, rolling_origin_preserves_overlap)
{
  Costmap2D costmap(4, 3, 1.0, 0.0, 0.0, FREE_SPACE);
  costmap.setCost(1, 1, LETHAL_OBSTACLE);
  costmap.setCost(3, 2, INSCRIBED_INFLATED_OBSTACLE);

  costmap.updateOrigin(0.25, 0.25);
  EXPECT_DOUBLE_EQ(costmap.getOriginX(), 0.0);
  EXPECT_DOUBLE_EQ(costmap.getOriginY(), 0.0);
  EXPECT_EQ(costmap.getCost(1, 1), LETHAL_OBSTACLE);

  costmap.updateOrigin(1.0, 0.0);
  EXPECT_DOUBLE_EQ(costmap.getOriginX(), 1.0);
  EXPECT_EQ(costmap.getCost(0, 1), LETHAL_OBSTACLE);
  EXPECT_EQ(costmap.getCost(3, 2), FREE_SPACE);
}

TEST(CostmapPerformanceRegression, voxel_origin_subcell_shift_is_noop)
{
  VoxelLayer layer;
  layer.resizeMap(4, 3, 1.0, 0.0, 0.0);
  layer.setCost(1, 1, LETHAL_OBSTACLE);

  layer.updateOrigin(0.25, 0.25);

  EXPECT_DOUBLE_EQ(layer.getOriginX(), 0.0);
  EXPECT_DOUBLE_EQ(layer.getOriginY(), 0.0);
  EXPECT_EQ(layer.getCost(1, 1), LETHAL_OBSTACLE);
}

TEST(CostmapPerformanceRegression, observation_copy_shares_cloud_payload)
{
  robot_geometry_msgs::Point origin;
  robot_sensor_msgs::PointCloud2 cloud;
  cloud.height = 1;
  cloud.width = 1;
  cloud.point_step = 4;
  cloud.row_step = 4;
  cloud.data = {1, 2, 3, 4};

  Observation observation(origin, cloud, 2.5, 3.0);
  Observation copied = observation;

  EXPECT_EQ(copied.cloud_, observation.cloud_);
  EXPECT_EQ(copied.cloud_handle_.use_count(), 2);
  EXPECT_EQ(copied.cloud_->data, cloud.data);
}

TEST(CostmapPerformanceRegression, latest_depth_frame_is_consumed_once)
{
  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
  ObservationBuffer buffer(
      "/camera/depth/data", 0.0, 0.5, 0.0, 2.0, 2.5, 3.0,
      8, 0.2, 3.0, tf_buffer, "odom", "", 0.2);

  robot_sensor_msgs::DepthCameraData::ConstPtr depth =
      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
  buffer.bufferDepthCamera(depth);

  std::vector<DepthCameraObservation> first_snapshot;
  buffer.getDepthObservations(first_snapshot);
  ASSERT_EQ(first_snapshot.size(), 1u);
  EXPECT_EQ(first_snapshot.front().data_, depth.get());
  EXPECT_EQ(first_snapshot.front().topic_, "/camera/depth/data");

  std::vector<DepthCameraObservation> second_snapshot;
  buffer.getDepthObservations(second_snapshot);
  EXPECT_TRUE(second_snapshot.empty());
}

TEST(CostmapPerformanceRegression, inflation_buckets_preserve_radial_costs)
{
  ASSERT_EQ(setenv("PNKX_NAV_CORE_CONFIG_DIR", ROBOT_COSTMAP_2D_DIR, 1), 0);

  LayeredCostmap layered_costmap("map", false, false);
  layered_costmap.resizeMap(7, 7, 1.0, 0.0, 0.0, true);
  tf3::BufferCore tf_buffer(tf3::Duration(10.0));
  InflationLayer inflation;
  inflation.initialize(&layered_costmap, "inflation", &tf_buffer);
  inflation.setInflationParameters(2.0, 1.0);

  Costmap2D& master = *layered_costmap.getCostmap();
  master.setCost(3, 3, LETHAL_OBSTACLE);
  inflation.updateCosts(master, 0, 0, 7, 7);

  EXPECT_EQ(master.getCost(3, 3), LETHAL_OBSTACLE);
  EXPECT_EQ(master.getCost(2, 3), master.getCost(4, 3));
  EXPECT_EQ(master.getCost(3, 2), master.getCost(3, 4));
  EXPECT_GT(master.getCost(4, 3), master.getCost(5, 3));
  EXPECT_EQ(master.getCost(6, 3), FREE_SPACE);
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest( &argc, argv );
  return RUN_ALL_TESTS();
}