/**
 * \file
 * \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
 *
 */

namespace GeoLib
{
template <typename InputIterator>
std::pair<Eigen::Vector3d, double> getNewellPlane(InputIterator pnts_begin,
                                                  InputIterator pnts_end)
{
    Eigen::Vector3d plane_normal({0, 0, 0});
    Eigen::Vector3d centroid({0, 0, 0});
    for (auto i = std::prev(pnts_end), j = pnts_begin; j != pnts_end;
         i = j, ++j)
    {
        auto const& pt_i = *(*i);
        auto const& pt_j = *(*j);
        plane_normal[0] +=
            (pt_i[1] - pt_j[1]) * (pt_i[2] + pt_j[2]);  // projection on yz
        plane_normal[1] +=
            (pt_i[2] - pt_j[2]) * (pt_i[0] + pt_j[0]);  // projection on xz
        plane_normal[2] +=
            (pt_i[0] - pt_j[0]) * (pt_i[1] + pt_j[1]);  // projection on xy

        centroid += pt_j.asEigenVector3d();
    }

    plane_normal.normalize();
    auto n_pnts(std::distance(pnts_begin, pnts_end));
    assert(n_pnts > 2);
    double d = 0.0;
    if (n_pnts > 0)
    {
        d = centroid.dot(plane_normal) / n_pnts;
    }
    return std::make_pair(plane_normal, d);
}

template <class T_POINT>
std::pair<Eigen::Vector3d, double> getNewellPlane(
    const std::vector<T_POINT*>& pnts)
{
    return getNewellPlane(pnts.begin(), pnts.end());
}

template <class T_POINT>
std::pair<Eigen::Vector3d, double> getNewellPlane(
    const std::vector<T_POINT>& pnts)
{
    Eigen::Vector3d plane_normal({0, 0, 0});
    Eigen::Vector3d centroid({0, 0, 0});
    std::size_t n_pnts(pnts.size());
    for (std::size_t i = n_pnts - 1, j = 0; j < n_pnts; i = j, j++)
    {
        plane_normal[0] += (pnts[i][1] - pnts[j][1]) *
                           (pnts[i][2] + pnts[j][2]);  // projection on yz
        plane_normal[1] += (pnts[i][2] - pnts[j][2]) *
                           (pnts[i][0] + pnts[j][0]);  // projection on xz
        plane_normal[2] += (pnts[i][0] - pnts[j][0]) *
                           (pnts[i][1] + pnts[j][1]);  // projection on xy

        centroid += pnts[j].asEigenVector3d();
    }

    plane_normal.normalize();
    double const d = centroid.dot(plane_normal) / n_pnts;
    return std::make_pair(plane_normal, d);
}

template <typename InputIterator>
void rotatePoints(Eigen::Matrix3d const& rot_mat, InputIterator pnts_begin,
                  InputIterator pnts_end)
{
    for (auto it = pnts_begin; it != pnts_end; ++it)
    {
        (*it)->asEigenVector3d() = rot_mat * (*it)->asEigenVector3d();
    }
}

template <typename InputIterator1, typename InputIterator2>
Eigen::Matrix3d rotatePointsToXY(InputIterator1 p_pnts_begin,
                                 InputIterator1 p_pnts_end,
                                 InputIterator2 r_pnts_begin,
                                 InputIterator2 r_pnts_end)
{
    assert(std::distance(p_pnts_begin, p_pnts_end) > 2);

    // compute the plane normal
    auto const [plane_normal, d] =
        GeoLib::getNewellPlane(p_pnts_begin, p_pnts_end);

    // rotate points into x-y-plane
    Eigen::Matrix3d const rot_mat = computeRotationMatrixToXY(plane_normal);
    rotatePoints(rot_mat, r_pnts_begin, r_pnts_end);

    for (auto it = r_pnts_begin; it != r_pnts_end; ++it)
    {
        (*(*it))[2] = 0.0;  // should be -= d but there are numerical errors
    }

    return rot_mat;
}

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

}  // end namespace GeoLib