/*---------------------------------------------------------------------------*\
  =========                 |
  \\      /  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 "demandDrivenData.H"
#include "volumeOptimizer.H"
#include "tetrahedron.H"

//#define DEBUGSmooth

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

Foam::scalar Foam::Module::volumeOptimizer::evaluateFunc() const
{
    const scalar K = evaluateStabilisationFactor();

    scalar func(0.0);

    forAll(tets_, tetI)
    {
        const partTet& pt = tets_[tetI];
        const tetrahedron<point, point> tet
        (
            points_[pt.a()],
            points_[pt.b()],
            points_[pt.c()],
            points_[pt.d()]
        );

        const scalar LSqrTri
        (
            magSqr(tet.d() - tet.a()) +
            magSqr(tet.d() - tet.b()) +
            magSqr(tet.d() - tet.c())
        );

        const scalar Vtri = tet.mag();
        const scalar stab = sqrt(sqr(Vtri) + K);
        const scalar Vstab = 0.5*(Vtri + stab);

        func += LSqrTri/pow(Vstab, 2./3.);
    }

    return func;
}


Foam::scalar Foam::Module::volumeOptimizer::evaluateStabilisationFactor() const
{
    scalar K = 0.0;

    scalar Vmin(VGREAT), LSqMax(0.0);

    forAll(tets_, tetI)
    {
        const partTet& pt = tets_[tetI];
        const tetrahedron<point, point> tet
        (
            points_[pt.a()],
            points_[pt.b()],
            points_[pt.c()],
            points_[pt.d()]
        );

        const scalar Vtri = tet.mag();

        Vmin = Foam::min(Vmin, Vtri);

        const scalar LSqrTri
        (
            magSqr(tet.d() - tet.a()) +
            magSqr(tet.d() - tet.b()) +
            magSqr(tet.d() - tet.c())
        );

        LSqMax = Foam::max(LSqMax, LSqrTri);
    }

    if (Vmin < SMALL*LSqMax)
        K = SMALL*LSqMax;

    return K;
}


void Foam::Module::volumeOptimizer::evaluateGradientsExact
(
    vector& gradF,
    tensor& gradGradF
) const
{
    gradF = vector::zero;
    gradGradF = tensor::zero;

    const scalar K = evaluateStabilisationFactor();

    tensor gradGradLsq(tensor::zero);
    gradGradLsq.xx() = 6.0;
    gradGradLsq.yy() = 6.0;
    gradGradLsq.zz() = 6.0;

    const point& p = points_[pointI_];

    const scalar C = 2./3. * pow(0.5, 2./3.);
    const scalar C1 = C/3.;

    forAll(tets_, tetI)
    {
        const partTet& pt = tets_[tetI];
        const tetrahedron<point, point> tet
        (
            points_[pt.a()],
            points_[pt.b()],
            points_[pt.c()],
            points_[pt.d()]
        );

        // calculate the gradient of the volume
        const vector gradV
        (
            (1.0/6.0) *
            (
                (tet.b() - tet.a()) ^
                (tet.c() - tet.a())
            )
        );

        // calculate the Frobenius norm
        const scalar LSqrTri
        (
            magSqr(tet.d() - tet.a()) +
            magSqr(tet.d() - tet.b()) +
            magSqr(tet.d() - tet.c())
        );

        // calculate the volume of the tetrahedron
        const scalar Vtri = tet.mag();

        // calculate the stabilisation factor for the volume
        const scalar stab = sqrt(sqr(Vtri) + K);

        // evaluate the stabilised volume
        const scalar Vs = 0.5*(Vtri + stab);

        if (Vs < VSMALL)
        {
            Info<< "Tet " << tet << endl;
            Info<< "gradV " << gradV << endl;
            Info<< "Vtri " << Vtri << endl;
            IOstream::defaultPrecision(20);
            Info<< "Vstab " << Vs << endl;

            FatalErrorInFunction
                << "I cannot continue " << exit(FatalError);
        }

        // calculate the gradient of the stabilisation volume
        const vector gradStab = Vtri*gradV/stab;

        // calculate the gradient of the Frobenius norm
        const vector gradLsq = 2. * (3. * p - tet.a() - tet.b() - tet.c());

        // calculate the gradient of the stabilised volume
        const vector gradVs = 0.5*(gradV + gradStab);

        // calculate the gradient of the functional
        const scalar Vs13 = pow(2. * Vs, 1./3.);
        const scalar Vstab = pow(Vs, 2./3.);
        const scalar sqrVstab = sqr(Vstab);
        const vector gradVstab = C*(2. * gradVs) / Vs13;

        gradF += gradLsq/Vstab - LSqrTri*gradVstab/sqrVstab;

        // calculate the second gradient of the stabilisation volume
        const tensor gradGradStab =
            (gradV*gradV) / stab -
            sqr(Vtri)*(gradV*gradV) / pow(stab, 3);

        // calculate the second gradient of the stabilised volume

        const tensor gradGradVstab =
            C*(gradGradStab/Vs13) -
            C1*4. * (gradVs*gradVs) / pow(Vs13, 4);

        // calculate the second gradient
        gradGradF +=
            gradGradLsq/Vstab -
            twoSymm(gradLsq*(gradVstab/sqrVstab)) -
            LSqrTri*gradGradVstab/sqrVstab +
            2.0*LSqrTri*(gradVstab*gradVstab)/(sqrVstab*Vstab);
    }
}


Foam::scalar Foam::Module::volumeOptimizer::optimiseDivideAndConquer
(
    const scalar tol
)
{
    point& pOpt = points_[pointI_];

    pOpt = 0.5*(bb_.max() + bb_.min());
    point currCentre = pOpt;
    scalar dx = (bb_.max().x() - bb_.min().x()) / 2.0;
    scalar dy = (bb_.max().y() - bb_.min().y()) / 2.0;
    scalar dz = (bb_.max().z() - bb_.min().z()) / 2.0;

    label iter(0);

    // find the value of the functional in the centre of the bnd box
    scalar funcBefore, funcAfter(evaluateFunc());

    do
    {
        funcBefore = funcAfter;

        funcAfter = VGREAT;
        point minCentre(vector::zero);

        for (label i = 0; i < 8; ++i)
        {
            pOpt.x() = currCentre.x() + 0.5*dirVecs[i].x()*dx;
            pOpt.y() = currCentre.y() + 0.5*dirVecs[i].y()*dy;
            pOpt.z() = currCentre.z() + 0.5*dirVecs[i].z()*dz;

            const scalar func = evaluateFunc();

            if (func < funcAfter)
            {
                minCentre = pOpt;
                funcAfter = func;
            }
        }

        // set the centre with the minimum value
        // as the centre for future search
        currCentre = minCentre;
        pOpt = minCentre;

        // halve the search range
        dx *= 0.5;
        dy *= 0.5;
        dz *= 0.5;

        // calculate the tolerence
        const scalar t = mag(funcAfter - funcBefore) / funcAfter;

        # ifdef DEBUGSmooth
        Info<< "Point position " << pOpt << endl;
        Info<< "Func before " << funcBefore << endl;
        Info<< "Func after " << funcAfter << endl;
        Info<< "Normalised difference " << t << endl;
        # endif

        if (t < tol)
            break;
    } while (++iter < 100);

    return funcAfter;
}


Foam::scalar Foam::Module::volumeOptimizer::optimiseSteepestDescent
(
    const scalar tol
)
{
    label iter(0);

    point& p = points_[pointI_];

    # ifdef DEBUGSmooth
    Info<< nl << "Smoothing point " << pointI_
         << " with coordinates " << p << endl;
    scalar Vmina(VGREAT);
    forAll(tets_, tetI)
    Vmina = Foam::min(Vmina, tets_[tetI].mag(points_));
    Info<< "Vmin before " << Vmina << endl;
    # endif

    vector gradF;
    vector disp(vector::zero);
    tensor gradGradF;
    point pOrig;

    scalar funcBefore, funcAfter(evaluateFunc());

    bool finished;
    do
    {
        finished = false;
        pOrig = p;
        funcBefore = funcAfter;

        evaluateGradientsExact(gradF, gradGradF);

        const scalar determinant = Foam::det(gradGradF);
        if (determinant > SMALL)
        {
            disp = (inv(gradGradF, determinant) & gradF);

            p -= disp;

            funcAfter = evaluateFunc();

            # ifdef DEBUGSmooth
            Info<< nl << "gradF " << gradF << endl;
            Info<< "gradGradF " << gradGradF << endl;
            Info<< "det(gradGradF) " << determinant << endl;
            Info<< "disp " << disp << endl;
            Info<< "Func before " << funcBefore << endl;
            Info<< "Func after " << funcAfter << endl;
            # endif

            scalar relax(0.8);
            label nLoops(0);

            while (funcAfter > funcBefore)
            {
                p = pOrig - relax*disp;
                relax *= 0.5;
                funcAfter = evaluateFunc();

                if (funcAfter < funcBefore)
                    continue;

                if (++nLoops == 5)
                {
                    // it seems that this direction is wrong, stop the loop
                    p = pOrig;
                    disp = vector::zero;
                    finished = true;
                    funcAfter = funcBefore;
                }
            }

            if (mag(funcBefore-funcAfter) / funcBefore < tol)
                finished = true;
        }
        else
        {
            // move in random direction
            // this is usually needed to move the point off the zero volume
            disp = vector::zero;
            forAll(tets_, tetI)
            {
                const partTet& tet = tets_[tetI];
                const scalar Vtri = tet.mag(points_);

                if (Vtri < SMALL)
                {
                    triangle<point, point> tri
                    (
                        points_[tet.a()],
                        points_[tet.b()],
                        points_[tet.c()]
                    );

                    vector n = tri.areaNormal();
                    const scalar d = mag(n);

                    if (d > VSMALL)
                        disp += 0.01*(n/d);
                }
            }

            p += disp;
            funcAfter = evaluateFunc();
        }
    } while ((++iter < 100) && !finished);

    # ifdef DEBUGSmooth
    scalar Vmin(VGREAT);
    forAll(tets_, tetI)
        Vmin = Foam::min(Vmin, tets_[tetI].mag(points_));

    Info<< nl << "New coordinates for point "
        << pointI_ << " are " << p << endl;
    Info<< "Num iterations " << iter << " gradient " << gradF << endl;
    Info<< "Vmin " << Vmin << endl;
    # endif

    return funcAfter;
}


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