//  Verifies FormfactorPolyhedron::contains(), the convex
//  point-in-polyhedron test by intersection of face half-spaces. For shapes with compact
//  analytic membership formulas (Box, prisms, pyramids, Bipyramid4) and for the Platonic
//  solids, we require exact agreement away from the boundary. For all polyhedra we cross-check
//  the canonical volume() against closed-form geometry formulas.

#include "Base/Const/Units.h"
#include "Sample/HardParticle/HardParticles.h"
#include "Tests/GTestWrapper/google_test.h"
#include <algorithm>
#include <cmath>
#include <ff/Polyhedron.h>
#include <functional>
#include <numbers>

using std::numbers::pi;

namespace {

using Oracle = std::function<bool(const R3&)>;

struct Plane {
    R3 normal;
    double offset;
};

//! Requires contains() to equal the analytic oracle at every grid point that is
//! unambiguously inside or outside (i.e. the oracle verdict is stable under a small shift),
//! thereby skipping only a thin shell around the boundary. Also asserts that a meaningful
//! number of points of each kind were actually checked.
void expectAgreesWithOracle(const FormfactorPolyhedron& ff, const Oracle& oracle, double hx,
                            double hy, double ztop)
{
    const double delta = 1e-4 * std::max({hx, hy, ztop});
    const int n = 23;
    auto stable = [&](const R3& p, bool& verdict) {
        verdict = oracle(p);
        for (int axis = 0; axis < 3; ++axis)
            for (int s = -1; s <= 1; s += 2) {
                const R3 q(p.x() + (axis == 0 ? s * delta : 0), p.y() + (axis == 1 ? s * delta : 0),
                           p.z() + (axis == 2 ? s * delta : 0));
                if (oracle(q) != verdict)
                    return false;
            }
        return true;
    };
    int n_inside = 0, n_outside = 0, n_mismatch = 0;
    for (int ix = 0; ix < n; ++ix)
        for (int iy = 0; iy < n; ++iy)
            for (int iz = 0; iz < n; ++iz) {
                const R3 p(hx * (-1.3 + 2.6 * ix / (n - 1.)), hy * (-1.3 + 2.6 * iy / (n - 1.)),
                           ztop * (-0.3 + 1.6 * iz / (n - 1.)));
                bool verdict;
                if (!stable(p, verdict))
                    continue;
                if (verdict)
                    ++n_inside;
                else
                    ++n_outside;
                if (ff.contains(p) != verdict)
                    ++n_mismatch;
            }
    EXPECT_EQ(n_mismatch, 0);
    EXPECT_GT(n_inside, 50);
    EXPECT_GT(n_outside, 50);
}

bool containedByPlanes(const R3& p, const std::vector<Plane>& planes)
{
    for (const Plane& plane : planes)
        if (plane.normal.dot(p) - plane.offset > 0)
            return false;
    return true;
}

std::vector<Plane> planesFromFaces(const std::vector<R3>& vertices,
                                   const std::vector<std::vector<int>>& faces, const R3& inner)
{
    std::vector<Plane> result;
    result.reserve(faces.size());
    for (const std::vector<int>& face : faces) {
        const R3 on_plane = vertices[face[0]];
        R3 normal = (vertices[face[1]] - on_plane).cross(vertices[face[2]] - on_plane);
        if (normal.dot(inner - on_plane) > 0) // orient outward: inner point on negative side
            normal = -normal;
        normal = normal / normal.mag();
        result.push_back({normal, normal.dot(on_plane)});
    }
    return result;
}

Oracle planeOracle(const std::vector<R3>& vertices, const std::vector<std::vector<int>>& faces,
                   const R3& inner)
{
    const std::vector<Plane> planes = ::planesFromFaces(vertices, faces, inner);
    return [planes](const R3& p) { return ::containedByPlanes(p, planes); };
}

Oracle tetrahedronOracle(double edge)
{
    const double r = edge / std::sqrt(3.);
    const double height = std::sqrt(2. / 3.) * edge;
    return [r, height](const R3& p) {
        if (p.z() < 0 || p.z() > height)
            return false;
        const double s = 1 - p.z() / height;
        return p.x() >= -s * r / 2 && std::abs(p.y()) <= (s * r - p.x()) / std::sqrt(3.);
    };
}

Oracle octahedronOracle(double edge)
{
    const double half_height = edge / std::sqrt(2.);
    const double half_edge = edge / 2;
    return [half_height, half_edge](const R3& p) {
        const double z = p.z() - half_height;
        if (std::abs(z) > half_height)
            return false;
        const double half_width = half_edge * (1 - std::abs(z) / half_height);
        return std::abs(p.x()) <= half_width && std::abs(p.y()) <= half_width;
    };
}

double regularPolygonArea(int n_vertices, double edge)
{
    return n_vertices * edge * edge / (4 * std::tan(pi / n_vertices));
}

double regularPrismVolume(int n_vertices, double edge, double height)
{
    return ::regularPolygonArea(n_vertices, edge) * height;
}

double regularFrustumVolume(int n_vertices, double bottom_edge, double top_edge, double height)
{
    const double area_factor = n_vertices / (4 * std::tan(pi / n_vertices));
    return height * area_factor
           * (bottom_edge * bottom_edge + bottom_edge * top_edge + top_edge * top_edge) / 3;
}

double squareFrustumVolume(double bottom_edge, double top_edge, double height)
{
    return height * (bottom_edge * bottom_edge + bottom_edge * top_edge + top_edge * top_edge) / 3;
}

double pyramid2Volume(double length, double width, double height, double alpha)
{
    const double r = height * std::tan(pi / 2 - alpha);
    return height * (length * width - (length + width) * r + 4 * r * r / 3);
}

double pyramid3Volume(double base_edge, double height, double alpha)
{
    const double top_edge = base_edge - 2 * std::sqrt(3.) * height * std::tan(pi / 2 - alpha);
    return ::regularFrustumVolume(3, base_edge, top_edge, height);
}

double pyramid4Volume(double base_edge, double height, double alpha)
{
    const double top_edge = base_edge - 2 * height * std::tan(pi / 2 - alpha);
    return ::squareFrustumVolume(base_edge, top_edge, height);
}

double pyramid6Volume(double base_edge, double height, double alpha)
{
    const double top_edge = base_edge - 2 * height * std::tan(pi / 2 - alpha) / std::sqrt(3.);
    return ::regularFrustumVolume(6, base_edge, top_edge, height);
}

double bipyramid4Volume(double length, double base_height, double height_ratio, double alpha)
{
    const double r = std::tan(pi / 2 - alpha) * 2 * base_height / length;
    const double lower_edge = length * (1 - r);
    const double upper_edge = length * (1 - r * height_ratio);
    return ::squareFrustumVolume(lower_edge, length, base_height)
           + ::squareFrustumVolume(length, upper_edge, height_ratio * base_height);
}

double cantellatedCubeVolume(double length, double removed_length)
{
    return std::pow(length, 3) - 6 * length * removed_length * removed_length
           + 16 * std::pow(removed_length, 3) / 3;
}

double truncatedCubeVolume(double length, double removed_length)
{
    return std::pow(length, 3) - 4 * std::pow(removed_length, 3) / 3;
}

Oracle dodecahedronOracle(double edge)
{
    const double phi = (1 + std::sqrt(5.)) / 2;
    const double r1 = std::sqrt((5 - std::sqrt(5.)) / 40) * edge;
    const double r2 = r1 * phi;
    const double r3 = edge / 2;
    const double r4 = r2 * phi;
    const double r5 = r3 * phi;
    const double r6 = 2 * r1 * phi;
    const double r7 = r4 * phi;
    const double r8 = r5 * phi;
    const double r9 = r6 * phi;
    const std::vector<R3> vertices{
        {r6, 0, 0},          {r1, r5, 0},        {-r4, r3, 0},       {-r4, -r3, 0},
        {r1, -r5, 0},        {r9, 0, r7 - r1},   {r2, r8, r7 - r1},  {-r7, r5, r7 - r1},
        {-r7, -r5, r7 - r1}, {r2, -r8, r7 - r1}, {-r9, 0, r7 + r1},  {-r2, -r8, r7 + r1},
        {r7, -r5, r7 + r1},  {r7, r5, r7 + r1},  {-r2, r8, r7 + r1}, {-r6, 0, 2 * r7},
        {-r1, -r5, 2 * r7},  {r4, -r3, 2 * r7},  {r4, r3, 2 * r7},   {-r1, r5, 2 * r7}};
    const std::vector<std::vector<int>> faces{
        {0, 4, 3, 2, 1},     {0, 5, 12, 9, 4},    {4, 9, 11, 8, 3},    {3, 8, 10, 7, 2},
        {2, 7, 14, 6, 1},    {1, 6, 13, 5, 0},    {8, 11, 16, 15, 10}, {9, 12, 17, 16, 11},
        {5, 13, 18, 17, 12}, {6, 14, 19, 18, 13}, {7, 10, 15, 19, 14}, {15, 16, 17, 18, 19}};
    return ::planeOracle(vertices, faces, R3(0, 0, r7));
}

Oracle icosahedronOracle(double edge)
{
    const double phi = (1 + std::sqrt(5.)) / 2;
    const double s2 = edge / (2 * std::sqrt(3.));
    const double s1 = s2 / phi;
    const double s3 = s2 * phi;
    const double s4 = edge / 2;
    const double s5 = 2 * s2;
    const double s6 = s3 * phi;
    const double s7 = phi * edge / 2;
    const double s8 = s5 * phi;
    const std::vector<R3> vertices{{s5, 0, 0},        {-s2, s4, 0},       {-s2, -s4, 0},
                                   {-s8, 0, s6 - s1}, {s3, s7, s6 - s1},  {s3, -s7, s6 - s1},
                                   {s8, 0, s6 + s1},  {-s3, s7, s6 + s1}, {-s3, -s7, s6 + s1},
                                   {-s5, 0, 2 * s6},  {s2, s4, 2 * s6},   {s2, -s4, 2 * s6}};
    const std::vector<std::vector<int>> faces{
        {0, 2, 1},  {0, 5, 2}, {2, 3, 1},  {1, 4, 0},   {0, 6, 5},  {2, 5, 8},  {2, 8, 3},
        {1, 3, 7},  {1, 7, 4}, {0, 4, 6},  {3, 8, 9},   {5, 11, 8}, {5, 6, 11}, {4, 10, 6},
        {4, 7, 10}, {3, 9, 7}, {8, 11, 9}, {6, 10, 11}, {7, 9, 10}, {9, 11, 10}};
    return ::planeOracle(vertices, faces, R3(0, 0, s6));
}

} // namespace


// Exact agreement with analytic membership formulas independent of
// FormfactorPolyhedron::contains(). Covers boundary-adjacent (but unambiguous) points on
// every face.
TEST(PolyhedronContains, MatchesAnalyticOracle)
{
    const Box box(50, 40, 30);
    ::expectAgreesWithOracle(
        box,
        [&](const R3& p) {
            return std::abs(p.x()) <= box.length() / 2 && std::abs(p.y()) <= box.width() / 2
                   && p.z() >= 0 && p.z() <= box.height();
        },
        25, 20, 30);

    const Prism3 prism3(40, 30);
    ::expectAgreesWithOracle(
        prism3,
        [&](const R3& p) {
            const double B = prism3.baseEdge(), H = prism3.height();
            const double l = B * std::sin(pi / 3), x_shift = B / 2 * std::tan(pi / 6);
            if (p.x() + x_shift < 0 || p.x() + x_shift > l || std::abs(p.y()) > B / 2 || p.z() < 0
                || p.z() > H)
                return false;
            double theta = 0;
            if (p.x() + x_shift != 0 || p.y() != 0)
                theta = std::asin(std::abs(p.y())
                                  / std::sqrt(std::pow(p.x() + x_shift, 2) + std::pow(p.y(), 2)));
            const double k = l / (std::sin(theta) / std::tan(pi / 6) + std::cos(theta));
            return std::pow(p.x() + x_shift, 2) + std::pow(p.y(), 2) <= std::pow(k, 2);
        },
        40, 40, 30);

    const Prism6 prism6(40, 30);
    ::expectAgreesWithOracle(
        prism6,
        [&](const R3& p) {
            const double B = prism6.baseEdge(), H = prism6.height();
            if (std::abs(p.x()) > B || std::abs(p.y()) > B || p.z() < 0 || p.z() > H)
                return false;
            double theta_prime = 0;
            if (p.x() != 0 || p.y() != 0)
                theta_prime = Units::rad2deg(std::asin(
                    std::abs(p.y()) / std::sqrt(std::pow(p.x(), 2) + std::pow(p.y(), 2))));
            const int c = static_cast<int>(theta_prime / 60);
            const double theta = Units::deg2rad(theta_prime - c * 60);
            const double k_z = B / (std::cos(theta) + std::sin(theta) / std::tan(pi / 3));
            return std::pow(p.x(), 2) + std::pow(p.y(), 2) <= std::pow(k_z, 2);
        },
        40, 40, 30);

    const Pyramid2 pyr2(50, 40, 12, 0.8);
    ::expectAgreesWithOracle(
        pyr2,
        [&](const R3& p) {
            const double l_z = pyr2.length() / 2 - p.z() / std::tan(pyr2.alpha());
            const double w_z = pyr2.width() / 2 - p.z() / std::tan(pyr2.alpha());
            return std::abs(p.x()) <= l_z && std::abs(p.y()) <= w_z && p.z() >= 0
                   && p.z() <= pyr2.height();
        },
        25, 20, 12);

    const Pyramid3 pyr3(50, 8, 0.8);
    ::expectAgreesWithOracle(
        pyr3,
        [&](const R3& p) {
            const double b_z = pyr3.baseEdge() - 2 * std::sqrt(3.) * p.z() / std::tan(pyr3.alpha());
            const double l = b_z * std::sin(pi / 3), x_shift = b_z / 2 * std::tan(pi / 6);
            if (p.x() + x_shift < 0 || p.x() + x_shift > l || std::abs(p.y()) > b_z / 2 || p.z() < 0
                || p.z() > pyr3.height())
                return false;
            double theta = 0;
            if (p.x() + x_shift != 0 || p.y() != 0)
                theta = std::asin(std::abs(p.y())
                                  / std::sqrt(std::pow(p.x() + x_shift, 2) + std::pow(p.y(), 2)));
            const double k = l / (std::sin(theta) / std::tan(pi / 6) + std::cos(theta));
            return std::pow(p.x() + x_shift, 2) + std::pow(p.y(), 2) <= std::pow(k, 2);
        },
        25, 25, 8);

    const Pyramid4 pyr4(50, 12, 0.8);
    ::expectAgreesWithOracle(
        pyr4,
        [&](const R3& p) {
            const double l_z = pyr4.baseEdge() / 2 - p.z() / std::tan(pyr4.alpha());
            return std::abs(p.x()) <= l_z && std::abs(p.y()) <= l_z && p.z() >= 0
                   && p.z() <= pyr4.height();
        },
        25, 25, 12);

    const Pyramid6 pyr6(50, 12, 0.8);
    ::expectAgreesWithOracle(
        pyr6,
        [&](const R3& p) {
            const double b_z =
                pyr6.baseEdge() - 2 * p.z() / (std::sqrt(3.) * std::tan(pyr6.alpha()));
            if (std::abs(p.x()) > b_z || std::abs(p.y()) > b_z || p.z() < 0
                || p.z() > pyr6.height())
                return false;
            double theta_prime = 0;
            if (p.x() != 0 || p.y() != 0)
                theta_prime = Units::rad2deg(std::asin(
                    std::abs(p.y()) / std::sqrt(std::pow(p.x(), 2) + std::pow(p.y(), 2))));
            const int c = static_cast<int>(theta_prime / 60);
            const double theta = Units::deg2rad(theta_prime - c * 60);
            const double k_z = b_z / (std::cos(theta) + std::sin(theta) / std::tan(pi / 3));
            return std::pow(p.x(), 2) + std::pow(p.y(), 2) <= std::pow(k_z, 2);
        },
        50, 50, 12);

    const Bipyramid4 bipyr(50, 12, 0.8, 0.8);
    ::expectAgreesWithOracle(
        bipyr,
        [&](const R3& p) {
            const double total = bipyr.base_height() * (1 + bipyr.heightRatio());
            if (std::abs(p.x()) > bipyr.length() / 2 || std::abs(p.y()) > bipyr.length() / 2
                || p.z() < 0 || p.z() > total)
                return false;
            const double l_z = bipyr.length() / 2
                               - std::abs(bipyr.base_height() - p.z()) / std::tan(bipyr.alpha());
            return std::abs(p.x()) <= l_z && std::abs(p.y()) <= l_z;
        },
        25, 25, bipyr.base_height() * (1 + bipyr.heightRatio()));

    const PlatonicTetrahedron tetra(40);
    ::expectAgreesWithOracle(tetra, ::tetrahedronOracle(tetra.edge()), tetra.edge() / std::sqrt(3.),
                             tetra.edge() / 2, std::sqrt(2. / 3.) * tetra.edge());

    const PlatonicOctahedron octa(40);
    ::expectAgreesWithOracle(octa, ::octahedronOracle(octa.edge()), octa.edge() / 2,
                             octa.edge() / 2, std::sqrt(2.) * octa.edge());

    const Dodecahedron dodeca(40);
    ::expectAgreesWithOracle(dodeca, ::dodecahedronOracle(dodeca.edge()),
                             1.376381920471174 * dodeca.edge(), 1.309016994374947 * dodeca.edge(),
                             2 * 1.113516364411607 * dodeca.edge());

    const Icosahedron icosa(40);
    ::expectAgreesWithOracle(icosa, ::icosahedronOracle(icosa.edge()),
                             0.9341723589627158 * icosa.edge(), 0.8090169943749473 * icosa.edge(),
                             2 * 0.7557613140761708 * icosa.edge());
}

// The canonical volume() must match the closed-form formula for every polyhedron.
TEST(PolyhedronContains, VolumeMatchesKnownFormula)
{
    struct Case {
        const char* name;
        std::shared_ptr<FormfactorPolyhedron> ff;
        double expected_volume;
    };
    const std::vector<Case> cases = {
        {"Box", std::make_shared<Box>(50, 40, 30), 50 * 40 * 30},
        {"Prism3", std::make_shared<Prism3>(40, 30), ::regularPrismVolume(3, 40, 30)},
        {"Prism6", std::make_shared<Prism6>(40, 30), ::regularPrismVolume(6, 40, 30)},
        {"Pyramid2", std::make_shared<Pyramid2>(50, 40, 12, 0.8),
         ::pyramid2Volume(50, 40, 12, 0.8)},
        {"Pyramid3", std::make_shared<Pyramid3>(50, 8, 0.8), ::pyramid3Volume(50, 8, 0.8)},
        {"Pyramid4", std::make_shared<Pyramid4>(50, 12, 0.8), ::pyramid4Volume(50, 12, 0.8)},
        {"Pyramid6", std::make_shared<Pyramid6>(50, 12, 0.8), ::pyramid6Volume(50, 12, 0.8)},
        {"Bipyramid4", std::make_shared<Bipyramid4>(50, 12, 0.8, 0.8),
         ::bipyramid4Volume(50, 12, 0.8, 0.8)},
        {"CantellatedCube", std::make_shared<CantellatedCube>(40, 10),
         ::cantellatedCubeVolume(40, 10)},
        {"TruncatedCube", std::make_shared<TruncatedCube>(40, 10), ::truncatedCubeVolume(40, 10)},
        {"PlatonicTetrahedron", std::make_shared<PlatonicTetrahedron>(40),
         std::sqrt(2.) * std::pow(40, 3) / 12},
        {"PlatonicOctahedron", std::make_shared<PlatonicOctahedron>(40),
         std::sqrt(2.) * std::pow(40, 3) / 3},
        {"Dodecahedron", std::make_shared<Dodecahedron>(40),
         (15 + 7 * std::sqrt(5.)) * std::pow(40, 3) / 4},
        {"Icosahedron", std::make_shared<Icosahedron>(40),
         5 * (3 + std::sqrt(5.)) * std::pow(40, 3) / 12}};
    for (const Case& c : cases) {
        EXPECT_NEAR(c.ff->volume(), c.expected_volume, 1e-12 * c.expected_volume) << c.name;
    }
}

// The Platonic solids and the cut cubes must give sane verdicts: an on-axis point is inside and a
// far point is outside. The centrosymmetric ones (all but the tetrahedron) additionally
// respect inversion through their center, which sits at mid-height for these shapes.
TEST(PolyhedronContains, PlatonicAndCubeShapesAreSaneAndSymmetric)
{
    const std::vector<std::shared_ptr<FormfactorPolyhedron>> shapes = {
        std::make_shared<PlatonicTetrahedron>(40), std::make_shared<PlatonicOctahedron>(40),
        std::make_shared<Dodecahedron>(40),        std::make_shared<Icosahedron>(40),
        std::make_shared<CantellatedCube>(40, 10), std::make_shared<TruncatedCube>(40, 10)};
    for (const auto& ff : shapes) {
        const double zc = (ff->zBottom(nullptr) + ff->zTop(nullptr)) / 2;
        const double r = ff->radialExtension();
        EXPECT_TRUE(ff->contains(R3(0, 0, zc)));       // on-axis mid-height point is inside
        EXPECT_FALSE(ff->contains(R3(10 * r, 0, zc))); // far point is outside
    }
    // Inversion symmetry about the mid-height center (the tetrahedron is not centrosymmetric).
    const std::vector<std::shared_ptr<FormfactorPolyhedron>> centric = {
        std::make_shared<PlatonicOctahedron>(40), std::make_shared<Dodecahedron>(40),
        std::make_shared<Icosahedron>(40)};
    for (const auto& ff : centric) {
        const R3 center(0, 0, (ff->zBottom(nullptr) + ff->zTop(nullptr)) / 2);
        const double r = ff->radialExtension();
        for (double a : {0.3, 0.6, 0.9})
            for (double b : {0.2, 0.5})
                for (double cc : {-0.4, 0.7}) {
                    const R3 d(a * r, b * r, cc * r);
                    EXPECT_EQ(ff->contains(center + d), ff->contains(center - d));
                }
    }
}
