/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 MGH, INRIA, USTL, UJF, CNRS                    *
*                                                                             *
* This library 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 library 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 library; if not, write to the Free Software Foundation,     *
* Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA.          *
*******************************************************************************
*                               SOFA :: Modules                               *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/
#ifndef SOFA_COMPONENT_RayMeshIntersectionEngine_H
#define SOFA_COMPONENT_RayMeshIntersectionEngine_H

#include "initContactMapping.h"
#include "projectToMesh.h"
#include <sofa/core/DataEngine.h>
#include <sofa/defaulttype/VecTypes.h>
#include <sofa/core/topology/BaseMeshTopology.h>
#include <sofa/helper/AdvancedTimer.h>
#include <SofaGeneralMeshCollision/TriangleOctree.h>
#include <sofa/helper/OptionsGroup.h>


#include <sofa/helper/IndexOpenMP.h>
#ifdef _OPENMP
#include <omp.h>
#endif

namespace sofa
{

namespace component
{

namespace engine
{

/**
 * Compute closest intersection on a mesh given origins and ray directions
 * @author Benjamin Gilles
 * @date 2016
 */
template <class DataTypes>
class RayMeshIntersectionEngine : public core::DataEngine
{
public:
    typedef core::DataEngine Inherited;
    SOFA_CLASS(SOFA_TEMPLATE(RayMeshIntersectionEngine, DataTypes), Inherited);

    typedef typename DataTypes::Coord    InCoord;
    typedef typename DataTypes::Deriv    InDeriv;
    typedef typename DataTypes::VecCoord InVecCoord;
    typedef typename DataTypes::VecDeriv InVecDeriv;
    typedef typename DataTypes::Real     Real;

    typedef typename core::topology::BaseMeshTopology::SeqEdges SeqEdges;
    typedef typename core::topology::BaseMeshTopology::Triangle Triangle;
    typedef typename core::topology::BaseMeshTopology::SeqTriangles SeqTriangles;
    typedef typename core::topology::BaseMeshTopology::SeqQuads SeqQuads;

    typedef helper::ProjectToMesh<InCoord> ProjectToMesh;
    typedef typename ProjectToMesh::ClosestStruct ClosestStruct;

    virtual std::string getTemplateName() const    { return templateName(this);    }
    static std::string templateName(const RayMeshIntersectionEngine<DataTypes>* = NULL) { return DataTypes::Name();   }

    Data< InVecCoord > d_source;
    Data< InVecCoord > d_directions;
    Data< InVecCoord > d_target;
    Data< SeqTriangles > d_triangles;  ///<triangles
    Data< SeqQuads > d_quads;  ///<quads
    Data< bool > d_maximizeDistances;
    Data< bool > d_parallel;		///< use openmp ?
    Data<helper::OptionsGroup> d_method;                    ///< acceleration method for intersection search

    Data< helper::vector<ClosestStruct> > d_closests; ///< store closest point on mesh from each parent1 point
    Data< InVecCoord > d_projections;

protected:

    RayMeshIntersectionEngine():   Inherited()
      , d_source(initData(&d_source,"sourcePosition","source dofs to project"))
      , d_directions(initData(&d_directions,"directions","project direction(s)"))
      , d_target(initData(&d_target,"targetPosition","mesh dofs"))
      , d_triangles(initData(&d_triangles,SeqTriangles(),"triangles","mesh triangles"))
      , d_quads(initData(&d_quads,SeqQuads(),"quads","mesh quads"))
      , d_maximizeDistances(initData(&d_maximizeDistances,false,"maximizeDistances","distance maximization insted of minimization?"))
      , d_parallel(initData(&d_parallel, true, "parallel", "use openmp parallelisation?"))
      , d_method ( initData ( &d_method,"method","method" ) )
      , d_closests(initData(&d_closests,"output","closest primitives with weights, normal, distance"))
      , d_projections(initData(&d_projections,"projections","computed projections"))
    {
        this->addAlias(&d_target, "position");
        this->addAlias(&d_closests, "closests");

        helper::OptionsGroup options(2, "0 - Brute force" , "1 - Octree");
        options.setSelectedItem(1);
        d_method.setValue(options);
    }

    virtual ~RayMeshIntersectionEngine() {}

    collision::TriangleOctreeRoot octree;
    SeqTriangles alltri; // all triangles including half quads

public:

    virtual void init()
    {
        addInput(&d_source);
        addInput(&d_directions);
        addInput(&d_target);
        addInput(&d_triangles);
        addInput(&d_quads);
        addInput(&d_maximizeDistances);
        addInput(&d_method);

        addOutput(&d_closests);
        addOutput(&d_projections);

        // track these Data to check if they changed since the last update
        m_dataTracker.trackData(d_target);
        m_dataTracker.trackData(d_triangles);
        m_dataTracker.trackData(d_quads);

        reinit();
        Inherited::init();
    }

    virtual void reinit()
    {
        Inherited::reinit();
        d_closests.setDirtyValue();
        update();
    }


    virtual void update()
    {
        helper::ScopedAdvancedTimer advancedTimer("RayMeshIntersectionEngine");

        bool targetDirty =  !octree.octreeRoot
                || m_dataTracker.isDirty( d_target )
                || m_dataTracker.isDirty( d_triangles )
                || m_dataTracker.isDirty( d_quads );

        if(f_printLog.getValue()) sout<<this->getName()<<": update, targetdirty="<<targetDirty<<sendl;

        updateAllInputsIfDirty();

        cleanDirty();

        const InVecCoord& source = d_source.getValue();
        const InVecCoord& directions = d_directions.getValue();
        const InVecCoord& target = d_target.getValue();
        const SeqTriangles& triangles = d_triangles.getValue();
        const SeqQuads& quads = d_quads.getValue();
        const bool maximizeDistances = d_maximizeDistances.getValue();

        std::vector<ClosestStruct>& closests = *d_closests.beginWriteOnly();
        helper::WriteOnlyAccessor<Data< InVecCoord > > projections(d_projections);

        closests.resize(source.size());
        projections.resize(source.size());

        ProjectToMesh projectToMesh;

        if(this->d_method.getValue().getSelectedId()==1) // OCTREE
        {
            if(targetDirty)
            {
                // build octree
                if(quads.size()==0)
                {
                    octree.buildOctree(&triangles, &target);
                }
                else
                {
                    alltri = triangles;
                    for(size_t i=0; i<quads.size(); ++i)
                    {
                        alltri.push_back(Triangle(quads[i][0], quads[i][1], quads[i][2]));
                        alltri.push_back(Triangle(quads[i][0], quads[i][2], quads[i][3]));
                    }
                    octree.buildOctree(&alltri, &target);
                }
            }

            // compute intersections
#ifdef _OPENMP
#pragma omp parallel for if (this->d_parallel.getValue())
#endif
            for(sofa::helper::IndexOpenMP<size_t>::type i=0;i<source.size();i++)
            {
                const InCoord& dir = directions[ std::min<size_t>(directions.size() - 1, i) ];
                projectToMesh.getRayIntersection(closests[i],source[i],dir,target,octree,maximizeDistances);
                projections[i]=source[i]-closests[i].dist*closests[i].u;
            }
        }
        else // BRUTEFORCE
        {
#ifdef _OPENMP
#pragma omp parallel for if (this->d_parallel.getValue())
#endif
            for(sofa::helper::IndexOpenMP<size_t>::type i=0;i<source.size();i++)
            {
                const InCoord& dir = directions[ std::min<size_t>(directions.size() - 1, i) ];
                projectToMesh.getRayIntersection(closests[i],source[i],dir,target,triangles,quads,maximizeDistances);
                projections[i]=source[i]-closests[i].dist*closests[i].u;
            }
        }


        d_closests.endEdit();
    }

};


#if defined(SOFA_EXTERN_TEMPLATE) && !defined(SOFA_COMPONENT_MAPPING_RayMeshIntersectionEngine_CPP)
#ifndef SOFA_FLOAT
extern template class SOFA_ContactMapping_API RayMeshIntersectionEngine< sofa::defaulttype::Vec3dTypes >;
#endif
#ifndef SOFA_DOUBLE
extern template class SOFA_ContactMapping_API RayMeshIntersectionEngine< sofa::defaulttype::Vec3fTypes >;
#endif
#endif

} // namespace engine

} // namespace component

} // namespace sofa

#endif //SOFA_COMPONENT_RayMeshIntersectionEngine_H
