//
// SPDX-License-Identifier: BSD-3-Clause
// Copyright Contributors to the OpenEXR Project.
//

#ifdef NDEBUG
#    undef NDEBUG
#endif

#include "testLineAlgo.h"
#include <ImathLineAlgo.h>
#include <ImathRandom.h>
#include <assert.h>
#include <iostream>

// Include ImathForward *after* other headers to validate forward declarations
#include <ImathForward.h>

using namespace std;
using namespace IMATH_INTERNAL_NAMESPACE;

namespace
{

void
testClosestPoints (
    const Line3f& line1,
    const Line3f& line2,
    bool          returnValue,
    const V3f&    point1,
    const V3f&    point2)
{
    V3f  p1;
    V3f  p2;
    bool rv = closestPoints (line1, line2, p1, p2);

    assert (rv == returnValue);

    if (rv)
    {
        float e = 10 * std::numeric_limits<float>::epsilon ();
        assert (point1.equalWithAbsError (p1, e));
        assert (point2.equalWithAbsError (p2, e));
    }
}

void
testClosestPoints ()
{
    cout << "closest points on two lines" << endl;

    cout << "  non-intersecting, non-parallel lines" << endl;

    testClosestPoints (
        Line3f (V3f (0, -1, -1), V3f (0, 1, -1)),
        Line3f (V3f (-1, 0, 1), V3f (1, 0, 1)),
        true,
        V3f (0, 0, -1),
        V3f (0, 0, 1));

    testClosestPoints (
        Line3f (V3f (2, -1, -1), V3f (2, 1, -1)),
        Line3f (V3f (-1, 3, 1), V3f (1, 3, 1)),
        true,
        V3f (2, 3, -1),
        V3f (2, 3, 1));

    cout << "  intersecting, non-parallel lines" << endl;

    testClosestPoints (
        Line3f (V3f (2, -1, 0), V3f (2, 1, 0)),
        Line3f (V3f (-1, 3, 0), V3f (1, 3, 0)),
        true,
        V3f (2, 3, 0),
        V3f (2, 3, 0));

    cout << "  parallel lines" << endl;

    testClosestPoints (
        Line3f (V3f (2, -1, 0), V3f (2, 1, 0)),
        Line3f (V3f (2, -1, 1), V3f (2, 1, 1)),
        false,
        V3f (0, 0, 0),
        V3f (0, 0, 0));

    testClosestPoints (
        Line3f (V3f (2, -1, 0), V3f (2, 1, 0)),
        Line3f (V3f (2, 1, 1), V3f (2, -1, 1)),
        false,
        V3f (0, 0, 0),
        V3f (0, 0, 0));

    cout << "  coincident lines" << endl;

    testClosestPoints (
        Line3f (V3f (2, -1, 0), V3f (2, -1, 1)),
        Line3f (V3f (2, -1, 0), V3f (2, -1, 1)),
        false,
        V3f (0, 0, 0),
        V3f (0, 0, 0));

    cout << "  random lines" << endl;

    Rand48 rand (7);

    for (int i = 0; i < 10000; ++i)
    {
        Line3f line1 (
            solidSphereRand<V3f> (rand) * 100.f,
            solidSphereRand<V3f> (rand) * 100.f);

        Line3f line2 (
            solidSphereRand<V3f> (rand) * 100.f,
            solidSphereRand<V3f> (rand) * 100.f);

        V3f  point1;
        V3f  point2;
        bool rv = closestPoints (line1, line2, point1, point2);

        if (rv)
        {
            //
            // We test if the line that connects point1 and point2
            // is perpendicular to line1 and line2.  The numerical
            // accuracy of point1 and point2 depends strongly on
            // the relative directions of line1 and line2; accuracy
            // degrades rather quickly if line1 and line2 become
            // close to parallel.
            //

            float e = 2000 * std::numeric_limits<float>::epsilon ();
            float d = 1 - (line1.dir ^ line2.dir) * (line1.dir ^ line2.dir);
            V3f   n = point1 - point2;

            assert (equalWithAbsError (0.0f, (line1.dir ^ n) * d, e));
            assert (equalWithAbsError (0.0f, (line2.dir ^ n) * d, e));
        }
    }
}

void
testIntersect (
    const Line3f& line,
    const V3f&    v0,
    const V3f&    v1,
    const V3f&    v2,
    const V3f&    point,
    bool          front,
    bool          returnValue)
{
    V3f  pt;
    V3f  bary;
    bool fr;

    bool rv = intersect (line, v0, v1, v2, pt, bary, fr);

    assert (rv == returnValue);

    float e = 10 * std::numeric_limits<float>::epsilon ();

    if (rv)
    {
        assert (front == fr);
        assert (pt.equalWithAbsError (point, e));
        V3f pt2 = v0 * bary.x + v1 * bary.y + v2 * bary.z;
        assert (pt.equalWithAbsError (pt2, e));
    }
}

void
testIntersect ()
{
    cout << "line-triangle intersection" << endl;

    cout << "  line-plane intersection inside triangle" << endl;

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (0, 0, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 7),
        true,
        true);

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (-1, -2, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (-1, -2, 7),
        true,
        true);

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (-1, 1, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (-1, 1, 7),
        true,
        true);

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (-1, 1, 7)),
        V3f (4, -4, 7),
        V3f (-4, -4, 7),
        V3f (0, 6, 7),
        V3f (-1, 1, 7),
        false,
        true);

    testIntersect (
        Line3f (V3f (1, 1, 2), V3f (0, 0, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 7),
        true,
        true);

    testIntersect (
        Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (-1, -2, 7),
        true,
        true);

    testIntersect (
        Line3f (V3f (2, 8, -10), V3f (-1, 1, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (-1, 1, 7),
        true,
        true);

    testIntersect (
        Line3f (V3f (-10, 2, -1), V3f (-1, 1, 7)),
        V3f (4, -4, 7),
        V3f (-4, -4, 7),
        V3f (0, 6, 7),
        V3f (-1, 1, 7),
        false,
        true);

    cout << "  line-plane intersection outside triangle" << endl;

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (4, 0, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (-4, 1, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (0, -5, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (0, -7, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    cout << "  line parallel to triangle" << endl;

    testIntersect (
        Line3f (V3f (0, 0, -1), V3f (4, 0, -1)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    testIntersect (
        Line3f (V3f (0, 4, 7), V3f (4, 0, 7)),
        V3f (-4, -4, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    cout << "  zero-area triangle" << endl;

    testIntersect (
        Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),
        V3f (0, 6, 7),
        V3f (4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    testIntersect (
        Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),
        V3f (-4, -4, 7),
        V3f (-4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    testIntersect (
        Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),
        V3f (-4, -4, 7),
        V3f (0, 6, 7),
        V3f (0, 6, 7),
        V3f (0, 0, 0),
        false,
        false);

    testIntersect (
        Line3f (V3f (2, 3, -5), V3f (-1, -2, 7)),
        V3f (-4, -4, 7),
        V3f (-4, -4, 7),
        V3f (-4, -4, 7),
        V3f (0, 0, 0),
        false,
        false);

    cout << "  random lines and triangles" << endl;

    Rand48 rand (8);

    for (int i = 0; i < 10000; ++i)
    {
        //
        // Generate a random triangle with non-zero area
        //

        V3f v0, v1, v2;
        V3f normal;

        do
        {
            v0     = solidSphereRand<V3f> (rand);
            v1     = solidSphereRand<V3f> (rand);
            v2     = solidSphereRand<V3f> (rand);
            normal = (v2 - v1) % (v1 - v0);
        } while (normal.length () < 0.01);

        {
            //
            // Generate a line that intersects inside the triangle
            //

            V3f b;

            do
            {
                b.x = float(rand.nextf (0.001, 0.999));
                b.y = float(rand.nextf (0.001, 0.999));
                b.z = 1 - b.x - b.y;
            } while (b.x + b.y > 0.999);

            V3f p1 = v0 * b.x + v1 * b.y + v2 * b.z;

            V3f p0;

            do
            {
                p0 = solidSphereRand<V3f> (rand);
            } while (abs (normal.normalized () ^ (p1 - p0).normalized ()) <
                     0.1);

            //
            // Test for intersection
            //

            V3f  point;
            V3f  bary;
            bool front;

            bool rv =
                intersect (Line3f (p0, p1), v0, v1, v2, point, bary, front);

            assert (rv == true);

            float nd = abs (normal.normalized () ^ (p1 - p0).normalized ());
            float ep = 20 * std::numeric_limits<float>::epsilon () / nd;

            assert (point.equalWithAbsError (p1, ep));
        }

        {
            //
            // Generate a line that intersects the triangle's plane
            // but outside the triangle
            //

            V3f b;

            do
            {
                b.x = float(rand.nextf (-3, 3));
                b.y = float(rand.nextf (-3, 3));
                b.z = 1 - b.x - b.y;
            } while (b.x > -0.001 && b.y > -0.001 && b.z > -0.001);

            V3f p1 = v0 * b.x + v1 * b.y + v2 * b.z;

            V3f p0;

            do
            {
                p0 = solidSphereRand<V3f> (rand) * 10;
            } while (abs (normal.normalized () ^ (p1 - p0).normalized ()) <
                     0.1);

            //
            // Test for intersection
            //

            V3f  point;
            V3f  bary;
            bool front;

            bool rv =
                intersect (Line3f (p0, p1), v0, v1, v2, point, bary, front);

            assert (rv == false);
        }
    }
}

} // namespace

void
testLineAlgo ()
{
    cout << "Testing line algorithms" << endl;

    testClosestPoints ();
    testIntersect ();

    cout << "ok\n" << endl;
}
