/******************************************************************************
*       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_MAPPING_DISTANCETOMESHMULTIMAPPING_INL
#define SOFA_COMPONENT_MAPPING_DISTANCETOMESHMULTIMAPPING_INL

#include "DistanceToMeshMultiMapping.h"
#include <sofa/core/visual/VisualParams.h>
#include <sofa/helper/AdvancedTimer.h>

namespace sofa
{

namespace component
{

namespace mapping
{



template <class TIn, class TOut>
DistanceToMeshMultiMapping< TIn, TOut >::DistanceToMeshMultiMapping()
    : Inherit()
    , d_closests(initData(&d_closests, "closests", "ClosestStruct computed by ProjectionToMeshEngine"))
    , f_indexPairs(initData(&f_indexPairs, "indexPairs", "input list of closest point pairs (parent1 index, parent2 index)"))
    , d_validPairs(initData(&d_validPairs, "validPairs", "Set of valid index pairs"))
    , d_showObjectScale(initData(&d_showObjectScale, (Real)0, "showObjectScale", "Scale for object display"))
    , d_color(initData(&d_color, "showColor", "Color for object display"))
    , fromModel1(NULL)
    , fromModel2(NULL)
    , toModel(NULL)
{
    // init jacobian
    m_jacobians.push_back(&m_jacobian1);
    m_jacobians.push_back(&m_jacobian2);
}




template <class TIn, class TOut>
void DistanceToMeshMultiMapping< TIn, TOut >::resizeOut(std::size_t size)
{
    if(toModel->getSize()!=size)
    {
        if(this->f_printLog.getValue()) sout<<this->getName()<<": resizeOut="<<size<<sendl;

        toModel->resize(size);
        m_jacobian1.resizeBlocks(size, fromModel1->getSize());
        m_jacobian2.resizeBlocks(size, fromModel2->getSize());
    }
    else
    {
        m_jacobian1.compressedMatrix.setZero();
        m_jacobian2.compressedMatrix.setZero();
    }
}



template <class TIn, class TOut>
void DistanceToMeshMultiMapping< TIn, TOut>::init()
{
    // check inputs
    if (this->getFrom().size() != 2) { serr << "must have two parents" << sendl; exit(EXIT_FAILURE); }
    if (this->getTo().size() != 1) { serr << "must have one child" << sendl; exit(EXIT_FAILURE);}
    if (d_closests.getValue().size() != f_indexPairs.getValue().size()) { serr << "have you computed 'closests' with a 'BaseProjectionToMeshEngine'?" << sendl; exit(EXIT_FAILURE); }

    fromModel1 = this->getFromModels()[0];
    fromModel2 = this->getFromModels()[1];
    toModel = this->getToModels()[0];

    reinit();

    Inherit::init();
}

template <class TIn, class TOut>
void DistanceToMeshMultiMapping< TIn, TOut>::reinit()
{
    toModel->resize(0);
    resizeOut(d_validPairs.getValue().empty() ? f_indexPairs.getValue().size() : d_validPairs.getValue().size());

    Inherit::reinit();
}



template <class TIn, class TOut>
void DistanceToMeshMultiMapping< TIn, TOut >::apply(const core::MechanicalParams*, const helper::vector<OutDataVecCoord*>& dataVecOutPos, const helper::vector<const InDataVecCoord*>& /*dataVecInPos*/)
{
    helper::ScopedAdvancedTimer advancedTimer("DistanceToMeshMultiMapping");

//    const InVecCoord& inPos1 = dataVecInPos[0]->getValue();
//    const InVecCoord& inPos2 = dataVecInPos[1]->getValue();

    const helper::vector<ClosestStruct>& closests = d_closests.getValue();
    const helper::vector<defaulttype::Vec2i>& indexPairs = f_indexPairs.getValue();

    typename Jacobian::CompressedMatrix& J1 = m_jacobian1.compressedMatrix;
    typename Jacobian::CompressedMatrix& J2 = m_jacobian2.compressedMatrix;
    typename Jacobian::Block J;

    helper::IndexPairIterator indexPairIterator( indexPairs.size(), d_validPairs.getValue(), d_validPairs.isSet() );


    resizeOut(indexPairIterator.size());
    if(!indexPairIterator.size()) return;

    // update output and jacobians
    OutVecCoord& outPos = *dataVecOutPos[0]->beginWriteOnly();
    for ( ; indexPairIterator.count()<indexPairIterator.size() ; ++indexPairIterator ) {
        const size_t i = indexPairIterator.get();
        const int iP = indexPairs[i][0];
        outPos[indexPairIterator.count()][0] = closests[i].dist;

        if( this->maskTo[0]->isActivated() && !this->maskTo[0]->getEntry(indexPairIterator.count()) ) continue;

        // compute jacobians
        for(std::size_t k=0; k<Nin; k++ ) J[0][k] = closests[i].u[k];
        J1.startVec( indexPairIterator.count() );
        for(std::size_t k=0; k<Nin; k++ ) J1.insertBack( indexPairIterator.count(), iP*Nin+k ) =  J[0][k];

        J2.startVec( indexPairIterator.count() );
        for(typename std::set<IndexToWeightPair>::iterator iwit=closests[i].indexWeightSet.begin(); iwit!=closests[i].indexWeightSet.end(); iwit++)
            for(std::size_t k=0; k<Nin; k++ ) J2.insertBack( indexPairIterator.count(), iwit->first*Nin+k ) =  -J[0][k]*iwit->second;
    }

    dataVecOutPos[0]->endEdit();

    J1.finalize();
    J2.finalize();
}



template <class TIn, class TOut>
void DistanceToMeshMultiMapping< TIn, TOut >::draw(const core::visual::VisualParams* vparams)
{
    if( !vparams->displayFlags().getShowMechanicalMappings() ) return;

    const helper::vector<ClosestStruct>& closests = d_closests.getValue();
    const helper::vector<defaulttype::Vec2i>& indexPairs = f_indexPairs.getValue();
    const std::list<size_t>& validPairs = d_validPairs.getValue();

    helper::vector< defaulttype::Vector3 > points;

    for( helper::IndexPairIterator indexPairIterator( indexPairs.size(), validPairs, d_validPairs.isSet() ) ; indexPairIterator.count()<indexPairIterator.size() ; ++indexPairIterator )
    {
        const size_t i = indexPairIterator.get();
        const int iP = indexPairs[i][0];
        const InCoord& pos1 = fromModel1->readPositions()[iP];
        points.push_back( defaulttype::Vector3( TIn::getCPos(pos1) ) );
        points.push_back( defaulttype::Vector3( TIn::getCPos(pos1 -  closests[i].u*closests[i].dist) ));
    }

    const helper::vector<defaulttype::Vec4f>&colors = d_color.getValue();
    const Real& showObjectScale =  d_showObjectScale.getValue();

    if( colors.empty() )
    {
        static const defaulttype::Vec4f defaultColor = defaulttype::Vec4f(1,1,0,1);
        if( showObjectScale == 0 )        vparams->drawTool()->drawLines ( points, 1, defaultColor );
        else for(std::size_t i=0; i<points.size()*0.5; i++ )            vparams->drawTool()->drawCylinder( points[2*i] , points[2*i+1] , showObjectScale, defaultColor );
    }
    else if( colors.size() == 2 ) {
        // first color for positive distance, second for negative
        std::vector<defaulttype::Vec4f> myColors;
        typename core::State<Out>::ReadVecCoord outDist = toModel->readPositions();
        for (std::size_t i = 0; i<outDist.size(); ++i) {
            if (outDist[i][0]<0.)
                myColors.push_back(colors[1]);
            else
                myColors.push_back(colors[0]);
        }
        if( showObjectScale == 0 )        vparams->drawTool()->drawLines ( points, 1, myColors );
        else for(std::size_t i=0; i<points.size()*0.5; i++ )            vparams->drawTool()->drawCylinder( points[2*i] , points[2*i+1] , showObjectScale, myColors[i] );

    }
    else if( colors.size() != points.size()/2 )
    {
        if( showObjectScale == 0 )        vparams->drawTool()->drawLines ( points, 1, colors[0] );
        else for(std::size_t i=0; i<points.size()*0.5; i++ )            vparams->drawTool()->drawCylinder( points[2*i] , points[2*i+1] , showObjectScale, colors[0] );
    }
    else
    {
        if( showObjectScale == 0 )        vparams->drawTool()->drawLines ( points, 1, colors );
        else for(std::size_t i=0; i<points.size()*0.5; i++ )            vparams->drawTool()->drawCylinder( points[2*i] , points[2*i+1] , showObjectScale, colors[i] );
    }
}



template <class TIn, class TOut>
void DistanceToMeshMultiMapping<TIn, TOut>::updateForceMask()
{
    const helper::vector<ClosestStruct>& closests = d_closests.getValue();
    const helper::vector<defaulttype::Vec2i>& indexPairs = f_indexPairs.getValue();
    const std::list<size_t>& validPairs = d_validPairs.getValue();

    for( helper::IndexPairIterator indexPairIterator( indexPairs.size(), validPairs, d_validPairs.isSet() ) ; indexPairIterator.count()<indexPairIterator.size() ; ++indexPairIterator )
    {
        if( this->maskTo[0]->getEntry(indexPairIterator.count()) )
        {
            const size_t i = indexPairIterator.get();
            const int iP = indexPairs[i][0];
            this->maskFrom[0]->insertEntry( iP );
            for(typename std::set<IndexToWeightPair>::iterator iwit=closests[i].indexWeightSet.begin(); iwit!=closests[i].indexWeightSet.end(); iwit++)
                this->maskFrom[1]->insertEntry(iwit->first);
        }
    }
}

} // namespace mapping

} // namespace component

} // namespace sofa

#endif // SOFA_COMPONENT_MAPPING_DISTANCETOMESHMULTIMAPPING_INL

