#include "medusa/bits/domains/PolygonShape.hpp"
#include "gtest/gtest.h"

namespace mm {

TEST(Domains, PolygonShapeOrientationTriangle) {
    std::vector<Vec2d> points = {{0.0, 0.0}, {3.4, 1.2}, {-1.2, 2.9}};
    PolygonShape<Vec2d> poly(points);
    EXPECT_EQ(points, poly.points());

    std::vector<Vec2d> points2 = {{0.0, 0.0}, {-1.2, 2.9}, {3.4, 1.2}};
    PolygonShape<Vec2d> poly2(points2);
    EXPECT_NE(points2, poly2.points());
}

TEST(Domains, PolygonShapeOrientation) {
    std::vector<Vec2d> pts =
            {{0.2400, 0.1538}, {0.0646, 0.3092}, {0.3015, 0.3554}, {0.2015, 0.5692},
             {0.3985, 0.5415}, {0.6785, 0.6123}, {0.7938, 0.5692}, {0.7446, 0.4862},
             {0.5569, 0.5092}, {0.3969, 0.4477}, {0.4877, 0.4277}, {0.6200, 0.4277},
             {0.8062, 0.4446}, {0.8462, 0.5631}, {0.8000, 0.6600}, {0.9215, 0.6477},
             {0.9169, 0.5200}, {0.8846, 0.4000}, {0.6877, 0.3662}, {0.4108, 0.3508},
             {0.4677, 0.2585}, {0.7277, 0.2969}, {0.8138, 0.3169}, {0.8385, 0.2354},
             {0.8015, 0.1908}, {0.8631, 0.1123}, {0.8492, 0.0431}, {0.7538, 0.0369},
             {0.7154, 0.1169}, {0.7338, 0.2000}, {0.5723, 0.1692}, {0.6323, 0.0631},
             {0.5431, 0.0292}, {0.4831, 0.0738}, {0.5138, 0.1569}, {0.4846, 0.1985},
             {0.3908, 0.2446}, {0.2662, 0.2631}, {0.3831, 0.2000}, {0.4169, 0.1000},
             {0.3615, 0.0569}, {0.3015, 0.1692}, {0.2015, 0.2831}, {0.1800, 0.2477},
             {0.2862, 0.1477}, {0.2492, 0.0338}, {0.0477, 0.0892}, {0.0338, 0.2400},
             {0.0985, 0.1400}};

    PolygonShape<Vec2d> poly(pts);
    EXPECT_NE(pts, poly.points());

    std::reverse(pts.begin(), pts.end());
    PolygonShape<Vec2d> poly2(pts);
    EXPECT_EQ(pts, poly2.points());
}

TEST(Domains, PolygonShapeContainsV) {
    PolygonShape<Vec2d> poly({{0.0,  0.0}, {1.0, 2.0}, {0.0,  0.1}, {-1.0,  2.0}});

    EXPECT_TRUE(poly.contains({0.0, 0.0}));
    EXPECT_TRUE(poly.contains({-1.0, 2.0}));
    EXPECT_TRUE(poly.contains({1.0, 2.0}));
    EXPECT_TRUE(poly.contains({0.0, 0.1}));
    EXPECT_TRUE(poly.contains({0.0, 0.05}));
    EXPECT_FALSE(poly.contains({0.0, 0.2}));
    EXPECT_FALSE(poly.contains({-0.1, 0.0}));
    EXPECT_FALSE(poly.contains({0.1, 0.0}));
    EXPECT_FALSE(poly.contains({0.0, 1.0}));
    EXPECT_TRUE(poly.contains({0.5, 0.05+1.95*0.5}));
    EXPECT_TRUE(poly.contains({-0.5, 0.05+1.95*0.5}));
}

TEST(Domains, PolygonShapeContainsM) {
    PolygonShape<Vec2d> poly({{0.0, 0.0}, {4.0, 0.0}, {4.0, 4.0}, {2.0, 2.0}, {0.0, 4.0}});

    EXPECT_TRUE(poly.contains({0.0, 0.0}));
    EXPECT_TRUE(poly.contains({4.0, 0.0}));
    EXPECT_TRUE(poly.contains({4.0, 4.0}));
    EXPECT_TRUE(poly.contains({2.0, 2.0}));
    EXPECT_TRUE(poly.contains({0.0, 4.0}));
    EXPECT_TRUE(poly.contains({1.0, 1.0}));
    EXPECT_FALSE(poly.contains({2.0, 3.0}));
    EXPECT_FALSE(poly.contains({2.0, -1.0}));
    EXPECT_TRUE(poly.contains({3.5, 3.5}));
    EXPECT_TRUE(poly.contains({1.5, 1.5}));
}

TEST(Domains, PolygonShapeContainsStar) {
    PolygonShape<Vec2d> poly({{0.0, -2.0}, {1.0, -1.0}, {2.0, 0.0}, {1.0, 1.0}, {0.0, 2.0},
                              {-1.0, 1.0}, {-2.0, 0.0}, {-1.0, -1.0}});
    EXPECT_TRUE(poly.contains({0.0, -2.0}));
    EXPECT_TRUE(poly.contains({1.0, -1.0}));
    EXPECT_TRUE(poly.contains({2.0, 0.0}));
    EXPECT_TRUE(poly.contains({0.0, 2.0}));
    EXPECT_TRUE(poly.contains({0.0, -1.0}));
    EXPECT_TRUE(poly.contains({0.0, 1.0}));
    EXPECT_TRUE(poly.contains({-1.0, 0.0}));
    EXPECT_TRUE(poly.contains({1.0, 0.0}));
    EXPECT_TRUE(poly.contains({0.0, 0.0}));
}

TEST(Domains, PolygonShapeMargin) {
    // Unit square.
    PolygonShape<Vec2d> poly({{0, 0}, {1, 0}, {1, 1}, {0, 1}});
    double margin = poly.margin();
    EXPECT_TRUE(poly.contains({0.5, 1+margin/2}));
    EXPECT_FALSE(poly.contains({0.5, 1+2*margin}));
    EXPECT_TRUE(poly.contains({1+margin/2, 0.5}));
    EXPECT_FALSE(poly.contains({1+2*margin, 0.5}));

    poly.setMargin(-1e-8);
    margin = poly.margin();
    EXPECT_FALSE(poly.contains({0.5, 1+margin/2}));
    EXPECT_TRUE(poly.contains({0.5, 1+2*margin}));
    EXPECT_FALSE(poly.contains({1+margin/2, 0.5}));
    EXPECT_TRUE(poly.contains({1+2*margin, 0.5}));
}

TEST(Domains, PolygonRandomContains) {
    std::vector<Vec2d> pts = {
            {0.0985, 0.1400}, {0.0338, 0.2400}, {0.0477, 0.0892}, {0.2492, 0.0338},
            {0.2862, 0.1477}, {0.1800, 0.2477}, {0.2015, 0.2831}, {0.3015, 0.1692},
            {0.3615, 0.0569}, {0.4169, 0.1000}, {0.3831, 0.2000}, {0.2662, 0.2631},
            {0.3908, 0.2446}, {0.4846, 0.1985}, {0.5138, 0.1569}, {0.4831, 0.0738},
            {0.5431, 0.0292}, {0.6323, 0.0631}, {0.5723, 0.1692}, {0.7338, 0.2000},
            {0.7154, 0.1169}, {0.7538, 0.0369}, {0.8492, 0.0431}, {0.8631, 0.1123},
            {0.8015, 0.1908}, {0.8385, 0.2354}, {0.8138, 0.3169}, {0.7277, 0.2969},
            {0.4677, 0.2585}, {0.4108, 0.3508}, {0.6877, 0.3662}, {0.8846, 0.4000},
            {0.9169, 0.5200}, {0.9215, 0.6477}, {0.8000, 0.6600}, {0.8462, 0.5631},
            {0.8062, 0.4446}, {0.6200, 0.4277}, {0.4877, 0.4277}, {0.3969, 0.4477},
            {0.5569, 0.5092}, {0.7446, 0.4862}, {0.7938, 0.5692}, {0.6785, 0.6123},
            {0.3985, 0.5415}, {0.2015, 0.5692}, {0.3015, 0.3554}, {0.0646, 0.3092},
            {0.2400, 0.1538}};

    PolygonShape<Vec2d> poly(pts);

    EXPECT_EQ(pts, poly.points());  // no orientation change

    // Results were obtained using Matlab's inpolygon function on the polygon with changed margins.
    // If margins are not taken into account, two points fail.
    std::string results =
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000001100000000000000000000001110000000000000001000000000000000000"
            "00000000000000001111100000000000000000000011111100000000000011111111100000000000"
            "00000000000011111111100000000100000000001111111111100000000011111111100000000000"
            "00000000111111111111110000000110000000011111111111100000000111111111100000000000"
            "00000111111111111111110000001111100000001111111111000000000111111111100000000000"
            "00001111111111111111110000001111110000001111111110000000001111111111100000000000"
            "00001111111111111111110000011111110000001111111110000000001111111111110000000000"
            "00001111111111111111111000011111100000000111111100000000001111111111100000000000"
            "00001111111111111111111000111111100000000111111100000000001111111111000000000000"
            "00001111000000000111111001111111100000000111111000000000001111111110000000000000"
            "00001110000000000001110001111111000000000111111000000000000111111100000000000000"
            "00001110000000000011100011111111000000000111111110000000000111111100000000000000"
            "00001100000000000111000111111110000000001111111111111100000111111000000000000000"
            "00011000000000001110001111111110000000011111111111111111111111111000000000000000"
            "00011000000000111100001111111000000001111111111111111111111111111100000000000000"
            "00010000000001111000011111100000000111111111111111111111111111111110000000000000"
            "00000000000011110000111111000000011111111111111111111111111111111111000000000000"
            "00000000000111100001111100000111111111111111111111111111111111111110000000000000"
            "00000000001111110011111111111111111111001111111111111111111111111110000000000000"
            "00000000011111110111111111111111111110000000000111111111111111111110000000000000"
            "00000000111111111111111111111111111100000000000000000011111111111100000000000000"
            "00000011111111111111111111111111111100000000000000000000000011111100000000000000"
            "00000001111111111111111111111111111000000000000000000000000000001100000000000000"
            "00000000000011111111111111111111111000000000000000000000000000000000000000000000"
            "00000000000000000111111111111111110000000000000000000000000000000000000000000000"
            "00000000000000000000001111111111100000000000000000000000000000000000000000000000"
            "00000000000000000000000011111111111111111111111111000000000000000000000000000000"
            "00000000000000000000000011111111111111111111111111111111111100000000000000000000"
            "00000000000000000000000111111111111111111111111111111111111111111000000000000000"
            "00000000000000000000000111111111111111111111111111111111111111111111111000000000"
            "00000000000000000000001111111111111111111111111111111111111111111111111100000000"
            "00000000000000000000001111111111111111111111111111111111111111111111111100000000"
            "00000000000000000000001111111111111100000000000000000000000111111111111100000000"
            "00000000000000000000011111111111100000000000000000000000000000000111111100000000"
            "00000000000000000000011111111111111000000000000000000000000000000111111110000000"
            "00000000000000000000111111111111111111000000000000000000000000000011111110000000"
            "00000000000000000000111111111111111111111000000000000000000100000011111110000000"
            "00000000000000000001111111111111111111111110000000011111111110000011111110000000"
            "00000000000000000001111111111111111111111111111111111111111110000001111111000000"
            "00000000000000000011111111111111111111111111111111111111111111000001111111000000"
            "00000000000000000011111111111111111111111111111111111111111111100000111111000000"
            "00000000000000000111111111110000000111111111111111111111111111100000111111000000"
            "00000000000000000111000000000000000000011111111111111111111111110000111111000000"
            "00000000000000000000000000000000000000000001111111111111111111100000111111000000"
            "00000000000000000000000000000000000000000000000111111111111100000001111111000000"
            "00000000000000000000000000000000000000000000000000011111100000000001111111000000"
            "00000000000000000000000000000000000000000000000000000000000000000011111111000000"
            "00000000000000000000000000000000000000000000000000000000000000000011111111000000"
            "00000000000000000000000000000000000000000000000000000000000000000111111111000000"
            "00000000000000000000000000000000000000000000000000000000000000000111111100000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000"
            "00000000000000000000000000000000000000000000000000000000000000000000000000000000";

    int N = 80;
    int c = 0;
    for (int i = 0; i < N; ++i) {
        for (int j = 0; j < N; ++j) {
            double x = static_cast<double>(j)/N;
            double y = static_cast<double>(i)/N;
            bool inside = poly.contains({x, y});
            EXPECT_EQ(inside, results[c] == '1') << "Failed at #" << c << ": " << x << ", " << y;
            c++;
        }
    }
}

TEST(Domains, PolygonDiscretizeBoundaryWithDensity) {
    PolygonShape<Vec2d> domain2d({{0.0, -2.0}, {1.3, -1.0}, {2.0, 0.0}, {1.0, 2.7}, {0.0, 3.0},
                                  {-1.0, 0.5}, {-2.0, 0.0}, {-1.6, -1.0}});
    double dx1 = 0.1;
    double dx2 = 0.02;
    double tol = 0.05;
    double s1 = 0;
    double c1 = 0;
    double s2 = 0;
    double c2 = 0;

    auto fill = [=](const Vec2d& p) { return (p[0] < 0.0) ? dx1 : dx2; };
    auto discretization2d = domain2d.discretizeBoundaryWithDensity(fill);
    EXPECT_EQ(discretization2d.size(), 408);

    KDTree<Vec2d> tree(discretization2d.positions());
    for (int i = 0; i  < discretization2d.size(); ++i) {
        if (discretization2d.pos(i, 0) < 0.0 - tol) {
            Range<double> distances2 = std::get<1>(tree.query(discretization2d.pos(i), 2));
            s1 += std::sqrt(distances2[1]);
            c1 += 1;
        } else if (discretization2d.pos(i, 0) > 0.0 + tol) {
            Range<double> distances2 = std::get<1>(tree.query(discretization2d.pos(i), 2));
            s2 += std::sqrt(distances2[1]);
            c2 += 1;
        }
    }

    EXPECT_NEAR(dx1, s1/c1, dx1*tol);  // 5% error tolerance
    EXPECT_NEAR(dx2, s2/c2, dx2*tol);
}

TEST(Domains, PolygonDiscretizeBoundaryWithStep) {
    PolygonShape<Vec2d> domain2d({{0.0, -2.0}, {1.3, -1.0}, {2.0, 0.0}, {1.0, 2.7}, {0.0, 3.0},
                                  {-1.0, 0.5}, {-2.0, 0.0}, {-1.6, -1.0}});
    double dx = 0.05;
    double tol = 0.05;
    double s = 0;
    double c = 0;

    auto discretization2d = domain2d.discretizeBoundaryWithStep(dx);
    EXPECT_EQ(discretization2d.size(), 274);

    KDTree<Vec2d> tree(discretization2d.positions());
    for (int i = 0; i  < discretization2d.size(); ++i) {
        Range<double> distances2 = std::get<1>(tree.query(discretization2d.pos(i), 2));
        s += std::sqrt(distances2[1]);
        c += 1;
    }

    EXPECT_NEAR(dx, s/c, dx*tol);  // 5% error tolerance
}

TEST(Domains, PolygonShapeUsageExample) {
    /// [PolygonShape usage example]
    PolygonShape<Vec2d> poly({{0.0, -2.0}, {1.0, -1.0}, {2.0, 0.0}, {1.0, 1.0}, {0.0, 2.0},
                              {-1.0, 1.0}, {-2.0, 0.0}, {-1.0, -1.0}});
    if (poly.contains({2.3, 4.5})) {
        // do something
    }
    std::cout << poly << std::endl;
    auto d = poly.discretizeBoundaryWithStep(0.1);
    /// [PolygonShape usage example]
    (void) d;
}

}  // namespace mm