/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, development version     *
*                (c) 2006-2017 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                             *
******************************************************************************/
#ifndef LINEARSUBSPACE_LSJacobsonShapeFunction_H
#define LINEARSUBSPACE_LSJacobsonShapeFunction_H

#include "LinearSubspace/config.h"
//#include <Flexible/shapeFunction/BaseShapeFunction.h>
#include "BaseLSShapeFunction.h"

#include <sofa/core/topology/BaseMeshTopology.h>

#include <sofa/helper/rmath.h>
#include <sofa/helper/kdTree.h>
#include <sofa/helper/OptionsGroup.h>

#include <Eigen/Core>
#include <Eigen/Dense>

#include <limits>
#include <algorithm>

#define LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE true


namespace Eigen {

template <typename RealT>
std::istream& operator>>( std::istream& in, Eigen::Matrix<RealT, Eigen::Dynamic, Eigen::Dynamic>& /*c*/ )
{
    msg_warning("Eigen::Matrix<RealT, Eigen::Dynamic, Eigen::Dynamic>>") << "Eigen::Matrix<RealT, Eigen::Dynamic, Eigen::Dynamic> is not yet readable";
    std::string bidon;
    in >> bidon;
    return in;
}
}

namespace sofa
{
namespace component
{
namespace shapefunction
{


template <class TLSShapeFunctionTypes>
class LSJacobsonShapeFunction : public core::behavior::BaseLSShapeFunction<TLSShapeFunctionTypes>
{
public:
    SOFA_CLASS(SOFA_TEMPLATE(LSJacobsonShapeFunction, TLSShapeFunctionTypes) , SOFA_TEMPLATE(core::behavior::BaseLSShapeFunction, TLSShapeFunctionTypes));
    typedef core::behavior::BaseLSShapeFunction<TLSShapeFunctionTypes> Inherit;

    typedef typename TLSShapeFunctionTypes::Real Real;
    typedef typename TLSShapeFunctionTypes::Coord Coord;
    typedef typename TLSShapeFunctionTypes::VCoord VCoord;
    enum {spatial_dimensions=TLSShapeFunctionTypes::spatial_dimensions};

    typedef typename TLSShapeFunctionTypes::VRef VRef;

    typedef typename TLSShapeFunctionTypes::Weight Weight;
    typedef typename TLSShapeFunctionTypes::VWeight VWeight;
    typedef typename TLSShapeFunctionTypes::VecVWeight VecVWeight;

    typedef typename TLSShapeFunctionTypes::AffineWeight AffineWeight;
    typedef typename TLSShapeFunctionTypes::VAffineWeight VAffineWeight;
    typedef typename TLSShapeFunctionTypes::VecVAffineWeight VecVAffineWeight;

    typedef typename TLSShapeFunctionTypes::Gradient Gradient;
    typedef typename TLSShapeFunctionTypes::VGradient VGradient;
    typedef typename TLSShapeFunctionTypes::VecVGradient VecVGradient;

    typedef typename TLSShapeFunctionTypes::AffineGradient AffineGradient;
    typedef typename TLSShapeFunctionTypes::VAffineGradient VAffineGradient;
    typedef typename TLSShapeFunctionTypes::VecVAffineGradient VecVAffineGradient;

    typedef Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic> DsMat;
    typedef Eigen::Matrix<Real, 4, 4> Mat4;
    typedef Eigen::Matrix<Real, 3, 3> Mat3;


    typedef sofa::defaulttype::Vec<2, Real> Vec2;

    typedef sofa::helper::kdTree<Coord> kdTree;
    typedef typename kdTree::distanceToPoint distanceToPoint;
    typedef typename kdTree::distanceSet distanceSet;

    kdTree m_kdt;

    Data<DsMat> d_W;

    typedef core::topology::BaseMeshTopology::Ptr TopoPtr;
    TopoPtr m_topo;


    Data<bool> d_projection;




    virtual void computeShapeFunction(const Coord& childPosition,
                                      VRef& ref, VRef& affineRef,
                                      VWeight& w, VAffineWeight& affineW )
    {
        helper::ReadAccessor<Data<DsMat > > W_accessor(d_W);
        const DsMat& W = W_accessor.ref();

        uint nbParentInW = W.cols();
        uint nbPosConstraints = this->d_nbPointParent.getValue();

        if((nbParentInW - nbPosConstraints)%4 != 0)
        {
            msg_error("LSJacobsonShapeFunction::computeShapeFunction()") << "nb cols in W (" << nbParentInW << ") do not match nb of position constraints (" << nbPosConstraints << ").";
            return;
        }

        uint nbRegionConstraints = (nbParentInW - nbPosConstraints) / 4;

        VWeight meshWeights;
        VRef meshParent;

//        msg_info("LSJacobsonShapeFunction::computeShapeFunction()") << "Computing mesh coordinate..." ;
        computeMeshCoordinates(childPosition, meshWeights, meshParent);
//        msg_info("LSJacobsonShapeFunction::computeShapeFunction()") << "...done." ;

        w.resize(nbPosConstraints);
        ref.resize(nbPosConstraints);
        for(uint i=0; i<nbPosConstraints; ++i)
        {
            ref[i] = i;
            w[i] = 0;


            for(uint j=0; j<meshParent.size(); ++j)
            {
                w[i] += meshWeights[j]*W(meshParent[j], i);
            }
        }

        affineW.resize(nbRegionConstraints);
        affineRef.resize(nbRegionConstraints);
        for(uint i=0; i<nbRegionConstraints; ++i)
        {
            affineRef[i] = i;

            for(uint k=0; k<4; ++k)
            {
                affineW[i][k] = 0;

                for(uint j=0; j<meshParent.size(); ++j)
                {
                    affineW[i][k] += meshWeights[j]*W(meshParent[j], nbPosConstraints + 4*i + k);
                }
            }
        }

//        uint nbParentMax = this->f_nbRef.getValue();
//        uint nbParent = std::min(nbParentInW, nbParentMax);
//        if(nbParent < nbParentInW)
//        {

//            std::sort(ref.begin(), ref.end(),
//                      [&w](const int& a, const int& b) {
//                return (w[a] > w[b]);
//            }
//            );

//            ref.resize(nbParent);

//            VReal w_temp(nbParent);

//            Real wSum=0.0;
//            for(uint i=0; i<nbParent; ++i)
//            {
//                w_temp[i] = w[ref[i]];
//                wSum += w_temp[i];
//            }

//            for(uint i=0; i<nbParent; ++i)
//            {
//                w_temp[i] /= wSum;
//            }

//            w = w_temp;
//        }
    }

    /*

    void computeMeshCoordinates(const Coord& targetPosition, VWeight& w, VRef& vertID)
    {
        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
            std::cout << "Computing mesh coordinates at location p=" << targetPosition << std::endl;

        /// retrieving mesh vertices positions
        helper::ReadAccessor<Data<VCoord > > position_accessor(this->d_position);
        const VCoord& position = position_accessor.ref();

        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
            std::cout << "  Looking for closest vertex..." << std::endl;

        std::cout << std::flush;

        uint closestVertID = m_kdt.getClosest(targetPosition, position);

        distanceSet closestVertsInit;
        m_kdt.getNClosest(closestVertsInit, targetPosition, position, 1);


        std::set<uint> closestVerts;
//        closestVerts.insert(closestVertID);

        for(distanceToPoint dtp : closestVertsInit)
            closestVerts.insert(dtp.second);



        std::set<uint> temp;

        for(uint i=0; i<2; ++i)
        {
            temp = closestVerts;
            for(uint vert_id : temp)
            {
                for(uint neib_id : m_topo->getVerticesAroundVertex(vert_id))
                {
                    closestVerts.insert(neib_id);
                }
            }
        }


//        for(uint i=0; i< m_topo->getNbPoints(); ++i)
//        {
//            closestVerts.insert(i);
//        }

//        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
//        {
//            std::cout << "    ... found V#" << closestVertID << std::endl;
//        }

        uint closestTetID;
        bool found_tetra = false;
//        core::topology::BaseMeshTopology::TetrahedraAroundVertex tets= m_topo->getTetrahedraAroundVertex(closestVertID);


        std::set<uint> tets;
        for(uint vert_id : closestVerts)
        {
            for(uint tet_id : m_topo->getTetrahedraAroundVertex(vert_id))
            {
                tets.insert(tet_id);
            }
        }


//        for(uint i=0; i< m_topo->getNbTetrahedra(); ++i)
//        {
//            tets.insert(i);
//        }

        Weight w_temp_min[4];

        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
        {
            std::cout << "  Looking for enclosing tetrahedron amongst " << tets.size() << " adjacent ones..." << std::endl;
        }

        for(uint tetID : tets)
        {
            Weight w_temp[4];
            computeBarycentricCoordinates(targetPosition, position[m_topo->getTetra(tetID)[0]], position[m_topo->getTetra(tetID)[1]], position[m_topo->getTetra(tetID)[2]], position[m_topo->getTetra(tetID)[3]], w_temp[0], w_temp[1], w_temp[2], w_temp[3]);
            if(w_temp[0] > 0 && w_temp[1] > 0 && w_temp[2] > 0 && w_temp[3] > 0)
            {
                closestTetID = tetID;
                found_tetra = true;
                w_temp_min[0] = w_temp[0];
                w_temp_min[1] = w_temp[1];
                w_temp_min[2] = w_temp[2];
                w_temp_min[3] = w_temp[3];
                break;
            }
        }
        if(found_tetra)
        {
            if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
            {
                std::cout << "    ...found Tet#" << closestTetID << " = (" << m_topo->getTetra(closestTetID)[0] << ", " << m_topo->getTetra(closestTetID)[1] << ", " << m_topo->getTetra(closestTetID)[2] << ", " << m_topo->getTetra(closestTetID)[3] << ")" << std::endl;
                std::cout << "    barycentric coordinates = " << w_temp_min[0] << ", " << w_temp_min[1] << ", " << w_temp_min[2] << ", " << w_temp_min[3] << std::endl;
            }

            w.resize(4);
            vertID.resize(4);

            for(uint j=0; j<4; ++j)
            {
                w[j] = w_temp_min[j];
                vertID[j] += m_topo->getTetra(closestTetID)[j];
            }

            if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
                std::cout << "Tetrahedron case : SUCCESS." << std::endl;

            return;
        }


//        w.resize(1);
//        w[0] = 1.0;
//        vertID.resize(1);
//        vertID[0] = closestVertID;
//        return;



        /// targetPosition is outside the tetrahedral mesh

        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
        {
            std::cout << "    ...no enclosing tetrahedron found." << std::endl;
        }

        uint closestTriID;
        Real minTriDist = std::numeric_limits<Real>::infinity();
        Real minBaryCoord[3];
        bool found_tri = false;
//        core::topology::BaseMeshTopology::TriangleAroundVertex tri= m_topo->getTrianglesAroundVertex(closestVertID);

        std::set<uint> tri;
        for(uint tet_id : tets)
        {
            for(uint tri_id : m_topo->getTrianglesInTetrahedron(tet_id))
            {
                if(isTriangleAtBorder(tri_id))
                {
                    tri.insert(tri_id);
                }
            }
        }

//        for(uint i=0; i< m_topo->getNbTriangles(); ++i)
//        {
//            tri.insert(i);
//        }

        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
        {
            std::cout << "  Looking for closest triangle amongst " << tri.size() << " adjacent ones..." << std::endl;
        }

        for(uint triID : tri)
        {
            /// checking that triID is on the border
//            if(!isTriangleAtBorder(triID))
//            {
//                continue;
//            }

            uint v0 = m_topo->getTriangle(triID)[0];
            uint v1 = m_topo->getTriangle(triID)[1];
            uint v2 = m_topo->getTriangle(triID)[2];

            Coord p0 = position[v0];
            Coord p1 = position[v1];
            Coord p2 = position[v2];


            Real baryCoord[3];
            computeBarycentricCoordinates(targetPosition, p0, p1, p2, baryCoord[0], baryCoord[1], baryCoord[2]);

            Coord normal = getTriangleNormal(triID);
            bool outside = normal * (targetPosition - (p0+p1+p2)/3.0) > 0;

            baryCoord[0] = std::max(baryCoord[0],0.0);
            baryCoord[1] = std::max(baryCoord[1],0.0);
            baryCoord[2] = std::max(baryCoord[2],0.0);

            baryCoord[0] = std::min(baryCoord[0],1.0);
            baryCoord[1] = std::min(baryCoord[1],1.0);
            baryCoord[2] = std::min(baryCoord[2],1.0);

            Real sum = baryCoord[0] + baryCoord[1] + baryCoord[2];

            baryCoord[0] /= sum;
            baryCoord[1] /= sum;
            baryCoord[2] /= sum;

//            bool outside = true;
//            uint tetID = m_topo->getTetrahedraAroundTriangle(triID)[0];
//            Weight w_temp[4];
//            computeBarycentricCoordinates(targetPosition, position[m_topo->getTetra(tetID)[0]], position[m_topo->getTetra(tetID)[1]], position[m_topo->getTetra(tetID)[2]], position[m_topo->getTetra(tetID)[3]], w_temp[0], w_temp[1], w_temp[2], w_temp[3]);
//            if(w_temp[0] > 0 && w_temp[1] > 0 && w_temp[2] > 0 && w_temp[3] > 0)
//            {
//                outside = false;
//            }

//            if(baryCoord[0] > 0 && baryCoord[1] > 0 && baryCoord[2] > 0
//            && baryCoord[0] < 1 && baryCoord[1] < 1 && baryCoord[2] < 1)
//            if(outside && baryCoord[0] > 0 && baryCoord[1] > 0 && baryCoord[2] > 0
//                       && baryCoord[0] < 1 && baryCoord[1] < 1 && baryCoord[2] < 1)
            {
                Coord targetPositionProjected = p0 * baryCoord[0]+ p1 * baryCoord[1] + p2 * baryCoord[2];
//                Coord targetPositionProjected = (p0 + p1+ p2 )/3.0;
                Real dist = (targetPosition - targetPositionProjected).norm();
                if(!found_tri || dist < minTriDist)
                {
                    found_tri = true;
                    minTriDist = dist;
                    closestTriID = triID;
                    minBaryCoord[0] = baryCoord[0];
                    minBaryCoord[1] = baryCoord[1];
                    minBaryCoord[2] = baryCoord[2];
                }
            }
        }

        if(found_tri)
        {

            if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
            {
                std::cout << "    ...found closest triangle : Tri#" << closestTriID << std::endl;
                std::cout << "    barycentric coordinates = " << minBaryCoord[0] << ", " << minBaryCoord[1] << ", " << minBaryCoord[2] << std::endl;
            }

            w.resize(3);
            vertID.resize(3);
            for(uint j=0; j<3; ++j)
            {
                w[j] = minBaryCoord[j];
                vertID[j] = m_topo->getTriangle(closestTriID)[j];
            }

            if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
            {
                std::cout << "Triangle case : SUCCESS (dist = " << minTriDist << ")." << std::endl;
            }
            return;
        }



        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
        {
            std::cout << "    ...no closest triangle found" << std::endl;
        }

        uint closestEdgeID;
        Real minEdgeDist = std::numeric_limits<Real>::infinity();
        Real minAlpha = 0.0;
        bool found_edge = false;
//        core::topology::BaseMeshTopology::EdgesAroundVertex edges= m_topo->getEdgesAroundVertex(closestVertID);


        std::set<uint> edges;
        for(uint tet_id : tets)
        {
            for(uint edge_id : m_topo->getEdgesInTetrahedron(tet_id))
            {
                if(isEdgeAtBorder(edge_id))
                {
                    edges.insert(edge_id);
                }
            }
        }


//        for(uint i=0; i< m_topo->getNbEdges(); ++i)
//        {
//            edges.insert(i);
//        }

        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
        {
            std::cout << "  Looking for closest edge amongst " << edges.size() << " adjacent ones..." << std::endl;
        }

        for(uint edgeID : edges)
        {
//            if(!isEdgeAtBorder(edgeID))
//            {
////                std::cout << "edge#" << i << " is not on the boundary." << std::endl;
//                continue;
//            }

//            std::cout << "edge#" << i << " (= " << m_topo->getEdge(edges[i])[0] << ", " << m_topo->getEdge(edges[i])[1] << ") is on the boundary." << std::endl;

            Coord p0 = position[m_topo->getEdge(edgeID)[0]];
            Coord p1 = position[m_topo->getEdge(edgeID)[1]];

            Real alpha = (targetPosition-p0)*(p1-p0) / (p1-p0).norm2();

//            helper::vector<uint> incident_border_triangles;
//            getIncidentBorderTriangles(edgeID, incident_border_triangles);
            bool outside = true;
//            for(uint triID : incident_border_triangles)
//            {
//                outside = outside && (getTriangleNormal(triID)*(targetPosition - getTriangleCenter(triID))>0);
//            }

//            std::cout << "incident_border_triangles.size() = " << incident_border_triangles.size() << std::endl;
//            assert(incident_border_triangles.size() == 2);
//            Coord normal = (getTriangleNormal(incident_border_triangles[0]) + getTriangleNormal(incident_border_triangles[1]));
//            normal = normal / normal.norm();

//            outside = (targetPosition - p0)*normal > 0;



//            for(uint tetID : m_topo->getTetrahedraAroundEdge(edgeID))
//            {
//                Weight w_temp[4];
//                computeBarycentricCoordinates(targetPosition, position[m_topo->getTetra(tetID)[0]], position[m_topo->getTetra(tetID)[1]], position[m_topo->getTetra(tetID)[2]], position[m_topo->getTetra(tetID)[3]], w_temp[0], w_temp[1], w_temp[2], w_temp[3]);
//                if(w_temp[0] > 0 && w_temp[1] > 0 && w_temp[2] > 0 && w_temp[3] > 0)
//                {
//                    outside = false;
//                    break;
//                }
//            }




            if(outside && alpha > 0.0 && 1.0 > alpha)
            {
                Coord targetPositionProjected = p0 * (1.0-alpha) + p1*alpha;
                Real dist = (targetPosition - targetPositionProjected).norm();
                if(!found_edge || dist < minEdgeDist)
                {
                    found_edge = true;
                    minEdgeDist = dist;
                    closestEdgeID = edgeID;
                    minAlpha = alpha;
                }
            }
        }
        if(found_edge)
        {

            if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
            {
                std::cout << "    ...found closest edge : E#" << closestEdgeID << std::endl;
                std::cout << "    barycentric coordinates = " << 1.0-minAlpha << ", " << minAlpha << std::endl;
            }

            w.resize(2);
            w[0] = 1.0-minAlpha;
            w[1] = minAlpha;

            vertID.resize(2);
            vertID[0] = m_topo->getEdge(closestEdgeID)[0];
            vertID[1] = m_topo->getEdge(closestEdgeID)[1];

            if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
            {
                Coord p0 = position[m_topo->getEdge(closestEdgeID)[0]];
                Coord p1 = position[m_topo->getEdge(closestEdgeID)[1]];
                std::cout << "Edge case : SUCCESS (edge [" << m_topo->getEdge(closestEdgeID)[0] << ", " << m_topo->getEdge(closestEdgeID)[1] << "] - dist = " << minEdgeDist << ", dist ratio = " << minEdgeDist / (p1-p0).norm() << ")." << std::endl;
            }
            return;
        }

        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
        {
            std::cout << "    ...no closest edge found" << std::endl;
            std::cout << "  Falling back on closest point..." << std::endl;
        }

        w.resize(1);
        w[0] = 1.0;

        uint closestVertID_2 = 0;
        Real minVertDist = std::numeric_limits<Real>::infinity();

        for(uint vertID : closestVerts)
        {
            Real dist = (targetPosition - position[vertID]).norm();
            if(dist < minVertDist)
            {
                minVertDist = dist;
                closestVertID_2 = vertID;
            }
        }

        vertID.resize(1);
        vertID[0] = closestVertID_2;

        if(LINEARSUBSPACE_LSJacobsonShapeFunction_VERBOSE)
        {
            std::cout << "Point case : SUCCESS (dist = " << minVertDist << ")." << std::endl;
        }
        return;
    }

    */


    void computeMeshCoordinates(const Coord& targetPosition, VWeight& w, VRef& vertID)
    {
        /// retrieving mesh vertices positions
        helper::ReadAccessor<Data<VCoord > > position_accessor(this->d_position);
        const VCoord& position = position_accessor.ref();

        std::cout << std::flush;

//        uint closestVertID = m_kdt.getClosest(targetPosition, position);

        distanceSet closestVertsInit;
        m_kdt.getNClosest(closestVertsInit, targetPosition, position, 1);


        std::set<uint> closestVerts;
//        closestVerts.insert(closestVertID);

        for(distanceToPoint dtp : closestVertsInit)
            closestVerts.insert(dtp.second);



        std::set<uint> temp;

        for(uint i=0; i<0; ++i)
        {
            temp = closestVerts;
            for(uint vert_id : temp)
            {
                for(uint neib_id : m_topo->getVerticesAroundVertex(vert_id))
                {
                    closestVerts.insert(neib_id);
                }
            }
        }



        std::set<uint> tets;
        for(uint vert_id : closestVerts)
        {
            for(uint tet_id : m_topo->getTetrahedraAroundVertex(vert_id))
            {
                tets.insert(tet_id);
            }
        }


//        for(uint i=0; i< m_topo->getNbTetrahedra(); ++i)
//        {
//            tets.insert(i);
//        }

        uint closestTetID;

        Real dist_min = std::numeric_limits<Real>::infinity();

        for(uint tetID : tets)
//        for(uint tetID = 0;  tetID < m_topo->getNbTetrahedra(); tetID++ )
        {
            Weight w_temp[4];

            Coord p0 = position[m_topo->getTetra(tetID)[0]];
            Coord p1 = position[m_topo->getTetra(tetID)[1]];
            Coord p2 = position[m_topo->getTetra(tetID)[2]];
            Coord p3 = position[m_topo->getTetra(tetID)[3]];

            computeBarycentricCoordinates(targetPosition, p0, p1, p2, p3, w_temp[0], w_temp[1], w_temp[2], w_temp[3]);

            Real dist = std::numeric_limits<Real>::infinity();

            if(w_temp[0] > 0 && w_temp[1] > 0 && w_temp[2] > 0 && w_temp[3] > 0)
            {
                dist = 0;
            }
            else
            {
                Weight w_temp_trunc[4] = {w_temp[0]>0 ? w_temp[0]:0, w_temp[1]>0 ? w_temp[1]:0, w_temp[2]>0 ? w_temp[2]:0, w_temp[3]>0 ? w_temp[3]:0};

                Real sum = w_temp_trunc[0] + w_temp_trunc[1] + w_temp_trunc[2] + w_temp_trunc[3];
                for(uint j=0; j<4; ++j)
                    w_temp_trunc[j] /= sum;

                Coord target_proj = w_temp_trunc[0] * p0 + w_temp_trunc[1] * p1 + w_temp_trunc[2] * p2 + w_temp_trunc[3] * p3;
                dist = (targetPosition - target_proj).norm();
            }

            if(dist < dist_min)
            {
                dist_min = dist;
                closestTetID = tetID;
            }
        }

        vertID.resize(4);
        vertID[0] = m_topo->getTetra(closestTetID)[0];
        vertID[1] = m_topo->getTetra(closestTetID)[1];
        vertID[2] = m_topo->getTetra(closestTetID)[2];
        vertID[3] = m_topo->getTetra(closestTetID)[3];

        Coord p0 = position[vertID[0]];
        Coord p1 = position[vertID[1]];
        Coord p2 = position[vertID[2]];
        Coord p3 = position[vertID[3]];

        w.resize(4);
        computeBarycentricCoordinates(targetPosition, p0, p1, p2, p3, w[0], w[1], w[2], w[3]);

        if(d_projection.getValue())
        {
            for(uint i=0; i<4; ++i)
            {
                w[i] = std::max(w[i], 0.0);
    //            w[i] = std::min(w[i], 1.0);
            }

            Real sum = 0;
            for(uint i=0; i<4; ++i)
                sum += w[i];

            for(uint i=0; i<4; ++i)
                w[i] = w[i] / sum;
        }

    }



    /// inspired from http://steve.hollasch.net/cgindex/geometry/ptintet.html
    bool isPointInsideTetra(const Coord& p, const Coord& p0, const Coord& p1, const Coord& p2, const Coord& p3)
    {
        Real c0, c1, c2, c3;
        computeBarycentricCoordinates(p, p0, p1, p2, p3, c0, c1, c2, c3);

        /// checking signs
        return c0 > 0 && c1 > 0 && c2 > 0 && c3 > 0;
    }

    /// Computes barycentric coordinates (c0, c1, c2, c3) of p inside tetrahedron (p0, p1, p2, p3)
    void computeBarycentricCoordinates(const Coord& p, const Coord& p0, const Coord& p1, const Coord& p2, const Coord& p3, Real& c0, Real& c1, Real& c2, Real& c3)
    {
        Real d =tetraDeterminant(p0, p1, p2, p3);
        Real d0=tetraDeterminant(p , p1, p2, p3);
        Real d1=tetraDeterminant(p0, p , p2, p3);
        Real d2=tetraDeterminant(p0, p1, p , p3);
        Real d3=tetraDeterminant(p0, p1, p2, p );

        c0 = d0/d;
        c1 = d1/d;
        c2 = d2/d;
        c3 = d3/d;
    }

    /// Computes barycentric coordinates (c0, c1, c2) of p inside triangle (p0, p1, p2)
    void computeBarycentricCoordinates(const Vec2& p, const Vec2& p0, const Vec2& p1, const Vec2& p2, Real& c0, Real& c1, Real& c2)
    {
        Real d =triDeterminant(p0, p1, p2);
        Real d0=triDeterminant(p , p1, p2);
        Real d1=triDeterminant(p0, p , p2);
        Real d2=triDeterminant(p0, p1, p );

        c0 = d0/d;
        c1 = d1/d;
        c2 = d2/d;
    }

    /// see http://math.stackexchange.com/questions/544946/determine-if-projection-of-3d-point-onto-plane-is-within-a-triangle
    void computeBarycentricCoordinates(const Coord& p, const Coord& p0, const Coord& p1, const Coord& p2, Real& c0, Real& c1, Real& c2)
    {
        Coord u = p1-p0;
        Coord v = p2-p0;
        Coord w = p-p0;
        Coord n = u.cross(v);

        Real n2 = n.norm2();

        c1=((w.cross(v)) * n)/n2;
        c2=((u.cross(w)) * n)/n2;
        c0=1-c1-c2;
    }

    /// signed volume computation
    Real tetraDeterminant(const Coord& p0, const Coord& p1, const Coord& p2, const Coord& p3)
    {
        Mat4 m;
        m  << p0[0] , p0[1] , p0[2] , 1.0
                , p1[0] , p1[1] , p1[2] , 1.0
                , p2[0] , p2[1] , p2[2] , 1.0
                , p3[0] , p3[1] , p3[2] , 1.0;

        return m.determinant();
    }

    /// signed area computation
    Real triDeterminant(const Vec2& p0, const Vec2& p1, const Vec2& p2)
    {
        Mat3 m;
        m  << p0[0] , p0[1] , 1.0
                , p1[0] , p1[1] , 1.0
                , p2[0] , p2[1] , 1.0;

        return m.determinant(); // equivalent with (p1-p0).cross(p2-p0)
    }

    bool isTriangleAtBorder(const uint tetID)
    {
        return m_topo->getTetrahedraAroundTriangle(tetID).size() == 1;
    }

    bool isEdgeAtBorder(const uint edgeID)
    {
        for(uint tri_id : m_topo->getTrianglesAroundEdge(edgeID))
        {
            if(isTriangleAtBorder(tri_id))
                return true;
        }
        return false;
    }

    Coord getTriangleNormal(const uint triID)
    {
        helper::ReadAccessor<Data<VCoord > > position_accessor(this->d_position);
        const VCoord& position = position_accessor.ref();


        uint p0 = m_topo->getTriangle(triID)[0];
        uint p1 = m_topo->getTriangle(triID)[1];
        uint p2 = m_topo->getTriangle(triID)[2];

        Coord result = (position[p1]-position[0]).cross(position[p2]-position[p0]);
        result = result / result.norm();

        assert(m_topo->getTetrahedraAroundTriangle(triID).size() == 1);
        uint tet = m_topo->getTetrahedraAroundTriangle(triID)[0];
        uint p3 = m_topo->getTetrahedron(tet)[0];
        if(p3 == p0 || p3 == p1 || p3 == p2)
            p3 = m_topo->getTetrahedron(tet)[1];
        if(p3 == p0 || p3 == p1 || p3 == p2)
            p3 = m_topo->getTetrahedron(tet)[2];
        if(p3 == p0 || p3 == p1 || p3 == p2)
            p3 = m_topo->getTetrahedron(tet)[3];

        Coord result2 = (position[p0] + position[p1] + position[p2])/3.0 - position[p3];
        result2 = result2 / result2.norm();

        if(result * result2 < 0.0)
            result = -1.0 * result;

        return result;
//        return result2;
    }

    Coord getTriangleNormal(const Coord& p0, const Coord& p1, const Coord& p2)
    {
        Coord result = (p1-p0).cross(p2-p0);
        return result / result.norm();
    }

    Coord getTriangleCenter(const uint triID)
    {
        helper::ReadAccessor<Data<VCoord > > position_accessor(this->d_position);
        const VCoord& position = position_accessor.ref();

        return (position[m_topo->getTriangle(triID)[0]]+position[m_topo->getTriangle(triID)[1]]+position[m_topo->getTriangle(triID)[2]]) / 3.0;
    }

    Coord getTriangleCenter(const Coord& p0, const Coord& p1, const Coord& p2)
    {
        return (p0+p1+p2) / 3.0;
    }

    void getIncidentBorderTriangles(const uint edgeID, helper::vector<uint>& result)
    {
        result.clear();
        for(uint tri_id : m_topo->getTrianglesAroundEdge(edgeID))
        {
            if(isTriangleAtBorder(tri_id))
            {
                result.push_back(tri_id);
            }
        }
    }

    void init()
    {
        //        this->addInput(d_W);

        /// Initialize the KD-Tree
        m_kdt.build(this->d_position.getValue());

        /// get the topology
        m_topo = this->getContext()->getMeshTopology();

        reinit();
    }

    void reinit()
    {
    }

protected:
    LSJacobsonShapeFunction() : Inherit()
      , d_W(initData(&d_W, DsMat(), "W", "Weight matrix."))
      , d_projection(initData(&d_projection, false, "projection", "activates projection of childrenposition onto the tet mesh surafce (using barycentric coordinate truncature)."))
    {
    }

    virtual ~LSJacobsonShapeFunction()
    {

    }
};


}
}
}


#endif
