/*---------------------------------------------------------------------------*\
 =========                   |
 \\      /   F ield          | OpenFOAM: The Open Source CFD Toolbox
  \\    /    O peration      |
   \\  /     A nd            | Copyright (C) 2009-2010 OpenCFD Ltd.
    \\/      M anipulation   |
-------------------------------------------------------------------------------
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 2 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, write to the Free Software Foundation,
    Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

Application
    momentOfInertiaTest

Description
    Calculates the inertia tensor and principal axes and moments of a
    command line specified triSurface.  Inertia can either be of the
    solid body or of a thin shell.

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

#include "argList.H"
#include "ListOps.H"
#include "face.H"
#include "tetPointRef.H"
#include "triFaceList.H"
#include "triSurface.H"
#include "OFstream.H"
#include "meshTools.H"
#include "Random.H"
#include "transform.H"
#include "IOmanip.H"
#include "Pair.H"

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

using namespace Foam;

void massPropertiesSolid
(
    const pointField& pts,
    const triFaceList triFaces,
    scalar density,
    scalar& mass,
    vector& cM,
    tensor& J
)
{
    // Reimplemented from: Wm4PolyhedralMassProperties.cpp
    // File Version: 4.10.0 (2009/11/18)

    // Geometric Tools, LC
    // Copyright (c) 1998-2010
    // Distributed under the Boost Software License, Version 1.0.
    // http://www.boost.org/LICENSE_1_0.txt
    // http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt

    // Boost Software License - Version 1.0 - August 17th, 2003

    // Permission is hereby granted, free of charge, to any person or
    // organization obtaining a copy of the software and accompanying
    // documentation covered by this license (the "Software") to use,
    // reproduce, display, distribute, execute, and transmit the
    // Software, and to prepare derivative works of the Software, and
    // to permit third-parties to whom the Software is furnished to do
    // so, all subject to the following:

    // The copyright notices in the Software and this entire
    // statement, including the above license grant, this restriction
    // and the following disclaimer, must be included in all copies of
    // the Software, in whole or in part, and all derivative works of
    // the Software, unless such copies or derivative works are solely
    // in the form of machine-executable object code generated by a
    // source language processor.

    // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
    // EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
    // OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE AND
    // NON-INFRINGEMENT. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR
    // ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR
    // OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
    // ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE
    // USE OR OTHER DEALINGS IN THE SOFTWARE.

    const scalar r6 = 1.0/6.0;
    const scalar r24 = 1.0/24.0;
    const scalar r60 = 1.0/60.0;
    const scalar r120 = 1.0/120.0;

    // order:  1, x, y, z, x^2, y^2, z^2, xy, yz, zx
    scalarField integrals(10, 0.0);

    forAll(triFaces, i)
    {
        const triFace& tri(triFaces[i]);

        // vertices of triangle i
        vector v0 = pts[tri[0]];
        vector v1 = pts[tri[1]];
        vector v2 = pts[tri[2]];

        // cross product of edges
        vector eA = v1 - v0;
        vector eB = v2 - v0;
        vector n = eA ^ eB;

        // compute integral terms
        scalar tmp0, tmp1, tmp2;

        scalar f1x, f2x, f3x, g0x, g1x, g2x;

        tmp0 = v0.x() + v1.x();
        f1x = tmp0 + v2.x();
        tmp1 = v0.x()*v0.x();
        tmp2 = tmp1 + v1.x()*tmp0;
        f2x = tmp2 + v2.x()*f1x;
        f3x = v0.x()*tmp1 + v1.x()*tmp2 + v2.x()*f2x;
        g0x = f2x + v0.x()*(f1x + v0.x());
        g1x = f2x + v1.x()*(f1x + v1.x());
        g2x = f2x + v2.x()*(f1x + v2.x());

        scalar f1y, f2y, f3y, g0y, g1y, g2y;

        tmp0 = v0.y() + v1.y();
        f1y = tmp0 + v2.y();
        tmp1 = v0.y()*v0.y();
        tmp2 = tmp1 + v1.y()*tmp0;
        f2y = tmp2 + v2.y()*f1y;
        f3y = v0.y()*tmp1 + v1.y()*tmp2 + v2.y()*f2y;
        g0y = f2y + v0.y()*(f1y + v0.y());
        g1y = f2y + v1.y()*(f1y + v1.y());
        g2y = f2y + v2.y()*(f1y + v2.y());

        scalar f1z, f2z, f3z, g0z, g1z, g2z;

        tmp0 = v0.z() + v1.z();
        f1z = tmp0 + v2.z();
        tmp1 = v0.z()*v0.z();
        tmp2 = tmp1 + v1.z()*tmp0;
        f2z = tmp2 + v2.z()*f1z;
        f3z = v0.z()*tmp1 + v1.z()*tmp2 + v2.z()*f2z;
        g0z = f2z + v0.z()*(f1z + v0.z());
        g1z = f2z + v1.z()*(f1z + v1.z());
        g2z = f2z + v2.z()*(f1z + v2.z());

        // update integrals
        integrals[0] += n.x()*f1x;
        integrals[1] += n.x()*f2x;
        integrals[2] += n.y()*f2y;
        integrals[3] += n.z()*f2z;
        integrals[4] += n.x()*f3x;
        integrals[5] += n.y()*f3y;
        integrals[6] += n.z()*f3z;
        integrals[7] += n.x()*(v0.y()*g0x + v1.y()*g1x + v2.y()*g2x);
        integrals[8] += n.y()*(v0.z()*g0y + v1.z()*g1y + v2.z()*g2y);
        integrals[9] += n.z()*(v0.x()*g0z + v1.x()*g1z + v2.x()*g2z);
    }

    integrals[0] *= r6;
    integrals[1] *= r24;
    integrals[2] *= r24;
    integrals[3] *= r24;
    integrals[4] *= r60;
    integrals[5] *= r60;
    integrals[6] *= r60;
    integrals[7] *= r120;
    integrals[8] *= r120;
    integrals[9] *= r120;

    // mass
    mass = integrals[0];

    // center of mass
    cM = vector(integrals[1], integrals[2], integrals[3])/mass;

    // inertia relative to origin
    J.xx() = integrals[5] + integrals[6];
    J.xy() = -integrals[7];
    J.xz() = -integrals[9];
    J.yx() = J.xy();
    J.yy() = integrals[4] + integrals[6];
    J.yz() = -integrals[8];
    J.zx() = J.xz();
    J.zy() = J.yz();
    J.zz() = integrals[4] + integrals[5];

    // inertia relative to center of mass
    J -= mass*((cM & cM)*I - cM*cM);

    // Apply density
    mass *= density;
    J *= density;
}


void massPropertiesShell
(
    const pointField& pts,
    const triFaceList triFaces,
    scalar density,
    scalar& mass,
    vector& cM,
    tensor& J
)
{
    // Reset properties for accumulation

    mass = 0.0;
    cM = vector::zero;
    J = tensor::zero;

    // Find centre of mass

    forAll(triFaces, i)
    {
        const triFace& tri(triFaces[i]);

        triPointRef t
        (
            pts[tri[0]],
            pts[tri[1]],
            pts[tri[2]]
        );

        scalar triMag = t.mag();

        cM +=  triMag*t.centre();

        mass += triMag;
    }

    cM /= mass;

    mass *= density;

    // Find inertia around centre of mass

    forAll(triFaces, i)
    {
        const triFace& tri(triFaces[i]);

        J += triPointRef
        (
            pts[tri[0]],
            pts[tri[1]],
            pts[tri[2]]
        ).inertia(cM, density);
    }
}


tensor applyParallelAxisTheorem
(
    scalar m,
    const vector& cM,
    const tensor& J,
    const vector& refPt
)
{
    // The displacement vector (refPt = cM) is the displacement of the
    // new reference point from the centre of mass of the body that
    // the inertia tensor applies to.

    vector d = (refPt - cM);

    return J + m*((d & d)*I - d*d);
}


int main(int argc, char *argv[])
{
    argList::addNote
    (
        "Calculates the inertia tensor and principal axes and moments "
        "of the specified surface.\n"
        "Inertia can either be of the solid body or of a thin shell."
    );

    argList::noParallel();
    argList::validArgs.append("surface file");
    argList::addBoolOption("shellProperties");

    argList::addOption
    (
        "density",
        "scalar",
        "Specify density, "
        "kg/m3 for solid properties, kg/m2 for shell properties"
    );

    argList::addOption
    (
        "referencePoint",
        "vector",
        "Inertia relative to this point, not the centre of mass"
    );

    argList args(argc, argv);

    const fileName surfFileName = args[1];
    const scalar density = args.optionLookupOrDefault("density", 1.0);

    vector refPt = vector::zero;
    bool calcAroundRefPt = args.optionReadIfPresent("referencePoint", refPt);

    triSurface surf(surfFileName);

    triFaceList faces(surf.size());

    forAll(surf, i)
    {
        faces[i] = triFace(surf[i]);
    }

    scalar m = 0.0;
    vector cM = vector::zero;
    tensor J = tensor::zero;

    if (args.optionFound("shellProperties"))
    {
        massPropertiesShell
        (
            surf.points(),
            faces,
            density,
            m,
            cM,
            J
        );
    }
    else
    {
        massPropertiesSolid
        (
            surf.points(),
            faces,
            density,
            m,
            cM,
            J
        );
    }

    if (m < 0)
    {
        WarningIn(args.executable() + "::main")
            << "Negative mass detected" << endl;
    }

    vector eVal = eigenValues(J);

    tensor eVec = eigenVectors(J);

    label pertI = 0;

    Random rand(57373);

    while ((magSqr(eVal) < VSMALL) && pertI < 10)
    {
        WarningIn(args.executable() + "::main")
            << "No eigenValues found, shape may have symmetry, "
            << "perturbing inertia tensor diagonal" << endl;

        J.xx() *= 1.0 + SMALL*rand.scalar01();
        J.yy() *= 1.0 + SMALL*rand.scalar01();
        J.zz() *= 1.0 + SMALL*rand.scalar01();

        eVal = eigenValues(J);

        eVec = eigenVectors(J);

        pertI++;
    }

    bool showTransform = true;

    if
    (
        (mag(eVec.x() ^ eVec.y()) > (1.0 - SMALL))
     && (mag(eVec.y() ^ eVec.z()) > (1.0 - SMALL))
     && (mag(eVec.z() ^ eVec.x()) > (1.0 - SMALL))
    )
    {
        // Make the eigenvectors a right handed orthogonal triplet
        eVec.z() *= sign((eVec.x() ^ eVec.y()) & eVec.z());

        // Finding the most natural transformation.  Using Lists
        // rather than tensors to allow indexed permutation.

        // Cartesian basis vectors - right handed orthogonal triplet
        List<vector> cartesian(3);

        cartesian[0] = vector(1, 0, 0);
        cartesian[1] = vector(0, 1, 0);
        cartesian[2] = vector(0, 0, 1);

        // Principal axis basis vectors - right handed orthogonal
        // triplet
        List<vector> principal(3);

        principal[0] = eVec.x();
        principal[1] = eVec.y();
        principal[2] = eVec.z();

        scalar maxMagDotProduct = -GREAT;

        // Matching axis indices, first: cartesian, second:principal

        Pair<label> match(-1, -1);

        forAll(cartesian, cI)
        {
            forAll(principal, pI)
            {
                scalar magDotProduct = mag(cartesian[cI] & principal[pI]);

                if (magDotProduct > maxMagDotProduct)
                {
                    maxMagDotProduct = magDotProduct;

                    match.first() = cI;

                    match.second() = pI;
                }
            }
        }

        scalar sense = sign
        (
            cartesian[match.first()] & principal[match.second()]
        );

        if (sense < 0)
        {
            // Invert the best match direction and swap the order of
            // the other two vectors

            List<vector> tPrincipal = principal;

            tPrincipal[match.second()] *= -1;

            tPrincipal[(match.second() + 1) % 3] =
                principal[(match.second() + 2) % 3];

            tPrincipal[(match.second() + 2) % 3] =
                principal[(match.second() + 1) % 3];

            principal = tPrincipal;

            vector tEVal = eVal;

            tEVal[(match.second() + 1) % 3] = eVal[(match.second() + 2) % 3];

            tEVal[(match.second() + 2) % 3] = eVal[(match.second() + 1) % 3];

            eVal = tEVal;
        }

        label permutationDelta = match.second() - match.first();

        if (permutationDelta != 0)
        {
            // Add 3 to the permutationDelta to avoid negative indices

            permutationDelta += 3;

            List<vector> tPrincipal = principal;

            vector tEVal = eVal;

            for (label i = 0; i < 3; i++)
            {
                tPrincipal[i] = principal[(i + permutationDelta) % 3];

                tEVal[i] = eVal[(i + permutationDelta) % 3];
            }

            principal = tPrincipal;

            eVal = tEVal;
        }

        label matchedAlready = match.first();

        match =Pair<label>(-1, -1);

        maxMagDotProduct = -GREAT;

        forAll(cartesian, cI)
        {
            if (cI == matchedAlready)
            {
                continue;
            }

            forAll(principal, pI)
            {
                if (pI == matchedAlready)
                {
                    continue;
                }

                scalar magDotProduct = mag(cartesian[cI] & principal[pI]);

                if (magDotProduct > maxMagDotProduct)
                {
                    maxMagDotProduct = magDotProduct;

                    match.first() = cI;

                    match.second() = pI;
                }
            }
        }

        sense = sign
        (
            cartesian[match.first()] & principal[match.second()]
        );

        if (sense < 0 || (match.second() - match.first()) != 0)
        {
            principal[match.second()] *= -1;

            List<vector> tPrincipal = principal;

            tPrincipal[(matchedAlready + 1) % 3] =
                principal[(matchedAlready + 2) % 3]*-sense;

            tPrincipal[(matchedAlready + 2) % 3] =
                principal[(matchedAlready + 1) % 3]*-sense;

            principal = tPrincipal;

            vector tEVal = eVal;

            tEVal[(matchedAlready + 1) % 3] = eVal[(matchedAlready + 2) % 3];

            tEVal[(matchedAlready + 2) % 3] = eVal[(matchedAlready + 1) % 3];

            eVal = tEVal;
        }

        eVec.x() = principal[0];
        eVec.y() = principal[1];
        eVec.z() = principal[2];

        // {
        //     tensor R = rotationTensor(vector(1, 0, 0), eVec.x());

        //     R = rotationTensor(R & vector(0, 1, 0), eVec.y()) & R;

        //     Info<< "R = " << nl << R << endl;

        //     Info<< "R - eVec.T() " << R - eVec.T() << endl;
        // }
    }
    else
    {
        WarningIn(args.executable() + "::main")
            << "Non-unique eigenvectors, cannot compute transformation "
            << "from Cartesian axes" << endl;

        showTransform = false;
    }

    Info<< nl << setprecision(10)
        << "Density: " << density << nl
        << "Mass: " << m << nl
        << "Centre of mass: " << cM << nl
        << "Inertia tensor around centre of mass: " << nl << J << nl
        << "eigenValues (principal moments): " << eVal << nl
        << "eigenVectors (principal axes): " << nl
        << eVec.x() << nl << eVec.y() << nl << eVec.z() << endl;

    if (showTransform)
    {
        Info<< "Transform tensor from reference state (Q). " << nl
            << "Rotation tensor required to transform "
               "from the body reference frame to the global "
               "reference frame, i.e.:" << nl
            << "globalVector = Q & bodyLocalVector"
            << nl << eVec.T()
            << endl;
    }

    if (calcAroundRefPt)
    {
        Info << "Inertia tensor relative to " << refPt << ": "
            << applyParallelAxisTheorem(m, cM, J, refPt)
            << endl;
    }

    OFstream str("axes.obj");

    Info<< nl << "Writing scaled principal axes at centre of mass of "
        << surfFileName <<  " to " << str.name() << endl;

    scalar scale = mag(cM - surf.points()[0])/eVal.component(findMin(eVal));

    meshTools::writeOBJ(str, cM);
    meshTools::writeOBJ(str, cM + scale*eVal.x()*eVec.x());
    meshTools::writeOBJ(str, cM + scale*eVal.y()*eVec.y());
    meshTools::writeOBJ(str, cM + scale*eVal.z()*eVec.z());

    for (label i = 1; i < 4; i++)
    {
        str << "l " << 1 << ' ' << i + 1 << endl;
    }

    Info<< nl << "End" << nl << endl;

    return 0;
}


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