/*---------------------------------------------------------------------------*\
  =========                 |
  \\      /  F ield         | cfMesh: A library for mesh generation
   \\    /   O peration     |
    \\  /    A nd           | www.cfmesh.com
     \\/     M anipulation  |
-------------------------------------------------------------------------------
    Copyright (C) 2014-2017 Creative Fields, Ltd.
-------------------------------------------------------------------------------
Author
     Franjo Juretic (franjo.juretic@c-fields.com)

License
    This file is part of OpenFOAM.

    OpenFOAM is free software: you can redistribute it and/or modify it
    under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    OpenFOAM is distributed in the hope that it will be useful, but WITHOUT
    ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
    FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
    for more details.

    You should have received a copy of the GNU General Public License
    along with OpenFOAM.  If not, see <http://www.gnu.org/licenses/>.

\*---------------------------------------------------------------------------*/

#include "quadricFitting.H"

// * * * * * * * * * Private Member Functions * * * * * * * * * * * * * * * * //

void Foam::Module::quadricFitting::calculateNormalVector()
{
    symmTensor mat(symmTensor::zero);

    forAll(otherPoints_, i)
    {
        const vector d = (otherPoints_[i] - origin_);

        mat += symm(d*d);
    }

    mat /= otherPoints_.size();

    //- find the eigenvalues of the tensor
    const vector ev = eigenValues(mat);

    //- estimate the normal as the eigenvector associated
    //- to the smallest eigenvalue
    normal_ = eigenVectors(mat, ev).x();
}


void Foam::Module::quadricFitting::calculateCoordinateSystem()
{
    if (mag(normal_) < VSMALL)
    {
        vecX_ = vecY_ = vector::zero;
        forAll(transformedPoints_, i)
            transformedPoints_[i] = vector::zero;

        return;
    }

    const plane pl(origin_, normal_);

    bool valid(false);

    label pI(0);
    while (!valid)
    {
        const point vp = pl.nearestPoint(otherPoints_[pI]);
        vecX_ = vp - origin_;

        if (magSqr(vecX_) < VSMALL)
        {
            ++pI;
            continue;
        }

        vecX_ /= mag(vecX_);
        vecY_ = normal_ ^ vecX_;
        valid = true;
    }

    if (!valid)
    {
        FatalErrorInFunction
            << "Cannot find a valid coordinate system!" << endl;

        normal_ = vecX_ = vecY_ = vector::zero;
    }

    //- transform other points to this new coordinate system
    transformedPoints_.setSize(otherPoints_.size());

    forAll(otherPoints_, i)
    {
        const vector delta = (otherPoints_[i] - origin_);
        point& p = transformedPoints_[i];

        p.x() = delta & vecX_;
        p.y() = delta & vecY_;
        p.z() = delta & normal_;
    }
}


void Foam::Module::quadricFitting::calculateQuadricCoeffs()
{
    if (mag(normal_) < VSMALL)
    {
        coefficients_.setSize(5);
        coefficients_ = 0.0;
        return;
    }

    simpleMatrix<scalar> mat(5, 0.0, 0.0);
    DynList<scalar> helper;
    helper.setSize(5);

    forAll(transformedPoints_, i)
    {
        const point& p = transformedPoints_[i];

        helper[0] = sqr(p.x());
        helper[1] = sqr(p.y());
        helper[2] = p.x()*p.y();
        helper[3] = p.x();
        helper[4] = p.y();

        for (label rowI = 0; rowI < 5; ++rowI)
        {
            for (label colI = rowI; colI < 5; ++colI)
            {
                mat[rowI][colI] += helper[rowI]*helper[colI];
            }

            mat.source()[rowI] += helper[rowI]*p.z();
        }
    }

    for (label rowI = 1; rowI < 5; ++rowI)
    {
        for (label colI = 0; colI < rowI; ++colI)
        {
            mat[rowI][colI] = mat[colI][rowI];
        }

        if (mag(mat[rowI][rowI]) < SMALL)
        {
            mat[rowI][rowI] = SMALL;
        }
    }

    coefficients_ = mat.LUsolve();
}


void Foam::Module::quadricFitting::calculateBestFit()
{
    label iteration(0);

    while (iteration++< 10)
    {
        calculateCoordinateSystem();

        calculateQuadricCoeffs();

        if ((mag(coefficients_[3]) > SMALL) || (mag(coefficients_[4]) > SMALL))
        {
            //- correct the normal
            const scalar d =
                Foam::sqrt(1 + sqr(coefficients_[3]) + sqr(coefficients_[4]));

            const vector newNormal
            (
                normal_/d -
                coefficients_[3]*vecX_/d -
                coefficients_[4]*vecY_/d
            );

            normal_ = newNormal;
        }
        else
        {
            break;
        }
    }

    # ifdef DEBUGQuadric
    Info<< "Other points " << otherPoints_ << endl;
    Info<< "Transformed points " << transformedPoints_ << endl;
    Info<< "normal " << normal_ << endl;
    Info<< "Coefficients " << coefficients_ << endl;
    # endif
}


// * * * * * * * * * * * * * * * *  Constructors * * * * * * * * * * * * * * //

template<class ListType>
inline Foam::Module::quadricFitting::quadricFitting
(
    const point& origin,
    const vector normal,
    const ListType& otherPoints
)
:
    origin_(origin),
    normal_(normal),
    vecX_(vector::zero),
    vecY_(vector::zero),
    otherPoints_(),
    transformedPoints_(),
    coefficients_()
{
    otherPoints_.setSize(otherPoints.size());
    forAll(otherPoints, i)
    {
        otherPoints_[i] = otherPoints[i];
    }

    if (magSqr(normal_) < VSMALL)
    {
        FatalErrorInFunction
            << "Cannot construct the quadric surface for point "
            << origin_ << " because the normal does not exist!"
            << "\nThis indicates that the input"
            << " surface mesh is of poor quality" << exit(FatalError);

        normal_ = vector::zero;
    }
    else
    {
        normal_ /= mag(normal_);
    }

    calculateBestFit();
}


template<class ListType>
inline Foam::Module::quadricFitting::quadricFitting
(
    const point& origin,
    const ListType& otherPoints
)
:
    origin_(origin),
    normal_(),
    vecX_(),
    vecY_(),
    otherPoints_(),
    transformedPoints_(),
    coefficients_()
{
    otherPoints_.setSize(otherPoints.size());
    forAll(otherPoints, i)
    {
        otherPoints_[i] = otherPoints[i];
    }

    calculateNormalVector();

    calculateBestFit();
}


// * * * * * * * * * * * * * * * Member Functions  * * * * * * * * * * * * * //

inline const Foam::vector& Foam::Module::quadricFitting::normal() const
{
    return normal_;
}


inline Foam::scalar Foam::Module::quadricFitting::minCurvature() const
{
    return
    (
        coefficients_[0] + coefficients_[1] -
        sqrt(sqr(coefficients_[0] - coefficients_[1]) + sqr(coefficients_[2]))
    );
}


inline Foam::scalar Foam::Module::quadricFitting::maxCurvature() const
{
    return
    (
        coefficients_[0] + coefficients_[1] +
        sqrt(sqr(coefficients_[0] - coefficients_[1]) + sqr(coefficients_[2]))
    );
}


inline Foam::scalar Foam::Module::quadricFitting::meanCurvature() const
{
    return 0.5*(minCurvature() + maxCurvature());
}


inline Foam::scalar Foam::Module::quadricFitting::gaussianCurvature() const
{
    return minCurvature()*maxCurvature();
}


inline Foam::vector Foam::Module::quadricFitting::minCurvatureVector() const
{
    const scalar theta =
        0.5*Foam::atan
        (
            coefficients_[2] /
            stabilise(coefficients_[0] - coefficients_[1], VSMALL)
        );

    return Foam::cos(theta)*vecX_ + Foam::sin(theta)*vecY_;
}


inline Foam::vector Foam::Module::quadricFitting::maxCurvatureVector() const
{
    const scalar theta =
        0.5*Foam::atan
        (
            coefficients_[2] /
            stabilise(coefficients_[0] - coefficients_[1], VSMALL)
        );

    return Foam::sin(theta)*vecX_ + Foam::cos(theta)*vecY_;
}


// ************************************************************************* //
