//
//  QuickHullTests.cpp
//  QuickHull
//
//  Created by Antti Kuukka on 1/5/16.
//  Copyright © 2016 Antti Kuukka. All rights reserved.
//

#include "WCPQuickhull/QuickHull.h"
#include "WCPQuickhull/MathUtils.h"
#include <iostream>
#include <random>
#include <cassert>
#include <chrono>

namespace quickhull {

    namespace tests {

        using FloatType = float;
        using vec3 = Vector3<FloatType>;

        // Setup RNG
        static std::mt19937 rng;
        static std::uniform_real_distribution<> dist(0, 1);

        FloatType rnd(FloatType from, FloatType to) { return from + (FloatType) dist(rng) * (to - from); };

        void assertSameValue(FloatType a, FloatType b) { assert(std::abs(a - b) < 0.0001f); }

        void testVector3()
        {
            typedef Vector3<FloatType> vec3;
            vec3 a(1, 0, 0);
            vec3 b(1, 0, 0);

            vec3 c = a.projection(b);
            assert((c - a).getLength() < 0.00001f);

            a = vec3(1, 1, 0);
            b = vec3(1, 3, 0);
            c = b.projection(a);
            assert((c - vec3(2, 2, 0)).getLength() < 0.00001f);
        }

        template <typename T>
        std::vector<Vector3<T>> createSphere(T radius, size_t M, Vector3<T> offset = Vector3<T>(0, 0, 0))
        {
            std::vector<Vector3<T>> pc;
            const T pi = 3.14159f;
            for (int i = 0; i <= M; i++) {
                FloatType y = std::sin(pi / 2 + (FloatType) i / (M) *pi);
                FloatType r = std::cos(pi / 2 + (FloatType) i / (M) *pi);
                FloatType K = FloatType(1) - std::abs((FloatType)((FloatType) i - M / 2.0f)) / (FloatType)(M / 2.0f);
                const size_t pcount = (size_t)(1 + K * M + FloatType(1) / 2);
                for (size_t j = 0; j < pcount; j++) {
                    FloatType x = pcount > 1 ? r * std::cos((FloatType) j / pcount * pi * 2) : 0;
                    FloatType z = pcount > 1 ? r * std::sin((FloatType) j / pcount * pi * 2) : 0;
                    pc.emplace_back(x + offset.x, y + offset.y, z + offset.z);
                }
            }
            return pc;
        }

        void sphereTest()
        {
            QuickHull<FloatType> qh;
            FloatType y = 1;
            for (;;) {
                auto pc = createSphere<FloatType>(1, 100, Vector3<FloatType>(0, y, 0));
                auto hull = qh.getConvexHull(pc, true, false);
                y *= 15;
                y /= 10;
                if (hull.getVertexBuffer().size() == 4) {
                    break;
                }
            }

            // Test worst case scenario: more and more points on the unit sphere. All points should be part of the
            // convex hull, as long as we can make epsilon smaller without running out of numerical accuracy.
            size_t i = 1;
            FloatType eps = 0.002f;
            for (;;) {
                auto pc = createSphere<FloatType>(1, i, Vector3<FloatType>(0, 0, 0));
                auto hull = qh.getConvexHull(pc, true, false, eps);
                std::cout << i << ":" << pc.size() << " : " << hull.getVertexBuffer().size() << " at eps=" << eps
                          << std::endl;
                if (qh.getDiagnostics().m_failedHorizonEdges) {
                    // This should not happen
                    assert(false);
                    break;
                }
                if (pc.size() == hull.getVertexBuffer().size()) {
                    // Fine, all the points on unit sphere do belong to the convex mesh.
                    i += 1;
                }
                else {
                    eps *= 0.5f;
                    std::cout << "Epsilon to " << eps << std::endl;
                }

                if (i == 500) {
                    break;
                }
            }
        }

        void testPlanarCase()
        {
            QuickHull<FloatType> qh;
            std::vector<vec3> pc;
            pc.emplace_back(-3.000000f, -0.250000f, -0.800000f);
            pc.emplace_back(-3.000000f, 0.250000f, -0.800000f);
            pc.emplace_back(-3.125000f, 0.250000f, -0.750000);
            pc.emplace_back(-3.125000f, -0.250000f, -0.750000);
            auto hull = qh.getConvexHull(pc, true, false);
            assert(hull.getIndexBuffer().size() == 12);
            assert(hull.getVertexBuffer().size() == 4);
        }

        void testHalfEdgeOutput()
        {
            QuickHull<FloatType> qh;

            // 8 corner vertices of a cube + tons of vertices inside. Output should be a half edge mesh with 12 faces (6
            // cube faces with 2 triangles per face) and 36 half edges (3 half edges per face).
            std::vector<vec3> pc;
            for (int h = 0; h < 1000; h++) {
                pc.emplace_back(rnd(-1, 1), rnd(-1, 1), rnd(-1, 1));
            }
            for (int h = 0; h < 8; h++) {
                pc.emplace_back(h & 1 ? -2 : 2, h & 2 ? -2 : 2, h & 4 ? -2 : 2);
            }
            HalfEdgeMesh<FloatType, size_t> mesh = qh.getConvexHullAsMesh(&pc[0].x, pc.size(), true);
            assert(mesh.m_faces.size() == 12);
            assert(mesh.m_halfEdges.size() == 36);
            assert(mesh.m_vertices.size() == 8);
        }

        void testPlanes()
        {
            Vector3<FloatType> N(1, 0, 0);
            Vector3<FloatType> p(2, 0, 0);
            Plane<FloatType> P(N, p);
            auto dist = mathutils::getSignedDistanceToPlane(Vector3<FloatType>(3, 0, 0), P);
            assertSameValue(dist, 1);
            dist = mathutils::getSignedDistanceToPlane(Vector3<FloatType>(1, 0, 0), P);
            assertSameValue(dist, -1);
            dist = mathutils::getSignedDistanceToPlane(Vector3<FloatType>(1, 0, 0), P);
            assertSameValue(dist, -1);
            N = Vector3<FloatType>(2, 0, 0);
            P = Plane<FloatType>(N, p);
            dist = mathutils::getSignedDistanceToPlane(Vector3<FloatType>(6, 0, 0), P);
            assertSameValue(dist, 8);
        }

        void run()
        {
            // Setup test env
            const size_t N = 200;
            std::vector<vec3> pc;
            QuickHull<FloatType> qh;
            ConvexHull<FloatType> hull;

            // Seed RNG using Unix time
            std::chrono::system_clock::time_point now = std::chrono::system_clock::now();
            auto seed = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();
            rng.seed((unsigned int) seed);

            // Test 1 : Put N points inside unit cube. Result mesh must have exactly 8 vertices because the convex hull
            // is the unit cube.
            pc.clear();
            for (int i = 0; i < 8; i++) {
                pc.emplace_back(i & 1 ? -1 : 1, i & 2 ? -1 : 1, i & 4 ? -1 : 1);
            }
            for (size_t i = 0; i < N; i++) {
                pc.emplace_back(rnd(-1, 1), rnd(-1, 1), rnd(-1, 1));
            }
            hull = qh.getConvexHull(pc, true, false);
            assert(hull.getVertexBuffer().size() == 8);
            assert(hull.getIndexBuffer().size() ==
                   3 * 2 * 6);  // 6 cube faces, 2 triangles per face, 3 indices per triangle
            assert(&(hull.getVertexBuffer()[0]) != &(pc[0]));
            auto hull2 = hull;
            assert(hull2.getVertexBuffer().size() == hull.getVertexBuffer().size());
            assert(hull2.getVertexBuffer()[0].x == hull.getVertexBuffer()[0].x);
            assert(hull2.getIndexBuffer().size() == hull.getIndexBuffer().size());
            auto hull3 = std::move(hull);
            assert(hull.getIndexBuffer().size() == 0);

            // Test 1.1 : Same test, but using the original indices.
            hull = qh.getConvexHull(pc, true, true);
            assert(hull.getIndexBuffer().size() == 3 * 2 * 6);
            assert(hull.getVertexBuffer().size() == pc.size());
            assert(&(hull.getVertexBuffer()[0]) == &(pc[0]));

            // Test 2 : random N points from the boundary of unit sphere. Result mesh must have exactly N points.
            pc = createSphere<FloatType>(1, 50);
            hull = qh.getConvexHull(pc, true, false);
            assert(pc.size() == hull.getVertexBuffer().size());
            hull = qh.getConvexHull(pc, true, true);
            // Add every vertex twice. This should not affect final mesh
            auto s = pc.size();
            for (size_t i = 0; i < s; i++) {
                const auto& p = pc[i];
                pc.push_back(p);
            }
            hull = qh.getConvexHull(pc, true, false);
            assert(pc.size() / 2 == hull.getVertexBuffer().size());

            // Test 2.1 : Multiply x components of the unit sphere vectors by a huge number => essentially we get a line
            const FloatType mul = 2 * 2 * 2;
            while (true) {
                for (auto& p : pc) {
                    p.x *= mul;
                }
                hull = qh.getConvexHull(pc, true, false);
                if (hull.getVertexBuffer().size() == 4) {
                    break;
                }
            }

            // Test 3: 0D
            pc.clear();
            vec3 centerPoint(2, 2, 2);
            pc.push_back(centerPoint);
            for (size_t i = 0; i < 100; i++) {
                auto newp = centerPoint +
                            vec3(rnd(-0.000001f, 0.000001f), rnd(-0.000001f, 0.000001f), rnd(-0.000001f, 0.000001f));
                pc.push_back(newp);
            }
            hull = qh.getConvexHull(pc, true, false);
            assert(hull.getIndexBuffer().size() == 12);

            // Test 4: 2d degenerate case
            testPlanarCase();

            // Test 5: first a planar circle, then make a cylinder out of it
            pc.clear();
            for (size_t i = 0; i < N; i++) {
                const FloatType alpha = (FloatType) i / N * 2 * 3.14159f;
                pc.emplace_back(std::cos(alpha), 0, std::sin(alpha));
            }
            hull = qh.getConvexHull(pc, true, false);

            assert(hull.getVertexBuffer().size() == pc.size());
            for (size_t i = 0; i < N; i++) {
                pc.push_back(pc[i] + vec3(0, 1, 0));
            }
            hull = qh.getConvexHull(pc, true, false);
            assert(hull.getVertexBuffer().size() == pc.size());
            assert(hull.getIndexBuffer().size() / 3 == pc.size() * 2 - 4);

            // Test 6
            for (int x = 0;; x++) {
                pc.clear();
                const FloatType l = 1;
                const FloatType r = l / (std::pow(10, x));
                for (size_t i = 0; i < N; i++) {
                    vec3 p = vec3(1, 0, 0) * i * l / (N - 1);
                    FloatType a = rnd(0, 2 * 3.1415f);
                    vec3 d = vec3(0, std::sin(a), std::cos(a)) * r;
                    pc.push_back(p + d);
                }
                hull = qh.getConvexHull(pc, true, false);
                if (hull.getVertexBuffer().size() == 4) {
                    break;
                }
            }

            // Test 7
            for (int h = 0; h < 100; h++) {
                pc.clear();
                const vec3 v1(rnd(-1, 1), rnd(-1, 1), rnd(-1, 1));
                const vec3 v2(rnd(-1, 1), rnd(-1, 1), rnd(-1, 1));
                pc.push_back(v1);
                pc.push_back(v2);
                for (int i = 0; i < N; i++) {
                    auto t1 = rnd(0, 1);
                    auto t2 = rnd(0, 1);
                    pc.push_back(t1 * v1 + t2 * v2);
                }
                hull = qh.getConvexHull(pc, true, false);
            }

            // Other tests
            testPlanes();
            sphereTest();
            testVector3();
            testHalfEdgeOutput();
            std::cout << "QuickHull tests succesfully passed." << std::endl;
        }

    }  // namespace tests
}  // namespace quickhull

int main()
{
    quickhull::tests::run();
    return 0;
}
