/**
 * \file
 * \author Thomas Fischer
 * \date   2010-03-17
 * \brief  Implementation of analytical geometry functions.
 *
 * \copyright
 * Copyright (c) 2012-2023, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include "AnalyticalGeometry.h"

#include <Eigen/Geometry>
#include <algorithm>
#include <cmath>
#include <limits>

#include "BaseLib/StringTools.h"
#include "MathLib/GeometricBasics.h"
#include "PointVec.h"
#include "Polyline.h"

extern double orient2d(double*, double*, double*);
extern double orient2dfast(double*, double*, double*);

namespace ExactPredicates
{
double getOrientation2d(MathLib::Point3d const& a, MathLib::Point3d const& b,
                        MathLib::Point3d const& c)
{
    return orient2d(const_cast<double*>(a.data()),
                    const_cast<double*>(b.data()),
                    const_cast<double*>(c.data()));
}

double getOrientation2dFast(MathLib::Point3d const& a,
                            MathLib::Point3d const& b,
                            MathLib::Point3d const& c)
{
    return orient2dfast(const_cast<double*>(a.data()),
                        const_cast<double*>(b.data()),
                        const_cast<double*>(c.data()));
}
}  // namespace ExactPredicates

namespace GeoLib
{
Orientation getOrientation(MathLib::Point3d const& p0,
                           MathLib::Point3d const& p1,
                           MathLib::Point3d const& p2)
{
    double const orientation = ExactPredicates::getOrientation2d(p0, p1, p2);
    if (orientation > 0)
    {
        return CCW;
    }
    if (orientation < 0)
    {
        return CW;
    }
    return COLLINEAR;
}

Orientation getOrientationFast(MathLib::Point3d const& p0,
                               MathLib::Point3d const& p1,
                               MathLib::Point3d const& p2)
{
    double const orientation =
        ExactPredicates::getOrientation2dFast(p0, p1, p2);
    if (orientation > 0)
    {
        return CCW;
    }
    if (orientation < 0)
    {
        return CW;
    }
    return COLLINEAR;
}

bool parallel(Eigen::Vector3d v, Eigen::Vector3d w)
{
    const double eps(std::numeric_limits<double>::epsilon());
    double const eps_squared = eps * eps;

    // check degenerated cases
    if (v.squaredNorm() < eps_squared)
    {
        return false;
    }

    if (w.squaredNorm() < eps_squared)
    {
        return false;
    }

    v.normalize();
    w.normalize();

    bool parallel(true);
    if (std::abs(v[0] - w[0]) > eps)
    {
        parallel = false;
    }
    if (std::abs(v[1] - w[1]) > eps)
    {
        parallel = false;
    }
    if (std::abs(v[2] - w[2]) > eps)
    {
        parallel = false;
    }

    if (!parallel)
    {
        parallel = true;
        // change sense of direction of v_normalised
        v *= -1.0;
        // check again
        if (std::abs(v[0] - w[0]) > eps)
        {
            parallel = false;
        }
        if (std::abs(v[1] - w[1]) > eps)
        {
            parallel = false;
        }
        if (std::abs(v[2] - w[2]) > eps)
        {
            parallel = false;
        }
    }

    return parallel;
}

bool lineSegmentIntersect(GeoLib::LineSegment const& s0,
                          GeoLib::LineSegment const& s1,
                          GeoLib::Point& s)
{
    GeoLib::Point const& pa{s0.getBeginPoint()};
    GeoLib::Point const& pb{s0.getEndPoint()};
    GeoLib::Point const& pc{s1.getBeginPoint()};
    GeoLib::Point const& pd{s1.getEndPoint()};

    if (!isCoplanar(pa, pb, pc, pd))
    {
        return false;
    }

    auto const& a = s0.getBeginPoint().asEigenVector3d();
    auto const& b = s0.getEndPoint().asEigenVector3d();
    auto const& c = s1.getBeginPoint().asEigenVector3d();
    auto const& d = s1.getEndPoint().asEigenVector3d();

    Eigen::Vector3d const v = b - a;
    Eigen::Vector3d const w = d - c;
    Eigen::Vector3d const qp = c - a;
    Eigen::Vector3d const pq = a - c;

    double const eps = std::numeric_limits<double>::epsilon();
    double const squared_eps = eps * eps;
    // handle special cases here to avoid computing intersection numerical
    if (qp.squaredNorm() < squared_eps || (d - a).squaredNorm() < squared_eps)
    {
        s = pa;
        return true;
    }
    if ((c - b).squaredNorm() < squared_eps ||
        (d - b).squaredNorm() < squared_eps)
    {
        s = pb;
        return true;
    }

    auto isLineSegmentIntersectingAB =
        [&v](Eigen::Vector3d const& ap, std::size_t i)
    {
        // check if p is located at v=(a,b): (ap = t*v, t in [0,1])
        return 0.0 <= ap[i] / v[i] && ap[i] / v[i] <= 1.0;
    };

    if (parallel(v, w))
    {  // original line segments (a,b) and (c,d) are parallel
        if (parallel(pq, v))
        {  // line segment (a,b) and (a,c) are also parallel
            // Here it is already checked that the line segments (a,b) and (c,d)
            // are parallel. At this point it is also known that the line
            // segment (a,c) is also parallel to (a,b). In that case it is
            // possible to express c as c(t) = a + t * (b-a) (analog for the
            // point d). Since the evaluation of all three coordinate equations
            // (x,y,z) have to lead to the same solution for the parameter t it
            // is sufficient to evaluate t only once.

            // Search id of coordinate with largest absolute value which is will
            // be used in the subsequent computations. This prevents division by
            // zero in case the line segments are parallel to one of the
            // coordinate axis.
            std::size_t i_max(std::abs(v[0]) <= std::abs(v[1]) ? 1 : 0);
            i_max = std::abs(v[i_max]) <= std::abs(v[2]) ? 2 : i_max;
            if (isLineSegmentIntersectingAB(qp, i_max))
            {
                s = pc;
                return true;
            }
            Eigen::Vector3d const ad = d - a;
            if (isLineSegmentIntersectingAB(ad, i_max))
            {
                s = pd;
                return true;
            }
            return false;
        }
        return false;
    }

    // general case
    const double sqr_len_v(v.squaredNorm());
    const double sqr_len_w(w.squaredNorm());

    Eigen::Matrix2d mat;
    mat(0, 0) = sqr_len_v;
    mat(0, 1) = -v.dot(w);
    mat(1, 1) = sqr_len_w;
    mat(1, 0) = mat(0, 1);

    Eigen::Vector2d rhs{v.dot(qp), w.dot(pq)};

    rhs = mat.partialPivLu().solve(rhs);

    // no theory for the following tolerances, determined by testing
    // lower tolerance: little bit smaller than zero
    const double l(-1.0 * std::numeric_limits<float>::epsilon());
    // upper tolerance a little bit greater than one
    const double u(1.0 + std::numeric_limits<float>::epsilon());
    if (rhs[0] < l || u < rhs[0] || rhs[1] < l || u < rhs[1])
    {
        return false;
    }

    // compute points along line segments with minimal distance
    GeoLib::Point const p0(a[0] + rhs[0] * v[0], a[1] + rhs[0] * v[1],
                           a[2] + rhs[0] * v[2]);
    GeoLib::Point const p1(c[0] + rhs[1] * w[0], c[1] + rhs[1] * w[1],
                           c[2] + rhs[1] * w[2]);

    double const min_dist(std::sqrt(MathLib::sqrDist(p0, p1)));
    double const min_seg_len(
        std::min(std::sqrt(sqr_len_v), std::sqrt(sqr_len_w)));
    if (min_dist < min_seg_len * 1e-6)
    {
        s[0] = 0.5 * (p0[0] + p1[0]);
        s[1] = 0.5 * (p0[1] + p1[1]);
        s[2] = 0.5 * (p0[2] + p1[2]);
        return true;
    }

    return false;
}

bool lineSegmentsIntersect(const GeoLib::Polyline* ply,
                           GeoLib::Polyline::SegmentIterator& seg_it0,
                           GeoLib::Polyline::SegmentIterator& seg_it1,
                           GeoLib::Point& intersection_pnt)
{
    std::size_t const n_segs(ply->getNumberOfSegments());
    // Neighbouring segments always intersects at a common vertex. The algorithm
    // checks for intersections of non-neighbouring segments.
    for (seg_it0 = ply->begin(); seg_it0 != ply->end() - 2; ++seg_it0)
    {
        seg_it1 = seg_it0 + 2;
        std::size_t const seg_num_0 = seg_it0.getSegmentNumber();
        for (; seg_it1 != ply->end(); ++seg_it1)
        {
            // Do not check first and last segment, because they are
            // neighboured.
            if (!(seg_num_0 == 0 && seg_it1.getSegmentNumber() == n_segs - 1))
            {
                if (lineSegmentIntersect(*seg_it0, *seg_it1, intersection_pnt))
                {
                    return true;
                }
            }
        }
    }
    return false;
}

void rotatePoints(Eigen::Matrix3d const& rot_mat,
                  std::vector<GeoLib::Point*>& pnts)
{
    rotatePoints(rot_mat, pnts.begin(), pnts.end());
}

Eigen::Matrix3d computeRotationMatrixToXY(Eigen::Vector3d const& n)
{
    Eigen::Matrix3d rot_mat = Eigen::Matrix3d::Zero();
    // check if normal points already in the right direction
    if (n[0] == 0 && n[1] == 0)
    {
        rot_mat(1, 1) = 1.0;

        if (n[2] > 0)
        {
            // identity matrix
            rot_mat(0, 0) = 1.0;
            rot_mat(2, 2) = 1.0;
        }
        else
        {
            // rotate by pi about the y-axis
            rot_mat(0, 0) = -1.0;
            rot_mat(2, 2) = -1.0;
        }

        return rot_mat;
    }

    // sqrt (n_1^2 + n_3^2)
    double const h0(std::sqrt(n[0] * n[0] + n[2] * n[2]));

    // In case the x and z components of the normal are both zero the rotation
    // to the x-z-plane is not required, i.e. only the rotation in the z-axis is
    // required. The angle is either pi/2 or 3/2*pi. Thus the components of
    // rot_mat are as follows.
    if (h0 < std::numeric_limits<double>::epsilon())
    {
        rot_mat(0, 0) = 1.0;
        if (n[1] > 0)
        {
            rot_mat(1, 2) = -1.0;
            rot_mat(2, 1) = 1.0;
        }
        else
        {
            rot_mat(1, 2) = 1.0;
            rot_mat(2, 1) = -1.0;
        }
        return rot_mat;
    }

    double const h1(1 / n.norm());

    // general case: calculate entries of rotation matrix
    rot_mat(0, 0) = n[2] / h0;
    rot_mat(0, 1) = 0;
    rot_mat(0, 2) = -n[0] / h0;
    rot_mat(1, 0) = -n[1] * n[0] / h0 * h1;
    rot_mat(1, 1) = h0 * h1;
    rot_mat(1, 2) = -n[1] * n[2] / h0 * h1;
    rot_mat(2, 0) = n[0] * h1;
    rot_mat(2, 1) = n[1] * h1;
    rot_mat(2, 2) = n[2] * h1;

    return rot_mat;
}

Eigen::Matrix3d rotatePointsToXY(std::vector<GeoLib::Point*>& pnts)
{
    return rotatePointsToXY(pnts.begin(), pnts.end(), pnts.begin(), pnts.end());
}

std::unique_ptr<GeoLib::Point> triangleLineIntersection(
    MathLib::Point3d const& a, MathLib::Point3d const& b,
    MathLib::Point3d const& c, MathLib::Point3d const& p,
    MathLib::Point3d const& q)
{
    Eigen::Vector3d const pq = q.asEigenVector3d() - p.asEigenVector3d();
    Eigen::Vector3d const pa = a.asEigenVector3d() - p.asEigenVector3d();
    Eigen::Vector3d const pb = b.asEigenVector3d() - p.asEigenVector3d();
    Eigen::Vector3d const pc = c.asEigenVector3d() - p.asEigenVector3d();

    double u = pq.cross(pc).dot(pb);
    if (u < 0)
    {
        return nullptr;
    }
    double v = pq.cross(pa).dot(pc);
    if (v < 0)
    {
        return nullptr;
    }
    double w = pq.cross(pb).dot(pa);
    if (w < 0)
    {
        return nullptr;
    }

    const double denom(1.0 / (u + v + w));
    u *= denom;
    v *= denom;
    w *= denom;
    return std::make_unique<GeoLib::Point>(u * a[0] + v * b[0] + w * c[0],
                                           u * a[1] + v * b[1] + w * c[1],
                                           u * a[2] + v * b[2] + w * c[2]);
}

void computeAndInsertAllIntersectionPoints(GeoLib::PointVec& pnt_vec,
                                           std::vector<GeoLib::Polyline*>& plys)
{
    auto computeSegmentIntersections =
        [&pnt_vec](GeoLib::Polyline& poly0, GeoLib::Polyline& poly1)
    {
        for (auto seg0_it(poly0.begin()); seg0_it != poly0.end(); ++seg0_it)
        {
            for (auto seg1_it(poly1.begin()); seg1_it != poly1.end(); ++seg1_it)
            {
                GeoLib::Point s(0.0, 0.0, 0.0, pnt_vec.size());
                if (lineSegmentIntersect(*seg0_it, *seg1_it, s))
                {
                    std::size_t const id(
                        pnt_vec.push_back(new GeoLib::Point(s)));
                    poly0.insertPoint(seg0_it.getSegmentNumber() + 1, id);
                    poly1.insertPoint(seg1_it.getSegmentNumber() + 1, id);
                }
            }
        }
    };

    for (auto it0(plys.begin()); it0 != plys.end(); ++it0)
    {
        auto it1(it0);
        ++it1;
        for (; it1 != plys.end(); ++it1)
        {
            computeSegmentIntersections(*(*it0), *(*it1));
        }
    }
}

std::tuple<std::vector<GeoLib::Point*>, Eigen::Vector3d>
rotatePolygonPointsToXY(GeoLib::Polygon const& polygon_in)
{
    // 1 copy all points
    std::vector<GeoLib::Point*> polygon_points;
    polygon_points.reserve(polygon_in.getNumberOfPoints());
    for (std::size_t k(0); k < polygon_in.getNumberOfPoints(); k++)
    {
        polygon_points.push_back(new GeoLib::Point(*(polygon_in.getPoint(k))));
    }

    // 2 rotate points
    auto [plane_normal, d_polygon] = GeoLib::getNewellPlane(polygon_points);
    Eigen::Matrix3d const rot_mat =
        GeoLib::computeRotationMatrixToXY(plane_normal);
    GeoLib::rotatePoints(rot_mat, polygon_points);

    // 3 set z coord to zero
    std::for_each(polygon_points.begin(), polygon_points.end(),
                  [](GeoLib::Point* p) { (*p)[2] = 0.0; });

    return {polygon_points, plane_normal};
}

std::vector<MathLib::Point3d> lineSegmentIntersect2d(
    GeoLib::LineSegment const& ab, GeoLib::LineSegment const& cd)
{
    GeoLib::Point const& a{ab.getBeginPoint()};
    GeoLib::Point const& b{ab.getEndPoint()};
    GeoLib::Point const& c{cd.getBeginPoint()};
    GeoLib::Point const& d{cd.getEndPoint()};

    double const orient_abc(getOrientation(a, b, c));
    double const orient_abd(getOrientation(a, b, d));

    // check if the segment (cd) lies on the left or on the right of (ab)
    if ((orient_abc > 0 && orient_abd > 0) ||
        (orient_abc < 0 && orient_abd < 0))
    {
        return std::vector<MathLib::Point3d>();
    }

    // check: (cd) and (ab) are on the same line
    if (orient_abc == 0.0 && orient_abd == 0.0)
    {
        double const eps(std::numeric_limits<double>::epsilon());
        if (MathLib::sqrDist2d(a, c) < eps && MathLib::sqrDist2d(b, d) < eps)
        {
            return {{a, b}};
        }
        if (MathLib::sqrDist2d(a, d) < eps && MathLib::sqrDist2d(b, c) < eps)
        {
            return {{a, b}};
        }

        // Since orient_ab and orient_abd vanish, a, b, c, d are on the same
        // line and for this reason it is enough to check the x-component.
        auto isPointOnSegment = [](double q, double p0, double p1)
        {
            double const t((q - p0) / (p1 - p0));
            return 0 <= t && t <= 1;
        };

        // check if c in (ab)
        if (isPointOnSegment(c[0], a[0], b[0]))
        {
            // check if a in (cd)
            if (isPointOnSegment(a[0], c[0], d[0]))
            {
                return {{a, c}};
            }
            // check b == c
            if (MathLib::sqrDist2d(b, c) < eps)
            {
                return {{b}};
            }
            // check if b in (cd)
            if (isPointOnSegment(b[0], c[0], d[0]))
            {
                return {{b, c}};
            }
            // check d in (ab)
            if (isPointOnSegment(d[0], a[0], b[0]))
            {
                return {{c, d}};
            }
            std::stringstream err;
            err.precision(std::numeric_limits<double>::digits10);
            err << ab << " x " << cd;
            OGS_FATAL(
                "The case of parallel line segments ({:s}) is not handled yet. "
                "Aborting.",
                err.str());
        }

        // check if d in (ab)
        if (isPointOnSegment(d[0], a[0], b[0]))
        {
            // check if a in (cd)
            if (isPointOnSegment(a[0], c[0], d[0]))
            {
                return {{a, d}};
            }
            // check if b==d
            if (MathLib::sqrDist2d(b, d) < eps)
            {
                return {{b}};
            }
            // check if b in (cd)
            if (isPointOnSegment(b[0], c[0], d[0]))
            {
                return {{b, d}};
            }
            // d in (ab), b not in (cd): check c in (ab)
            if (isPointOnSegment(c[0], a[0], b[0]))
            {
                return {{c, d}};
            }

            std::stringstream err;
            err.precision(std::numeric_limits<double>::digits10);
            err << ab << " x " << cd;
            OGS_FATAL(
                "The case of parallel line segments ({:s}) is not handled yet. "
                "Aborting.",
                err.str());
        }
        return std::vector<MathLib::Point3d>();
    }

    // precondition: points a, b, c are collinear
    // the function checks if the point c is onto the line segment (a,b)
    auto isCollinearPointOntoLineSegment = [](MathLib::Point3d const& a,
                                              MathLib::Point3d const& b,
                                              MathLib::Point3d const& c)
    {
        if (b[0] - a[0] != 0)
        {
            double const t = (c[0] - a[0]) / (b[0] - a[0]);
            return 0.0 <= t && t <= 1.0;
        }
        if (b[1] - a[1] != 0)
        {
            double const t = (c[1] - a[1]) / (b[1] - a[1]);
            return 0.0 <= t && t <= 1.0;
        }
        if (b[2] - a[2] != 0)
        {
            double const t = (c[2] - a[2]) / (b[2] - a[2]);
            return 0.0 <= t && t <= 1.0;
        }
        return false;
    };

    if (orient_abc == 0.0)
    {
        if (isCollinearPointOntoLineSegment(a, b, c))
        {
            return {{c}};
        }
        return std::vector<MathLib::Point3d>();
    }

    if (orient_abd == 0.0)
    {
        if (isCollinearPointOntoLineSegment(a, b, d))
        {
            return {{d}};
        }
        return std::vector<MathLib::Point3d>();
    }

    // check if the segment (ab) lies on the left or on the right of (cd)
    double const orient_cda(getOrientation(c, d, a));
    double const orient_cdb(getOrientation(c, d, b));
    if ((orient_cda > 0 && orient_cdb > 0) ||
        (orient_cda < 0 && orient_cdb < 0))
    {
        return std::vector<MathLib::Point3d>();
    }

    // at this point it is sure that there is an intersection and the system of
    // linear equations will be invertible
    // solve the two linear equations (b-a, c-d) (t, s)^T = (c-a) simultaneously
    Eigen::Matrix2d mat;
    mat(0, 0) = b[0] - a[0];
    mat(0, 1) = c[0] - d[0];
    mat(1, 0) = b[1] - a[1];
    mat(1, 1) = c[1] - d[1];
    Eigen::Vector2d rhs{c[0] - a[0], c[1] - a[1]};

    rhs = mat.partialPivLu().solve(rhs);
    if (0 <= rhs[1] && rhs[1] <= 1.0)
    {
        return {MathLib::Point3d{std::array<double, 3>{
            {c[0] + rhs[1] * (d[0] - c[0]), c[1] + rhs[1] * (d[1] - c[1]),
             c[2] + rhs[1] * (d[2] - c[2])}}}};
    }
    return std::vector<MathLib::Point3d>();  // parameter s not in the valid
                                             // range
}

void sortSegments(MathLib::Point3d const& seg_beg_pnt,
                  std::vector<GeoLib::LineSegment>& sub_segments)
{
    double const eps(std::numeric_limits<double>::epsilon());

    auto findNextSegment =
        [&eps](MathLib::Point3d const& seg_beg_pnt,
               std::vector<GeoLib::LineSegment>& sub_segments,
               std::vector<GeoLib::LineSegment>::iterator& sub_seg_it)
    {
        if (sub_seg_it == sub_segments.end())
        {
            return;
        }
        // find appropriate segment for the given segment begin point
        auto act_beg_seg_it = std::find_if(
            sub_seg_it, sub_segments.end(),
            [&seg_beg_pnt, &eps](GeoLib::LineSegment const& seg)
            {
                return MathLib::sqrDist(seg_beg_pnt, seg.getBeginPoint()) <
                           eps ||
                       MathLib::sqrDist(seg_beg_pnt, seg.getEndPoint()) < eps;
            });
        if (act_beg_seg_it == sub_segments.end())
        {
            return;
        }
        // if necessary correct orientation of segment, i.e. swap beg and
        // end
        if (MathLib::sqrDist(seg_beg_pnt, act_beg_seg_it->getEndPoint()) <
            MathLib::sqrDist(seg_beg_pnt, act_beg_seg_it->getBeginPoint()))
        {
            std::swap(act_beg_seg_it->getBeginPoint(),
                      act_beg_seg_it->getEndPoint());
        }
        assert(sub_seg_it != sub_segments.end());
        // exchange segments within the container
        if (sub_seg_it != act_beg_seg_it)
        {
            std::swap(*sub_seg_it, *act_beg_seg_it);
        }
    };

    // find start segment
    auto seg_it = sub_segments.begin();
    findNextSegment(seg_beg_pnt, sub_segments, seg_it);

    while (seg_it != sub_segments.end())
    {
        MathLib::Point3d& new_seg_beg_pnt(seg_it->getEndPoint());
        seg_it++;
        if (seg_it != sub_segments.end())
        {
            findNextSegment(new_seg_beg_pnt, sub_segments, seg_it);
        }
    }
}

Eigen::Matrix3d compute2DRotationMatrixToX(Eigen::Vector3d const& v)
{
    Eigen::Matrix3d rot_mat = Eigen::Matrix3d::Zero();
    const double cos_theta = v[0];
    const double sin_theta = v[1];
    rot_mat(0, 0) = rot_mat(1, 1) = cos_theta;
    rot_mat(0, 1) = sin_theta;
    rot_mat(1, 0) = -sin_theta;
    rot_mat(2, 2) = 1.0;
    return rot_mat;
}

Eigen::Matrix3d compute3DRotationMatrixToX(Eigen::Vector3d const& v)
{
    // a vector on the plane
    Eigen::Vector3d yy = Eigen::Vector3d::Zero();
    auto const eps = std::numeric_limits<double>::epsilon();
    if (std::abs(v[0]) > 0.0 && std::abs(v[1]) + std::abs(v[2]) < eps)
    {
        yy[2] = 1.0;
    }
    else if (std::abs(v[1]) > 0.0 && std::abs(v[0]) + std::abs(v[2]) < eps)
    {
        yy[0] = 1.0;
    }
    else if (std::abs(v[2]) > 0.0 && std::abs(v[0]) + std::abs(v[1]) < eps)
    {
        yy[1] = 1.0;
    }
    else
    {
        for (unsigned i = 0; i < 3; i++)
        {
            if (std::abs(v[i]) > 0.0)
            {
                yy[i] = -v[i];
                break;
            }
        }
    }
    // z"_vec
    Eigen::Vector3d const zz = v.cross(yy).normalized();
    // y"_vec
    yy = zz.cross(v).normalized();

    Eigen::Matrix3d rot_mat;
    rot_mat.row(0) = v;
    rot_mat.row(1) = yy;
    rot_mat.row(2) = zz;
    return rot_mat;
}

}  // end namespace GeoLib