/*
 * PolygonTest.cpp
 *
 *  Created on: Mar 24, 2015
 *      Author: Martin Wermelinger, Péter Fankhauser
 *	 Institute: ETH Zurich, ANYbotics
 */

#include "grid_map_core/Polygon.hpp"

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

// Eigen
#include <Eigen/Core>

using Eigen::MatrixXd;
using Eigen::Vector2d;
using Eigen::VectorXd;

using grid_map::Length;
using grid_map::Polygon;
using grid_map::Position;

TEST(Polygon, getCentroidTriangle)
{
  Polygon triangle;
  triangle.addVertex(Vector2d(0.0, 0.0));
  triangle.addVertex(Vector2d(1.0, 0.0));
  triangle.addVertex(Vector2d(0.5, 1.0));

  Position expectedCentroid;
  expectedCentroid.x() = 1.0 / 3.0 * (1.0 + 0.5);
  expectedCentroid.y() = 1.0 / 3.0;
  Position centroid = triangle.getCentroid();
  EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
  EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
}

TEST(Polygon, getCentroidRectangle)
{
  Polygon rectangle;
  rectangle.addVertex(Vector2d(-2.0, -1.0));
  rectangle.addVertex(Vector2d(-2.0, 2.0));
  rectangle.addVertex(Vector2d(1.0, 2.0));
  rectangle.addVertex(Vector2d(1.0, -1.0));

  Position expectedCentroid(-0.5, 0.5);
  Position centroid = rectangle.getCentroid();
  EXPECT_DOUBLE_EQ(expectedCentroid.x(), centroid.x());
  EXPECT_DOUBLE_EQ(expectedCentroid.y(), centroid.y());
}

TEST(Polygon, getBoundingBox)
{
  Polygon triangle;
  triangle.addVertex(Vector2d(0.0, 0.0));
  triangle.addVertex(Vector2d(0.5, -1.2));
  triangle.addVertex(Vector2d(1.0, 0.0));

  Position expectedCenter(0.5, -0.6);
  Length expectedLength(1.0, 1.2);
  Position center;
  Length length;
  triangle.getBoundingBox(center, length);

  EXPECT_DOUBLE_EQ(expectedCenter.x(), center.x());
  EXPECT_DOUBLE_EQ(expectedCenter.y(), center.y());
  EXPECT_DOUBLE_EQ(expectedLength.x(), length.x());
  EXPECT_DOUBLE_EQ(expectedLength.y(), length.y());
}

TEST(Polygon, convexHullPoints)
{
  // Test that points which already create a convex shape (square) can be used to create a convex polygon.
  std::vector<Position> points1;
  points1.emplace_back(0.0, 0.0);
  points1.emplace_back(1.0, 0.0);
  points1.emplace_back(1.0, 1.0);
  points1.emplace_back(0.0, 1.0);
  Polygon polygon1 = Polygon::monotoneChainConvexHullOfPoints(points1);
  EXPECT_EQ(4, polygon1.nVertices());
  EXPECT_TRUE(polygon1.isInside(Vector2d(0.5, 0.5)));
  EXPECT_FALSE(polygon1.isInside(Vector2d(-0.01, 0.5)));

  // Test that a random set of points can be used to create a convex polygon.
  std::vector<Position> points2;
  points2.emplace_back(0.0, 0.0);
  points2.emplace_back(1.0, 0.0);
  points2.emplace_back(2.0, 1.0);
  points2.emplace_back(1.0, 2.0);
  points2.emplace_back(-1.0, 2.0);
  points2.emplace_back(-1.0, -2.0);
  points2.emplace_back(0.0, 1.0);
  points2.emplace_back(1.0, 1.0);
  Polygon polygon2 = Polygon::monotoneChainConvexHullOfPoints(points2);
  EXPECT_EQ(4, polygon2.nVertices());
  EXPECT_TRUE(polygon2.isInside(Vector2d(0.5, 0.5)));
  EXPECT_TRUE(polygon2.isInside(Vector2d(0.0, 1.0)));
  EXPECT_TRUE(polygon2.isInside(Vector2d(-0.5, -0.5)));
  EXPECT_FALSE(polygon2.isInside(Vector2d(2.0, 0.0)));
  EXPECT_FALSE(polygon2.isInside(Vector2d(-0.5, -2)));
  EXPECT_FALSE(polygon2.isInside(Vector2d(1.75, 1.75)));
}

TEST(Polygon, convexHullPolygon)
{
  Polygon polygon1;
  polygon1.addVertex(Vector2d(0.0, 0.0));
  polygon1.addVertex(Vector2d(1.0, 1.0));
  polygon1.addVertex(Vector2d(0.0, 1.0));
  polygon1.addVertex(Vector2d(1.0, 0.0));

  Polygon polygon2;
  polygon2.addVertex(Vector2d(0.5, 0.5));
  polygon2.addVertex(Vector2d(0.5, 1.5));
  polygon2.addVertex(Vector2d(1.5, 0.5));
  polygon2.addVertex(Vector2d(1.5, 1.5));

  Polygon hull = Polygon::convexHull(polygon1, polygon2);

  EXPECT_EQ(6, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.5)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.01, 1.49)));
}

TEST(Polygon, convexHullCircles)
{
  Position center1(0.0, 0.0);
  Position center2(1.0, 0.0);
  double radius = 0.5;
  const int nVertices = 15;

  Polygon hull = Polygon::convexHullOfTwoCircles(center1, center2, radius);
  EXPECT_EQ(20, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
  EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));

  hull = Polygon::convexHullOfTwoCircles(center1, center2, radius, nVertices);
  EXPECT_EQ(nVertices + 1, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.5, 0.4)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.6)));
  EXPECT_FALSE(hull.isInside(Vector2d(1.5, 0.2)));

  hull = Polygon::convexHullOfTwoCircles(center1, center1, radius);
  EXPECT_EQ(20, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));

  hull = Polygon::convexHullOfTwoCircles(center1, center1, radius, nVertices);
  EXPECT_EQ(nVertices, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.0, 0.25)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.0, -0.25)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.5)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.6, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(-0.6, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.0, 0.6)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.0, -0.6)));
}

TEST(Polygon, convexHullCircle)
{
  Position center(0.0, 0.0);
  double radius = 0.5;
  const int nVertices = 15;

  Polygon hull = Polygon::fromCircle(center, radius);

  EXPECT_EQ(20, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
  EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));

  hull = Polygon::fromCircle(center, radius, nVertices);
  EXPECT_EQ(nVertices, hull.nVertices());
  EXPECT_TRUE(hull.isInside(Vector2d(-0.25, 0.0)));
  EXPECT_TRUE(hull.isInside(Vector2d(0.49, 0.0)));
  EXPECT_FALSE(hull.isInside(Vector2d(0.5, 0.4)));
  EXPECT_FALSE(hull.isInside(Vector2d(1.0, 0.0)));
}

TEST(convertToInequalityConstraints, triangle1)
{
  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.1, -1.1)});
  MatrixXd A;
  VectorXd b;
  ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
  EXPECT_NEAR(-1.3636, A(0, 0), 1e-4);
  EXPECT_NEAR( 1.3636, A(0, 1), 1e-4);
  EXPECT_NEAR(-1.5000, A(1, 0), 1e-4);
  EXPECT_NEAR(-1.5000, A(1, 1), 1e-4);
  EXPECT_NEAR( 2.8636, A(2, 0), 1e-4);
  EXPECT_NEAR( 0.1364, A(2, 1), 1e-4);
  EXPECT_NEAR( 0.0000, b(0), 1e-4);
  EXPECT_NEAR( 0.0000, b(1), 1e-4);
  EXPECT_NEAR( 3.0000, b(2), 1e-4);
}

TEST(convertToInequalityConstraints, triangle2)
{
  Polygon polygon({Position(-1.0, 0.5), Position(-1.0, -0.5), Position(1.0, -0.5)});
  MatrixXd A;
  VectorXd b;
  ASSERT_TRUE(polygon.convertToInequalityConstraints(A, b));
  EXPECT_NEAR(-1.5000, A(0, 0), 1e-4);
  EXPECT_NEAR( 0.0000, A(0, 1), 1e-4);
  EXPECT_NEAR( 0.0000, A(1, 0), 1e-4);
  EXPECT_NEAR(-3.0000, A(1, 1), 1e-4);
  EXPECT_NEAR( 1.5000, A(2, 0), 1e-4);
  EXPECT_NEAR( 3.0000, A(2, 1), 1e-4);
  EXPECT_NEAR( 1.5000, b(0), 1e-4);
  EXPECT_NEAR( 1.5000, b(1), 1e-4);
  EXPECT_NEAR( 0.0000, b(2), 1e-4);
}

TEST(offsetInward, triangle)
{
  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
  polygon.offsetInward(0.1);
  EXPECT_NEAR(0.9, polygon.getVertex(0)(0), 1e-4);
  EXPECT_NEAR(0.758579, polygon.getVertex(0)(1), 1e-4);
  EXPECT_NEAR(0.141421, polygon.getVertex(1)(0), 1e-4);
  EXPECT_NEAR(0.0, polygon.getVertex(1)(1), 1e-4);
  EXPECT_NEAR(0.9, polygon.getVertex(2)(0), 1e-4);
  EXPECT_NEAR(-0.758579, polygon.getVertex(2)(1), 1e-4);
}

TEST(triangulation, triangle)
{
  Polygon polygon({Position(1.0, 1.0), Position(0.0, 0.0), Position(1.0, -1.0)});
  std::vector<Polygon> polygons;
  polygons = polygon.triangulate();
  ASSERT_EQ(1, polygons.size());
  EXPECT_EQ(polygon.getVertex(0).x(), polygons[0].getVertex(0).x());
  EXPECT_EQ(polygon.getVertex(0).y(), polygons[0].getVertex(0).y());
  EXPECT_EQ(polygon.getVertex(1).x(), polygons[0].getVertex(1).x());
  EXPECT_EQ(polygon.getVertex(1).y(), polygons[0].getVertex(1).y());
  EXPECT_EQ(polygon.getVertex(2).x(), polygons[0].getVertex(2).x());
  EXPECT_EQ(polygon.getVertex(2).y(), polygons[0].getVertex(2).y());
}

TEST(triangulation, rectangle)
{
  Polygon rectangle;
  rectangle.addVertex(Vector2d(-2.0, -1.0));
  rectangle.addVertex(Vector2d(-2.0, 2.0));
  rectangle.addVertex(Vector2d(1.0, 2.0));
  rectangle.addVertex(Vector2d(1.0, -1.0));
  std::vector<Polygon> polygons;
  polygons = rectangle.triangulate();
  ASSERT_EQ(2, polygons.size());
  // TODO Extend.
}