#include <random>
#include <memory>
#include <gtest/gtest.h>

#include <costmap_converter/costmap_to_polygons.h>

namespace {
geometry_msgs::Point32 create_point(double x, double y)
{
  geometry_msgs::Point32 result;
  result.x = x;
  result.y = y;
  result.z = 0.;
  return result;
}
} // end namespace

// make things accesible in the test
class CostmapToPolygons : public costmap_converter::CostmapToPolygonsDBSMCCH
{
  public:
    const std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint>& points() const {return occupied_cells_;}
    costmap_converter::CostmapToPolygonsDBSMCCH::Parameters& parameters() {return parameter_;}
    using costmap_converter::CostmapToPolygonsDBSMCCH::addPoint;
    using costmap_converter::CostmapToPolygonsDBSMCCH::regionQuery;
    using costmap_converter::CostmapToPolygonsDBSMCCH::dbScan;
    using costmap_converter::CostmapToPolygonsDBSMCCH::convexHull2;
    using costmap_converter::CostmapToPolygonsDBSMCCH::simplifyPolygon;
};

class CostmapToPolygonsDBSMCCHTest : public ::testing::Test
{
  protected:
    void SetUp() override {
      // parameters
      costmap_to_polygons.parameters().max_distance_ = 0.5;
      costmap_to_polygons.parameters().max_pts_ = 100;
      costmap_to_polygons.parameters().min_pts_ = 2;
      costmap_to_polygons.parameters().min_keypoint_separation_ = 0.1;

      costmap.reset(new costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
      costmap_to_polygons.setCostmap2D(costmap.get());

      std::random_device rand_dev;
      std::mt19937 generator(rand_dev());
      std::uniform_real_distribution<> random_angle(-M_PI, M_PI);
      std::uniform_real_distribution<> random_dist(0.,   costmap_to_polygons.parameters().max_distance_);

      costmap_to_polygons.addPoint(0., 0.);
      costmap_to_polygons.addPoint(1.3, 1.3);

      // adding random points
      double center_x = costmap_to_polygons.points()[0].x;
      double center_y = costmap_to_polygons.points()[0].y;
      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_ - 1; ++i)
      {
        double alpha = random_angle(generator);
        double dist  = random_dist(generator);
        double wx = center_x + std::cos(alpha) * dist;
        double wy = center_y + std::sin(alpha) * dist;
        costmap_to_polygons.addPoint(wx, wy);
      }

      // some noisy points not belonging to a cluster
      costmap_to_polygons.addPoint(-1, -1);
      costmap_to_polygons.addPoint(-2, -2);
      
      // adding random points
      center_x = costmap_to_polygons.points()[1].x;
      center_y = costmap_to_polygons.points()[1].y;
      for (int i = 0; i < costmap_to_polygons.parameters().max_pts_/2; ++i)
      {
        double alpha = random_angle(generator);
        double dist  = random_dist(generator);
        double wx = center_x + std::cos(alpha) * dist;
        double wy = center_y + std::sin(alpha) * dist;
        costmap_to_polygons.addPoint(wx, wy);
      }
    }

    void regionQueryTrivial(int curr_index, std::vector<int>& neighbor_indices)
    {
      neighbor_indices.clear();
      const auto& query_point = costmap_to_polygons.points()[curr_index];
      for (int i = 0; i < int(costmap_to_polygons.points().size()); ++i)
      {
        if (i == curr_index)
          continue;
        const auto& kp = costmap_to_polygons.points()[i];
        double dx = query_point.x - kp.x;
        double dy = query_point.y - kp.y;
        double dist = sqrt(dx*dx + dy*dy);
        if (dist < costmap_to_polygons.parameters().max_distance_)
          neighbor_indices.push_back(i);
      }
    }

    CostmapToPolygons costmap_to_polygons;
    std::shared_ptr<costmap_2d::Costmap2D> costmap;
};

TEST_F(CostmapToPolygonsDBSMCCHTest, regionQuery)
{
  std::vector<int> neighbors, neighbors_trivial;
  costmap_to_polygons.regionQuery(0, neighbors);
  regionQueryTrivial(0, neighbors_trivial);
  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_ - 1, int(neighbors.size()));
  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
  std::sort(neighbors.begin(), neighbors.end());
  ASSERT_EQ(neighbors_trivial, neighbors);

  costmap_to_polygons.regionQuery(1, neighbors);
  regionQueryTrivial(1, neighbors_trivial);
  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2, int(neighbors.size()));
  ASSERT_EQ(neighbors_trivial.size(), neighbors.size());
  std::sort(neighbors.begin(), neighbors.end());
  ASSERT_EQ(neighbors_trivial, neighbors);
}

TEST_F(CostmapToPolygonsDBSMCCHTest, dbScan)
{
  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
  costmap_to_polygons.dbScan(clusters);
  
  ASSERT_EQ(3, clusters.size());
  ASSERT_EQ(2, clusters[0].size()); // noisy points not belonging to a cluster
  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_, clusters[1].size()); // first cluster at (0,0)
  ASSERT_EQ(costmap_to_polygons.parameters().max_pts_/2 + 1, clusters[2].size()); // second cluster at (1,1)
}

TEST(CostmapToPolygonsDBSMCCH, EmptyMap)
{
  std::shared_ptr<costmap_2d::Costmap2D> costmap =
    std::make_shared<costmap_2d::Costmap2D>(costmap_2d::Costmap2D(100, 100, 0.1, -5., -5.));
  CostmapToPolygons costmap_to_polygons;
  costmap_to_polygons.setCostmap2D(costmap.get());

  std::vector< std::vector<costmap_converter::CostmapToPolygonsDBSMCCH::KeyPoint> > clusters;
  costmap_to_polygons.dbScan(clusters);
  ASSERT_EQ(1, clusters.size());    // noise cluster exists
  ASSERT_EQ(0, clusters[0].size()); // noise clsuter is empty
}

TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygon)
{
  const double simplification_threshold = 0.1;
  CostmapToPolygons costmap_to_polygons;
  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
  
  geometry_msgs::Polygon polygon;
  polygon.points.push_back(create_point(0., 0.));
  polygon.points.push_back(create_point(1., 0.));

  // degenerate case with just two points
  geometry_msgs::Polygon original_polygon = polygon;
  costmap_to_polygons.simplifyPolygon(polygon);
  ASSERT_EQ(2, polygon.points.size());
  for (size_t i = 0; i < polygon.points.size(); ++i)
  {
    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  
  }

  // degenerate case with three points
  polygon.points.push_back(create_point(1., simplification_threshold / 2.));
  original_polygon = polygon;
  costmap_to_polygons.simplifyPolygon(polygon);
  ASSERT_EQ(3, polygon.points.size());
  for (size_t i = 0; i < polygon.points.size(); ++i)
  {
    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);  
  }

  polygon.points.push_back(create_point(1., 1.));
  original_polygon = polygon;
  // remove the point that has to be removed by the simplification
  original_polygon.points.erase(original_polygon.points.begin()+2);
  costmap_to_polygons.simplifyPolygon(polygon);
  ASSERT_EQ(3, polygon.points.size());
  for (size_t i = 0; i < polygon.points.size(); ++i)
  {
    ASSERT_FLOAT_EQ(original_polygon.points[i].x, polygon.points[i].x);
    ASSERT_FLOAT_EQ(original_polygon.points[i].y, polygon.points[i].y);
  }
}

TEST(CostmapToPolygonsDBSMCCH, SimplifyPolygonPerfectLines)
{
  const double simplification_threshold = 0.1;
  CostmapToPolygons costmap_to_polygons;
  costmap_to_polygons.parameters().min_keypoint_separation_ = simplification_threshold;
  
  geometry_msgs::Polygon polygon;
  for (int i = 0; i <= 100; ++i)
    polygon.points.push_back(create_point(i*1., 0.));
  geometry_msgs::Point32 lastPoint = polygon.points.back();
  for (int i = 0; i <= 100; ++i)
    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
  lastPoint = polygon.points.back();
  for (int i = 0; i <= 100; ++i)
    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));
  lastPoint = polygon.points.back();
  for (int i = 0; i <= 100; ++i)
    polygon.points.push_back(create_point(lastPoint.x, lastPoint.y + i * 1.));
  lastPoint = polygon.points.back();
  for (int i = 0; i <= 100; ++i)
    polygon.points.push_back(create_point(lastPoint.x + i * 1., lastPoint.y));

  costmap_to_polygons.simplifyPolygon(polygon);
  ASSERT_EQ(6, polygon.points.size());
  ASSERT_FLOAT_EQ(  0., polygon.points[0].x);
  ASSERT_FLOAT_EQ(  0., polygon.points[0].y);
  ASSERT_FLOAT_EQ(100., polygon.points[1].x);
  ASSERT_FLOAT_EQ(  0., polygon.points[1].y);
  ASSERT_FLOAT_EQ(100., polygon.points[2].x);
  ASSERT_FLOAT_EQ(100., polygon.points[2].y);
  ASSERT_FLOAT_EQ(200., polygon.points[3].x);
  ASSERT_FLOAT_EQ(100., polygon.points[3].y);
  ASSERT_FLOAT_EQ(200., polygon.points[4].x);
  ASSERT_FLOAT_EQ(200., polygon.points[4].y);
  ASSERT_FLOAT_EQ(300., polygon.points[5].x);
  ASSERT_FLOAT_EQ(200., polygon.points[5].y);
}

int main(int argc, char** argv)
{
  testing::InitGoogleTest(&argc, argv);
  ros::init(argc, argv, "CostmapConverterTester");
  ros::NodeHandle nh;
  return RUN_ALL_TESTS();
}