/******************************************************************************
*                 SOFA, Simulation Open-Framework Architecture                *
*                    (c) 2006 INRIA, USTL, UJF, CNRS, MGH                     *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU Lesser General Public License as published by    *
* the Free Software Foundation; either version 2.1 of the License, or (at     *
* your option) any later version.                                             *
*                                                                             *
* This program 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 Lesser General Public License *
* for more details.                                                           *
*                                                                             *
* You should have received a copy of the GNU Lesser General Public License    *
* along with this program. If not, see <http://www.gnu.org/licenses/>.        *
*******************************************************************************
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#pragma once
#include <sofa/component/topology/container/dynamic/HexahedronSetGeometryAlgorithms.h>
#include <sofa/core/visual/VisualParams.h>
#include <sofa/component/topology/container/dynamic/CommonAlgorithms.h>
#include <sofa/core/topology/Topology.h>
#include <fstream>

namespace sofa::component::topology::container::dynamic
{

using sofa::core::topology::verticesInHexahedronArray;
template< class DataTypes>
NumericalIntegrationDescriptor<typename HexahedronSetGeometryAlgorithms< DataTypes >::Real,3> &HexahedronSetGeometryAlgorithms< DataTypes >::getHexahedronNumericalIntegrationDescriptor()
{
    // initialize the cubature table only if needed.
    if (initializedHexahedronCubatureTables==false) {
        initializedHexahedronCubatureTables=true;
        defineHexahedronCubaturePoints();
    }
    return hexahedronNumericalIntegration;
}

template< class DataTypes>
void HexahedronSetGeometryAlgorithms< DataTypes >::defineHexahedronCubaturePoints() {
    typedef typename NumericalIntegrationDescriptor<typename HexahedronSetGeometryAlgorithms< DataTypes >::Real,3>::QuadraturePoint QuadraturePoint;
    typedef typename NumericalIntegrationDescriptor<typename HexahedronSetGeometryAlgorithms< DataTypes >::Real,3>::BarycentricCoordinatesType BarycentricCoordinatesType;
    // Gauss method
    typename NumericalIntegrationDescriptor<typename HexahedronSetGeometryAlgorithms< DataTypes >::Real,3>::QuadratureMethod m=NumericalIntegrationDescriptor<typename HexahedronSetGeometryAlgorithms< DataTypes >::Real,3>::GAUSS_LEGENDRE_METHOD;
    typename NumericalIntegrationDescriptor<typename HexahedronSetGeometryAlgorithms< DataTypes >::Real,3>::QuadraturePointArray qpa;
    typename NumericalIntegrationDescriptor<typename EdgeSetGeometryAlgorithms< DataTypes >::Real,1>::QuadraturePointArray qpa1D;

    BarycentricCoordinatesType v;
    Real w;


    NumericalIntegrationDescriptor<typename EdgeSetGeometryAlgorithms< DataTypes >::Real,1> &nide=this->getEdgeNumericalIntegrationDescriptor();

    /// create gauss points as tensor product of Gauss Legendre points in 1D
    /// create integration method up to order 8 (could go up to 12 if needed) where the number of gauss points is the cube of the number of 1D Gauss points
    size_t o,i,j,k;
    for (o=1;o<8;++o) {
        qpa.clear();
        qpa1D=nide.getQuadratureMethod(NumericalIntegrationDescriptor<typename EdgeSetGeometryAlgorithms< DataTypes >::Real,1>::GAUSS_LEGENDRE_METHOD,o);
        for (i=0;i<qpa1D.size();++i) {
            for (j=0;j<qpa1D.size();++j) {
                for (k=0;k<qpa1D.size();++k) {
                    v=BarycentricCoordinatesType(qpa1D[i].first[0],qpa1D[j].first[0],qpa1D[k].first[0]);
                    w=qpa1D[i].second*qpa1D[j].second*qpa1D[k].second;
                    qpa.push_back(QuadraturePoint(v,(Real)w));
                }
            }
        }
        hexahedronNumericalIntegration.addQuadratureMethod(m,o,qpa);
    }
    /*
    /// consider non tensor product rules : taken from getfem++ file Hexahedron_5.im
     m=NumericalIntegrationDescriptor<typename HexahedronSetGeometryAlgorithms< DataTypes >::Real,3>::GAUSS_CUBE_METHOD;
    /// integration with  accuracy of order 5 with 14 gauss points.
    Real a=0.8979112128771107316322744102380675;
    Real b=0.5;
    w=0.1108033240997229916897506925207755;
    for (i=0;i<3;++i) {
        v=BarycentricCoordinatesType(b,b,b);
        v[i]=a;
        qpa.push_back(QuadraturePoint(v,w));
        v[i]=1-a;
        qpa.push_back(QuadraturePoint(v,w));
    }
    a=0.8793934553196640731345171390561335;
    w=0.0418975069252077562326869806094182;
    for (i=0;i<2;++i) {
        for (j=0;j<2;++j) {
            for (k=0;k<2;++k) {
                /// barycentric coordinates are either a or 1-a
                v=BarycentricCoordinatesType(a+i*(1-2*a),a+j*(1-2*a),a+k*(1-2*a));
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    hexahedronNumericalIntegration.addQuadratureMethod(m,5,qpa);


    /// consider non tensor product rules : taken from getfem++ file Hexahedron_9.im
    /// integration with  accuracy of order 9 with 58 gauss points.

    /// 6 points
    a=.8068407347958544969174424448702780;
    b=0.5;
    w=.0541593744687068178762288491492902;
    for (i=0;i<3;++i) {
        v=BarycentricCoordinatesType(b,b,b);
        v[i]=a;
        qpa.push_back(QuadraturePoint(v,w));
        v[i]=1-a;
        qpa.push_back(QuadraturePoint(v,w));
    }
    // 12 points
    a=0.9388435616288391432433878794971660;
    b=0.5;
    w=0.0114737257670222052714055736149557;
    for (i=0;i<3;++i) {
        v=BarycentricCoordinatesType(b,b,b);
        for (j=0;j<2;++j) {
            v[(i+1)%3]=a+j*(1-2*a);
            for (k=0;k<2;++k) {
                v[(i+2)%3]=a+k*(1-2*a);
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    /// 8 points
    a=0.7820554035100150271333094993315360;
    w=0.0248574797680029375401085898232011;
    for (i=0;i<2;++i) {
        for (j=0;j<2;++j) {
            for (k=0;k<2;++k) {
                /// barycentric coordinates are either a or 1-a
                v=BarycentricCoordinatesType(a+i*(1-2*a),a+j*(1-2*a),a+k*(1-2*a));
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    /// 8 points
    a=0.9350498923309879588075319044319620;
    w=0.0062685994124186287334314359655827;
    for (i=0;i<2;++i) {
        for (j=0;j<2;++j) {
            for (k=0;k<2;++k) {
                /// barycentric coordinates are either a or 1-a
                v=BarycentricCoordinatesType(a+i*(1-2*a),a+j*(1-2*a),a+k*(1-2*a));
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    /// 24 points
    a=0.7161339513154310822080124307584715;
    b=0.9692652109323358726644884348015390;
    w=0.0120146004391716708040599923089382;
    for (i=0;i<3;++i) {
        v=BarycentricCoordinatesType(b,b,b);
        for (j=0;j<2;++j) {
            v[(i+1)%3]=a+j*(1-2*a);
            for (k=0;k<2;++k) {
                v[i]=b;
                v[(i+2)%3]=a+k*(1-2*a);
                qpa.push_back(QuadraturePoint(v,w));
                v[i]=1-b;
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    hexahedronNumericalIntegration.addQuadratureMethod(m,9,qpa);


    /// consider non tensor product rules : taken from getfem++ file Hexahedron_11.im
    /// integration with  accuracy of order 11 with 90 gauss points.

    /// 6 points
    a=.9063071670498132481961877986898720;
    b=0.5;
    w=.0253096342016000238231671413708773;
    for (i=0;i<3;++i) {
        v=BarycentricCoordinatesType(b,b,b);
        v[i]=a;
        qpa.push_back(QuadraturePoint(v,w));
        v[i]=1-a;
        qpa.push_back(QuadraturePoint(v,w));
    }
    // 12 points
    a=.8673341434985040086731923849337745;
    b=0.5;
    w=0.0181499182325144622865632250992823;
    for (i=0;i<3;++i) {
        v=BarycentricCoordinatesType(b,b,b);
        for (j=0;j<2;++j) {
            v[(i+1)%3]=a+j*(1-2*a);
            for (k=0;k<2;++k) {
                v[(i+2)%3]=a+k*(1-2*a);
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    /// 8 points
    a=.6566967022580273605228866152789755;
    w=.0269990056568711411641833332980551;
    for (i=0;i<2;++i) {
        for (j=0;j<2;++j) {
            for (k=0;k<2;++k) {
                /// barycentric coordinates are either a or 1-a
                v=BarycentricCoordinatesType(a+i*(1-2*a),a+j*(1-2*a),a+k*(1-2*a));
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    /// 8 points
    a=.8008376320991313508172065028926585;
    w= .0146922934945570350487414755013352;
    for (i=0;i<2;++i) {
        for (j=0;j<2;++j) {
            for (k=0;k<2;++k) {
                /// barycentric coordinates are either a or 1-a
                v=BarycentricCoordinatesType(a+i*(1-2*a),a+j*(1-2*a),a+k*(1-2*a));
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
        /// 8 points
    a=.9277278805088799923375457353451730;
    w= .0055804890098536552051251442852662;
    for (i=0;i<2;++i) {
        for (j=0;j<2;++j) {
            for (k=0;k<2;++k) {
                /// barycentric coordinates are either a or 1-a
                v=BarycentricCoordinatesType(a+i*(1-2*a),a+j*(1-2*a),a+k*(1-2*a));
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    /// 24 points
    a=.9706224286053016319555750788155670;
    b=.6769514072983150674551564354064455;
    w=.0028267870173527355278995288336230;
    for (i=0;i<3;++i) {
        v=BarycentricCoordinatesType(b,b,b);
        for (j=0;j<2;++j) {
            v[(i+1)%3]=a+j*(1-2*a);
            for (k=0;k<2;++k) {
                v[i]=b;
                v[(i+2)%3]=a+k*(1-2*a);
                qpa.push_back(QuadraturePoint(v,w));
                v[i]=1-b;
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    /// 24 points
    a=.7253999675572547151889421728651345;
    b=.9825498327563551314651409115626720;
    w=.0076802492622294169003437555791307;
    for (i=0;i<3;++i) {
        v=BarycentricCoordinatesType(b,b,b);
        for (j=0;j<2;++j) {
            v[(i+1)%3]=a+j*(1-2*a);
            for (k=0;k<2;++k) {
                v[i]=b;
                v[(i+2)%3]=a+k*(1-2*a);
                qpa.push_back(QuadraturePoint(v,w));
                v[i]=1-b;
                qpa.push_back(QuadraturePoint(v,w));
            }
        }
    }
    hexahedronNumericalIntegration.addQuadratureMethod(m,11,qpa);
    */

}
template< class DataTypes>
bool HexahedronSetGeometryAlgorithms< DataTypes >::isHexahedronAffine(const HexaID hx, const VecCoord& p, const Real tolerance) const
{
    /// check that the hexahedron is a parallelepiped returns true if it is the case and false otherwise.
    /// given 4 points of binary coordinates 000 010 100 001 checks that the 4 other points are translated versions
    const Hexahedron &h = this->m_topology->getHexahedron(hx);
    Coord dpos;
    dpos=(p[h[verticesInHexahedronArray[1][0][1]]]-p[h[verticesInHexahedronArray[1][0][0]]])-(p[h[verticesInHexahedronArray[0][0][1]]]-p[h[verticesInHexahedronArray[0][0][0]]]);
    if (dpos.norm()>tolerance)
        return false;
    else {
        dpos=(p[h[verticesInHexahedronArray[1][1][0]]]-p[h[verticesInHexahedronArray[1][0][0]]])-(p[h[verticesInHexahedronArray[0][1][0]]]-p[h[verticesInHexahedronArray[0][0][0]]]);
        if (dpos.norm()>tolerance)
            return false;
        else {
            dpos=(p[h[verticesInHexahedronArray[1][0][1]]]-p[h[verticesInHexahedronArray[0][0][1]]])-(p[h[verticesInHexahedronArray[0][0][1]]]-p[h[verticesInHexahedronArray[0][0][0]]]);
            if (dpos.norm()>tolerance)
                return false;
            else {
                dpos=(p[h[verticesInHexahedronArray[1][1][1]]]-p[h[verticesInHexahedronArray[0][1][1]]])-(p[h[verticesInHexahedronArray[1][0][1]]]-p[h[verticesInHexahedronArray[0][0][1]]]);
                if (dpos.norm()>tolerance)
                    return false;
                else
                    return true;
            }
        }
    }
}
template< class DataTypes>
typename  DataTypes::Real HexahedronSetGeometryAlgorithms< DataTypes >::computeShapeFunction(const LocalCoord nc,const HexahedronBinaryIndex bi) const 
{
    return((bi[0] ? nc[0] : 1-nc[0])*(bi[1] ? nc[1] : 1-nc[1])*(bi[2] ? nc[2] : 1-nc[2]));
}
template< class DataTypes>
typename DataTypes::Coord HexahedronSetGeometryAlgorithms< DataTypes >::computeNodalValue(const HexaID hx,const LocalCoord nc,const VecCoord& p) const
{
     const Hexahedron &h = this->m_topology->getHexahedron(hx);
     sofa::Index i,j,k;
     Coord pos[8];
     for (i=0;i<8;++i)
         pos[i]=p[h[i]];
     Coord res;

     for (i=0;i<2;++i) {
         for (j=0;j<2;++j) {
             for (k=0;k<2;++k) {
                 res+= (i ? nc[0] : 1-nc[0])*(j ? nc[1] : 1-nc[1])*(k ? nc[2] : 1-nc[2])*pos[h[verticesInHexahedronArray[i][j][k]]];
             }
         }
     }
/*
    const Coord pos = p[0] * ((1-fx) * (1-fy) * (1-fz))
            + p[1] * ((  fx) * (1-fy) * (1-fz))
            + p[3] * ((1-fx) * (  fy) * (1-fz))
            + p[2] * ((  fx) * (  fy) * (1-fz))
            + p[4] * ((1-fx) * (1-fy) * (  fz))
            + p[5] * ((  fx) * (1-fy) * (  fz))
            + p[7] * ((1-fx) * (  fy) * (  fz))
            + p[6] * ((  fx) * (  fy) * (  fz)); */

    return res;
}

template< class DataTypes>
void HexahedronSetGeometryAlgorithms< DataTypes >::computePositionDerivative(const HexaID hx,const LocalCoord nc,const VecCoord& p,  Coord dpos[3]) const
{
     const Hexahedron &h = this->m_topology->getHexahedron(hx);
     sofa::Index i,j,k;
     sofa::Index ind[3];

     Coord pos[8];
     for (i=0;i<8;++i)
         pos[i]=p[h[i]];

     for (i=0;i<3;++i) {
         Coord pos0,pos1;
         for (j=0;j<2;++j) {
             for (k=0;k<2;++k) {
                 ind[i]=1;
                 ind[(i+1)%3]=j;
                 ind[(i+2)%3]=k;
                 pos1+= (j ? nc[(i+1)%3] : 1-nc[(i+1)%3])*(k ? nc[(i+2)%3] : 1-nc[(i+2)%3])*p[h[verticesInHexahedronArray[ind[0]][ind[1]][ind[2]]]];
                 ind[i]=0;
                 pos0+= (j ? nc[(i+1)%3] : 1-nc[(i+1)%3])*(k ? nc[(i+2)%3] : 1-nc[(i+2)%3])*p[h[verticesInHexahedronArray[ind[0]][ind[1]][ind[2]]]];
             }
         }
         dpos[i]=pos1-pos0;
     }

}

template< class DataTypes>
typename DataTypes::Real HexahedronSetGeometryAlgorithms< DataTypes >::computeJacobian(const HexaID hx,const LocalCoord nc,const VecCoord& p) const
{
    Coord dpos[3];
    this->computePositionDerivative(hx,nc,p,dpos);
    return (tripleProduct(dpos[0],dpos[1],dpos[2]));
}

template< class DataTypes>
void HexahedronSetGeometryAlgorithms< DataTypes >::computeHexahedronAABB(const HexaID h, Coord& minCoord, Coord& maxCoord) const
{
    const Hexahedron &t = this->m_topology->getHexahedron(h);
    const typename DataTypes::VecCoord& p =(this->object->read(core::vec_id::read_access::position)->getValue());

    for(unsigned int i=0; i<3; ++i)
    {
        minCoord[i] = std::min( std::min(std::min(p[t[0]][i], p[t[1]][i]), std::min(p[t[2]][i], p[t[3]][i])),
                std::min(std::min(p[t[4]][i], p[t[5]][i]), std::min(p[t[6]][i], p[t[7]][i])));

        maxCoord[i] = std::max( std::max(std::max(p[t[0]][i], p[t[1]][i]), std::max(p[t[2]][i], p[t[3]][i])),
                std::max(std::max(p[t[4]][i], p[t[5]][i]), std::max(p[t[6]][i], p[t[7]][i])));
    }
}

template< class DataTypes>
void HexahedronSetGeometryAlgorithms< DataTypes >::computeHexahedronRestAABB(const HexaID h, Coord& minCoord, Coord& maxCoord) const
{
    const Hexahedron &t = this->m_topology->getHexahedron(h);
    const typename DataTypes::VecCoord& p = (this->object->read(core::vec_id::read_access::restPosition)->getValue());

    for(unsigned int i=0; i<3; ++i)
    {
        minCoord[i] = std::min( std::min(std::min(p[t[0]][i], p[t[1]][i]), std::min(p[t[2]][i], p[t[3]][i])),
                std::min(std::min(p[t[4]][i], p[t[5]][i]), std::min(p[t[6]][i], p[t[7]][i])));

        maxCoord[i] = std::max( std::max(std::max(p[t[0]][i], p[t[1]][i]), std::max(p[t[2]][i], p[t[3]][i])),
                std::max(std::max(p[t[4]][i], p[t[5]][i]), std::max(p[t[6]][i], p[t[7]][i])));
    }
}

template<class DataTypes>
typename DataTypes::Coord HexahedronSetGeometryAlgorithms<DataTypes>::computeHexahedronCenter(const HexaID h) const
{
    const Hexahedron &t = this->m_topology->getHexahedron(h);
    const typename DataTypes::VecCoord& p =(this->object->read(core::vec_id::read_access::position)->getValue());

    return (p[t[0]] + p[t[1]] + p[t[2]] + p[t[3]] + p[t[4]] + p[t[5]] + p[t[6]] + p[t[7]]) * (Real) 0.125;
}

template<class DataTypes>
typename DataTypes::Coord HexahedronSetGeometryAlgorithms<DataTypes>::computeHexahedronRestCenter(const HexaID h) const
{
    const Hexahedron &t = this->m_topology->getHexahedron(h);
    const typename DataTypes::VecCoord& p = (this->object->read(core::vec_id::read_access::restPosition)->getValue());

    return (p[t[0]] + p[t[1]] + p[t[2]] + p[t[3]] + p[t[4]] + p[t[5]] + p[t[6]] + p[t[7]]) * (Real) 0.125;
}

template< class DataTypes>
void HexahedronSetGeometryAlgorithms< DataTypes >::getHexahedronVertexCoordinates(const HexaID h, Coord pnt[8]) const
{
    const Hexahedron &t = this->m_topology->getHexahedron(h);
    const typename DataTypes::VecCoord& p =(this->object->read(core::vec_id::read_access::position)->getValue());

    for(unsigned int i=0; i<8; ++i)
    {
        pnt[i] = p[t[i]];
    }
}

template< class DataTypes>
void HexahedronSetGeometryAlgorithms< DataTypes >::getRestHexahedronVertexCoordinates(const HexaID h, Coord pnt[8]) const
{
    const Hexahedron &t = this->m_topology->getHexahedron(h);
    const typename DataTypes::VecCoord& p = (this->object->read(core::vec_id::read_access::restPosition)->getValue());

    for(unsigned int i=0; i<8; ++i)
    {
        pnt[i] = p[t[i]];
    }
}

template<class DataTypes>
typename DataTypes::Coord HexahedronSetGeometryAlgorithms<DataTypes>::getRestPointPositionInHexahedron(const HexaID h,
        const Real baryC[3]) const
{
    Coord	p[8];
    getRestHexahedronVertexCoordinates(h, p);

    const Real &fx = baryC[0];
    const Real &fy = baryC[1];
    const Real &fz = baryC[2];

    const Coord pos = p[0] * ((1-fx) * (1-fy) * (1-fz))
            + p[1] * ((  fx) * (1-fy) * (1-fz))
            + p[3] * ((1-fx) * (  fy) * (1-fz))
            + p[2] * ((  fx) * (  fy) * (1-fz))
            + p[4] * ((1-fx) * (1-fy) * (  fz))
            + p[5] * ((  fx) * (1-fy) * (  fz))
            + p[7] * ((1-fx) * (  fy) * (  fz))
            + p[6] * ((  fx) * (  fy) * (  fz));

    return pos;
}

template<class DataTypes>
typename DataTypes::Coord HexahedronSetGeometryAlgorithms<DataTypes>::getRestPointPositionInHexahedron(const HexaID h,
        const sofa::type::Vec3& baryC) const
{
    Coord	p[8];
    getRestHexahedronVertexCoordinates(h, p);

    const Real fx = (Real) baryC[0];
    const Real fy = (Real) baryC[1];
    const Real fz = (Real) baryC[2];

    const Coord pos = p[0] * ((1-fx) * (1-fy) * (1-fz))
            + p[1] * ((  fx) * (1-fy) * (1-fz))
            + p[3] * ((1-fx) * (  fy) * (1-fz))
            + p[2] * ((  fx) * (  fy) * (1-fz))
            + p[4] * ((1-fx) * (1-fy) * (  fz))
            + p[5] * ((  fx) * (1-fy) * (  fz))
            + p[7] * ((1-fx) * (  fy) * (  fz))
            + p[6] * ((  fx) * (  fy) * (  fz));

    return pos;
}

template<class DataTypes>
typename DataTypes::Coord HexahedronSetGeometryAlgorithms<DataTypes>::getPointPositionInHexahedron(const HexaID h,
        const Real baryC[3]) const
{
    Coord	p[8];
    getHexahedronVertexCoordinates(h, p);

    const Real &fx = baryC[0];
    const Real &fy = baryC[1];
    const Real &fz = baryC[2];

    const Coord pos = p[0] * ((1-fx) * (1-fy) * (1-fz))
            + p[1] * ((  fx) * (1-fy) * (1-fz))
            + p[3] * ((1-fx) * (  fy) * (1-fz))
            + p[2] * ((  fx) * (  fy) * (1-fz))
            + p[4] * ((1-fx) * (1-fy) * (  fz))
            + p[5] * ((  fx) * (1-fy) * (  fz))
            + p[7] * ((1-fx) * (  fy) * (  fz))
            + p[6] * ((  fx) * (  fy) * (  fz));

    return pos;
}

template<class DataTypes>
typename DataTypes::Coord HexahedronSetGeometryAlgorithms<DataTypes>::getPointPositionInHexahedron(const HexaID h,
        const sofa::type::Vec3& baryC) const
{
    Coord	p[8];
    getHexahedronVertexCoordinates(h, p);

    const Real fx = (Real) baryC[0];
    const Real fy = (Real) baryC[1];
    const Real fz = (Real) baryC[2];

    const Coord pos = p[0] * ((1-fx) * (1-fy) * (1-fz))
            + p[1] * ((  fx) * (1-fy) * (1-fz))
            + p[3] * ((1-fx) * (  fy) * (1-fz))
            + p[2] * ((  fx) * (  fy) * (1-fz))
            + p[4] * ((1-fx) * (1-fy) * (  fz))
            + p[5] * ((  fx) * (1-fy) * (  fz))
            + p[7] * ((1-fx) * (  fy) * (  fz))
            + p[6] * ((  fx) * (  fy) * (  fz));

    return pos;
}

template<class DataTypes>
sofa::type::Vec3 HexahedronSetGeometryAlgorithms<DataTypes>::computeHexahedronRestBarycentricCoeficients(const HexaID h,
        const Coord& pos) const
{
    Coord	p[8];
    getRestHexahedronVertexCoordinates(h, p);

    const unsigned int max_spatial_dimensions = std::min((unsigned int)3,(unsigned int)DataTypes::spatial_dimensions);

    sofa::type::Vec3 origin, p1, p3, p4, pnt;
    for( unsigned int w=0 ; w<max_spatial_dimensions ; ++w )
    {
        origin[w] = p[0][w];
        p1[w] = p[1][w];
        p3[w] = p[3][w];
        p4[w] = p[4][w];
        pnt[w] = pos[w];
    }

    sofa::type::Mat3x3d		m, mt, base;
    m[0] = p1-origin;
    m[1] = p3-origin;
    m[2] = p4-origin;
    mt.transpose(m);
    const bool canInvert = base.invert(mt);
    assert(canInvert);
    SOFA_UNUSED(canInvert);

    return base * (pnt - origin);
}

template<class DataTypes>
sofa::type::Vec3 HexahedronSetGeometryAlgorithms<DataTypes>::computeHexahedronBarycentricCoeficients(const HexaID h,
        const Coord& pos) const
{
    // Warning: this is only correct if the hexahedron is not deformed
    // as only 3 perpendicular edges are considered as a base
    // other edges are assumed to be parallel to the respective base edge (and have the same length)

    Coord	p[8];
    getHexahedronVertexCoordinates(h, p);

    const unsigned int max_spatial_dimensions = std::min((unsigned int)3,(unsigned int)DataTypes::spatial_dimensions);

    sofa::type::Vec3 origin, p1, p3, p4, pnt;
    for( unsigned int w=0 ; w<max_spatial_dimensions ; ++w )
    {
        origin[w] = p[0][w];
        p1[w] = p[1][w];
        p3[w] = p[3][w];
        p4[w] = p[4][w];
        pnt[w] = pos[w];
    }

    sofa::type::Mat3x3d		m, mt, base;
    m[0] = p1-origin;
    m[1] = p3-origin;
    m[2] = p4-origin;
    mt.transpose(m);
    const bool canInvert = base.invert(mt);
    assert(canInvert);
    SOFA_UNUSED(canInvert);

    return base * (pnt - origin);
}

template< class DataTypes>
typename DataTypes::Real HexahedronSetGeometryAlgorithms< DataTypes >::computeElementDistanceMeasure(const HexaID h, const Coord pos) const
{
    typedef typename DataTypes::Real Real;

    const sofa::type::Vec3 v = computeHexahedronBarycentricCoeficients(h, pos);

    Real d = (Real) std::max(std::max(-v[0], -v[1]), std::max(std::max(-v[2], v[0]-1), std::max(v[1]-1, v[2]-1)));

    if(d>0)
        d = (pos - computeHexahedronCenter(h)).norm2();

    return d;
}

template< class DataTypes>
typename DataTypes::Real HexahedronSetGeometryAlgorithms< DataTypes >::computeElementRestDistanceMeasure(const HexaID h, const Coord pos) const
{
    typedef typename DataTypes::Real Real;

    const sofa::type::Vec3 v = computeHexahedronRestBarycentricCoeficients(h, pos);

    Real d = (Real) std::max(std::max(-v[0], -v[1]), std::max(std::max(-v[2], v[0]-1), std::max(v[1]-1, v[2]-1)));

    if(d>0)
        d = (pos - computeHexahedronRestCenter(h)).norm2();

    return d;
}

template< class DataTypes>
int HexahedronSetGeometryAlgorithms< DataTypes >::findNearestElement(const Coord& pos, sofa::type::Vec3& baryC, Real& distance) const
{
    sofa::Index index = sofa::InvalidID;
    distance = std::numeric_limits<Real>::max();

    for(sofa::Index c=0; c<this->m_topology->getNbHexahedra(); ++c)
    {
        const Real d = computeElementDistanceMeasure(c, pos);

        if(d<distance)
        {
            distance = d;
            index = c;
        }
    }

    if(index != sofa::InvalidID)
        baryC = computeHexahedronBarycentricCoeficients(index, pos);

    return index;
}

template< class DataTypes>
void HexahedronSetGeometryAlgorithms< DataTypes >::findNearestElements(const VecCoord& pos,
        type::vector<int>& elem,
        type::vector<type::Vec3>& baryC,
        type::vector<Real>& dist) const
{
    for(size_t i=0; i<pos.size(); ++i)
    {
        elem[i] = findNearestElement(pos[i], baryC[i], dist[i]);
    }
}

template< class DataTypes>
int HexahedronSetGeometryAlgorithms< DataTypes >::findNearestElementInRestPos(const Coord& pos, sofa::type::Vec3& baryC, Real& distance) const
{
    sofa::Index index = sofa::InvalidID;
    distance = std::numeric_limits<Real>::max();

    for(sofa::Index c=0; c<this->m_topology->getNbHexahedra(); ++c)
    {
        const Real d = computeElementRestDistanceMeasure(c, pos);

        if(d<distance)
        {
            distance = d;
            index = c;
        }
    }

    if(index != sofa::InvalidID)
        baryC = computeHexahedronRestBarycentricCoeficients(index, pos);

    return index;
}

template< class DataTypes>
void HexahedronSetGeometryAlgorithms< DataTypes >::findNearestElementsInRestPos( const VecCoord& pos, type::vector<int>& elem, type::vector<type::Vec3>& baryC, type::vector<Real>& dist) const
{
    for(size_t i=0; i<pos.size(); ++i)
    {
        elem[i] = findNearestElementInRestPos(pos[i], baryC[i], dist[i]);
    }
}

template< class DataTypes>
typename DataTypes::Real HexahedronSetGeometryAlgorithms< DataTypes >::computeHexahedronVolume( const HexaID hexa) const
{
 const Hexahedron &h = this->m_topology->getHexahedron(hexa);
    const VecCoord& p = (this->object->read(core::vec_id::read_access::position)->getValue());
    Coord dp[3];
    unsigned char i,j,k,ind[3];
    Real volume;
    for (i=0;i<3;++i) {
        dp[i]=Coord();
        for (j=0;j<2;++j) {
            for (k=0;k<2;++k) {
                ind[i]=1;
                ind[(i+1)%3]=j;
                ind[(i+2)%3]=k;
                dp[i]+=p[h[verticesInHexahedronArray[ind[0]][ind[1]][ind[2]]]];
                ind[i]=0;
                dp[i]-=p[h[verticesInHexahedronArray[ind[0]][ind[1]][ind[2]]]];
            }
        }
    }
    volume=tripleProduct(dp[0],dp[1],dp[2])/48.0f;
    dp[0]=p[h[verticesInHexahedronArray[0][1][1]]]-p[h[verticesInHexahedronArray[0][0][0]]];
    dp[1]=p[h[verticesInHexahedronArray[1][0][1]]]-p[h[verticesInHexahedronArray[0][0][0]]];
    dp[2]=p[h[verticesInHexahedronArray[1][1][0]]]-p[h[verticesInHexahedronArray[0][0][0]]];
    volume-=tripleProduct(dp[0],dp[1],dp[2])/12.0f;
    dp[0]=p[h[verticesInHexahedronArray[1][0][0]]]-p[h[verticesInHexahedronArray[1][1][1]]];
    dp[1]=p[h[verticesInHexahedronArray[0][1][0]]]-p[h[verticesInHexahedronArray[1][1][1]]];
    dp[2]=p[h[verticesInHexahedronArray[0][0][1]]]-p[h[verticesInHexahedronArray[1][1][1]]];
    volume+=tripleProduct(dp[0],dp[1],dp[2])/12.0f;
    return volume;
}

template< class DataTypes>
typename DataTypes::Real HexahedronSetGeometryAlgorithms< DataTypes >::computeRestHexahedronVolume( const HexaID hexa) const
{
    const Hexahedron &h = this->m_topology->getHexahedron(hexa);
    const VecCoord& p =  (this->object->read(core::vec_id::read_access::restPosition)->getValue());
    Coord dp[3];
    size_t i,j,k,ind[3];
    Real volume;
    for (i=0;i<3;++i) {
        dp[i]=Coord();
        for (j=0;j<2;++j) {
            for (k=0;k<2;++k) {
                ind[i]=1;
                ind[(i+1)%3]=j;
                ind[(i+2)%3]=k;
                dp[i]+=p[h[verticesInHexahedronArray[ind[0]][ind[1]][ind[2]]]];
                ind[i]=0;
                dp[i]-=p[h[verticesInHexahedronArray[ind[0]][ind[1]][ind[2]]]];
            }
        }
    }
    volume=tripleProduct(dp[0],dp[1],dp[2])/48.0f;
    dp[0]=p[h[verticesInHexahedronArray[0][1][1]]]-p[h[verticesInHexahedronArray[0][0][0]]];
    dp[1]=p[h[verticesInHexahedronArray[1][0][1]]]-p[h[verticesInHexahedronArray[0][0][0]]];
    dp[2]=p[h[verticesInHexahedronArray[1][1][0]]]-p[h[verticesInHexahedronArray[0][0][0]]];
    volume-=tripleProduct(dp[0],dp[1],dp[2])/12.0f;
    dp[0]=p[h[verticesInHexahedronArray[1][0][0]]]-p[h[verticesInHexahedronArray[1][1][1]]];
    dp[1]=p[h[verticesInHexahedronArray[0][1][0]]]-p[h[verticesInHexahedronArray[1][1][1]]];
    dp[2]=p[h[verticesInHexahedronArray[0][0][1]]]-p[h[verticesInHexahedronArray[1][1][1]]];
    volume+=tripleProduct(dp[0],dp[1],dp[2])/12.0f;
    return volume;
}

template<class DataTypes>
void HexahedronSetGeometryAlgorithms<DataTypes>::computeHexahedronVolume( BasicArrayInterface<Real> &ai) const
{
    //const sofa::type::vector<Hexahedron> &ta=this->m_topology->getHexahedra();
    //const typename DataTypes::VecCoord& p =(this->object->read(core::vec_id::read_access::position)->getValue());
    for(sofa::Index i=0; i<this->m_topology->getNbHexahedra(); ++i)
    {
        //const Hexahedron &t=this->m_topology->getHexahedron(i); //ta[i];
        ai[i]=Real(0.0); /// @todo : implementation of computeHexahedronVolume
    }
}


template<class DataTypes>
void HexahedronSetGeometryAlgorithms<DataTypes>::draw(const core::visual::VisualParams* vparams)
{
    QuadSetGeometryAlgorithms<DataTypes>::draw(vparams);

    const auto stateLifeCycle = vparams->drawTool()->makeStateLifeCycle();
    vparams->drawTool()->disableLighting();

    // Draw Hexa indices
    if (d_showHexaIndices.getValue() && this->m_topology->getNbHexahedra() != 0)
    {
        const VecCoord& coords =(this->object->read(core::vec_id::read_access::position)->getValue());
        float scale = this->getIndicesScale();

        //for hexa:
        scale = scale/2;

        const sofa::type::vector<Hexahedron> &hexaArray = this->m_topology->getHexahedra();

        std::vector<type::Vec3> positions;
        for (size_t i =0; i<hexaArray.size(); i++)
        {

            Hexahedron the_hexa = hexaArray[i];
            sofa::type::Vec3f center;

            for (unsigned int j = 0; j<8; j++)
            {
                type::Vec3 vertex; vertex = DataTypes::getCPos(coords[ the_hexa[j] ]);
                center += vertex;
            }

            center = center/8;
            positions.push_back(center);
        }

        vparams->drawTool()->draw3DText_Indices(positions, scale, d_drawColorHexahedra.getValue());
    }


    //Draw hexahedra
    if (d_drawHexahedra.getValue() && this->m_topology->getNbHexahedra() != 0)
    {
        if (vparams->displayFlags().getShowWireFrame())
            vparams->drawTool()->setPolygonMode(0, true);

        const sofa::type::vector<Hexahedron> &hexaArray = this->m_topology->getHexahedra();

        const VecCoord& coords =(this->object->read(core::vec_id::read_access::position)->getValue());
        sofa::type::vector<sofa::type::Vec3> hexaCoords;

        for (size_t i = 0; i<hexaArray.size(); i++)
        {
            const Hexahedron& H = hexaArray[i];

            for (unsigned int j = 0; j<8; j++)
            {
                sofa::type::Vec3 p; p = DataTypes::getCPos(coords[H[j]]);

                hexaCoords.push_back(p);
            }
        }

        const float& scale = d_drawScaleHexahedra.getValue();

        if(scale >= 1.0 && scale < 0.001)
            vparams->drawTool()->drawHexahedra(hexaCoords, d_drawColorHexahedra.getValue());
        else
            vparams->drawTool()->drawScaledHexahedra(hexaCoords, d_drawColorHexahedra.getValue(), scale);

        if (vparams->displayFlags().getShowWireFrame())
            vparams->drawTool()->setPolygonMode(0, false);
    }


}

template <class DataTypes>
bool HexahedronSetGeometryAlgorithms<DataTypes>::mustComputeBBox() const
{
    return ((d_showHexaIndices.getValue() || d_drawHexahedra.getValue()) && this->m_topology->getNbHexahedra() != 0) || Inherit1::mustComputeBBox();
}

} //namespace sofa::component::topology::container::dynamic
