//! Used for testing the is_inside functions
#include "ff/Face.h"
#include "ff/Make.h"
#include "ff/Polyhedron.cpp"
#include "ff/Topology.h"
#include "test/3rdparty/catch.hpp"
#include "test/util/StreamOperators.h"
#include <iomanip>
#include <iostream>

namespace {
auto box = std::unique_ptr<ff::Polyhedron>(ff::make::Box(1, 1, 1)->asPolyhedron());

auto pyramid4 = std::unique_ptr<ff::Polyhedron>(ff::make::Pyramid4(1, 1, 1.4));
auto octahedron = std::unique_ptr<ff::Polyhedron>(ff::make::Octahedron(0.5));
auto icosahedron = std::unique_ptr<ff::Polyhedron>(ff::make::Icosahedron(0.5));
auto dodecahedron = std::unique_ptr<ff::Polyhedron>(ff::make::Dodecahedron(0.5));

//! function that takes an int as number of edges and returns a polygon with that number of edges

std::vector<R3> polygon(int n, int r = 1)
{
    std::vector<R3> result;
    for (int i = 0; i < n; i++)
        result.push_back({r * cos(2 * M_PI * i / n), r * sin(2 * M_PI * i / n), 0});
    return result;
};

bool insideFormCube(const R3& v)
{
    return v.x() > -0.5 && v.x() < 0.5 && v.y() > -0.5 && v.y() < 0.5 && v.z() > -0.5 && v.z() < 0.5;
}

bool insideFormOctahedron(const R3& v)
{
    double B = 0.5;
    double B05 = B / 2;
    double H05 = -octahedron->z_bottom(); // half-height

    if (v.z() > H05 || v.z() < -H05)
        return false;
    double d = (1 - std::abs(v.z()) / H05);
    return v.x() > -B05 * d && v.x() < B05 * d && v.y() > -B05 * d && v.y() < B05 * d;
}

bool insideFormPyramid4(const R3& v)
{
    double z_bot = pyramid4->z_bottom();
    double H = 1;
    double alpha = 1.4;

    if (v.z() < z_bot || v.z() > z_bot + H)
        return false;
    double z_rel = v.z() - z_bot; // height above base
    double l_z = 0.5 - z_rel / std::tan(alpha); // half-length of square at given height
    return std::abs(v.x()) <= l_z && std::abs(v.y()) <= l_z;
}

// TODO: add functions for all bodies

} // namespace

//! Test ff::Face::is_inside for multiple convex and concave faces
TEST_CASE("IsInsidePolygon", "[ff][Face][is_inside]")
{
    const std::vector<std::vector<R3>> edgeLoops{polygon(3), polygon(4), polygon(5), polygon(6),
                                                 polygon(7), polygon(8), polygon(9)};
    auto outer_vertices = polygon(16, 2);
    for (auto edge : edgeLoops) {
        ff::Face face(edge);

        INFO(edge);
        INFO(face.center_of_polygon());
        REQUIRE(face.is_inside(face.center_of_polygon()));
        for (const R3 outer_vertex : outer_vertices)
            REQUIRE(!face.is_inside(outer_vertex));
    }
}


//! Test if vertices are recognized correctly in intersects_in_positive_halfspace
TEST_CASE("IntersectsInPositiveHalfspace", "[ff][Face][intersects_in_positive_halfspace]")
{
    // Test Set storing r0, r1, i, expected
    const std::vector<std::tuple<R3, R3, R3, bool>> testSet{
        {{0, 0, 0}, {1, 0, 0}, {0.5, 0, 0}, true}, {{0, 0, 0}, {1, 0, 0}, {-0.5, 0, 0}, false},
        {{0, 0, 0}, {0, 1, 0}, {0, 0.5, 0}, true}, {{0, 0, 0}, {0, 1, 0}, {0, -0.5, 0}, false},
        {{0, 0, 0}, {0, 0, 1}, {0, 0, 0.5}, true}, {{0, 0, 0}, {0, 0, 1}, {0, 0, -0.5}, false},
    };
    for (const auto& [r0, r1, i, expected] : testSet)
        REQUIRE(intersects_in_positive_halfspace(r0, r1, i) == expected);
}

//! Test if vertices are correct in R3 ray_plane_intersection(R3 r0, R3 r1, R3 p0, R3 n)
TEST_CASE("RayPlaneIntersection", "[ff][Face][ray_plane_intersection]")
{
    // Test Set storing r0, r1, p0, n, expected
    const std::vector<std::tuple<R3, R3, R3, R3, R3>> testSet{
        {{0, 0, 0}, {1, 0, 0}, {1, 0, 0}, {1, 0, 0}, {1, 0, 0}},
        {{0, 0, 0}, {0, 1, 0}, {0, 1, 0}, {0, 1, 0}, {0, 1, 0}},
        {{0, 0, 0}, {0, 0, 1}, {0, 0, 1}, {0, 0, 1}, {0, 0, 1}},
    };
    for (const auto& [r0, r1, p0, n, expected] : testSet)
        REQUIRE(ray_plane_intersection(r0, r1, p0, n) == expected);
}

//! Test if the is_inside function works for Polyhedra in make : using specific calculations

TEST_CASE("IsInsidePolyhedron:Make", "[ff][Polyhedron][is_inside]")
{
    const double EPS = 1e-13;

    // Test Set storing polyhedron, detection function, vertices to test
    const std::vector<std::tuple<std::unique_ptr<ff::Polyhedron>&, std::function<bool(const R3&)>,
                                 std::vector<R3>>>
        testSet{
            {
                box,
                insideFormCube,
                {{0, 0, -0.5 - EPS},
                 {0, 0, -0.5 + EPS},
                 {0, 0, 0.5 - EPS},
                 {0, 0, 0.5 + EPS},
                 {0, 0, 0},
                 {-0.5 - EPS, -0.5 - EPS, 0},
                 {-0.5 + EPS, -0.5 + EPS, 0}},
            },
            {
                octahedron,
                insideFormOctahedron,
                {{0, 0, 0},
                 {0, 0, -octahedron->z_bottom() + EPS},
                 {0, 0, octahedron->z_bottom() - EPS},
                 {0, 0, +EPS}},
            },
            {pyramid4,
             insideFormPyramid4,
             {{0, 0, pyramid4->z_bottom() - EPS},
              {0, 0, pyramid4->z_bottom() + EPS},
              {0, 0, pyramid4->z_bottom() + 1 - EPS},
              {0, 0, pyramid4->z_bottom() + 1 + EPS},
              {0, 0, 0},
              {-0.5 - EPS, -0.5 - EPS, pyramid4->z_bottom() + 0.5},
              {-0.5 + EPS, -0.5 + EPS, pyramid4->z_bottom() + 0.5}}},
            // TODO: add more polyhedra with their tests from bornagain InsideForm.cpp
        };

    for (const auto& [polyhedron, testFunc, vertices] : testSet) {
        for (const R3& v : vertices) {
            INFO(v);
            REQUIRE(polyhedron->is_inside(v, 7) == testFunc(v));
        }
    }
}
