/******************************************************************************
*       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_LinearLSMultiMapping_H
#define LINEARSUBSPACE_LinearLSMultiMapping_H

#include "LinearSubspace/config.h"
#include "BaseLSMultiMapping.h"

#include "LSJacobianBlock.h"

#include <Flexible/deformationMapping/BaseDeformationImpl.inl>

#include "LSJacobianBlock_point.inl"
#include "LSJacobianBlock_affine.inl"

#ifdef __APPLE__
// a strange behaviour of the mac's linker requires to compile a few stuffs again
#include "BaseDeformationMultiMapping.inl"
#endif

namespace sofa
{
namespace component
{
namespace mapping
{


/** Generic linear mapping, from a variety of input types to a variety of output types.
*/


template <class TIn1, class TIn2, class TOut>
class LinearLSMultiMapping : public BaseLSMultiMappingT<defaulttype::LSJacobianBlock<TIn1,TOut>, defaulttype::LSJacobianBlock<TIn2,TOut> >
{
public:
    typedef defaulttype::LSJacobianBlock<TIn1,TOut> BlockType1;
    typedef defaulttype::LSJacobianBlock<TIn2,TOut> BlockType2;
    typedef BaseLSMultiMappingT<BlockType1,BlockType2> Inherit;

    typedef typename Inherit::Real Real;
    typedef typename Inherit::Coord Coord;
    typedef typename Inherit::VecCoord VecCoord;
    typedef typename Inherit::InVecCoord1 InVecCoord1;
    typedef typename Inherit::InVecCoord2 InVecCoord2;
    typedef typename Inherit::OutVecCoord OutVecCoord;


    typedef typename Inherit::Weight1 Weight1;
    typedef typename Inherit::VWeight1 VWeight1;
    typedef typename Inherit::Weight2 Weight2;
    typedef typename Inherit::VWeight2 VWeight2;

    typedef typename Inherit::MaterialToSpatial MaterialToSpatial;
    typedef typename Inherit::VRef VRef;
    typedef typename Inherit::VReal VReal;
    typedef typename Inherit::Gradient1 Gradient1;
    typedef typename Inherit::VGradient1 VGradient1;
    typedef typename Inherit::Gradient2 Gradient2;
    typedef typename Inherit::VGradient2 VGradient2;
    typedef typename Inherit::Hessian1 Hessian1;
    typedef typename Inherit::VHessian1 VHessian1;
    typedef typename Inherit::Hessian2 Hessian2;
    typedef typename Inherit::VHessian2 VHessian2;

    typedef defaulttype::StdVectorTypes<defaulttype::Vec<Inherit::spatial_dimensions,Real>,defaulttype::Vec<Inherit::spatial_dimensions,Real>,Real> VecSpatialDimensionType;
    typedef defaulttype::LSJacobianBlock<TIn1,VecSpatialDimensionType> PointMapperType1;
    typedef defaulttype::LSJacobianBlock<TIn2,VecSpatialDimensionType> PointMapperType2;
    typedef defaulttype::DefGradientTypes<Inherit::spatial_dimensions, Inherit::material_dimensions, 0, Real> FType;
    typedef defaulttype::LSJacobianBlock<TIn1,FType> DeformationGradientMapperType1;
    typedef defaulttype::LSJacobianBlock<TIn2,FType> DeformationGradientMapperType2;

    SOFA_CLASS(SOFA_TEMPLATE3(LinearLSMultiMapping,TIn1,TIn2,TOut), SOFA_TEMPLATE2(BaseLSMultiMappingT,BlockType1,BlockType2 ));

protected:
    LinearLSMultiMapping ()
        : Inherit ()
    {
    }

    virtual ~LinearLSMultiMapping()     { }


    virtual void mapPosition(Coord& p,const Coord &p0, const VRef& ref1, const VRef& ref2, const VWeight1& w1, const VWeight2& w2)
    {
        helper::ReadAccessor<Data<InVecCoord1> > in10 (*this->fromModel1->read(core::ConstVecCoordId::restPosition()));
        helper::ReadAccessor<Data<InVecCoord1> > in1 (*this->fromModel1->read(core::ConstVecCoordId::position()));
        helper::ReadAccessor<Data<InVecCoord2> > in20 (*this->fromModel2->read(core::ConstVecCoordId::restPosition()));
        helper::ReadAccessor<Data<InVecCoord2> > in2 (*this->fromModel2->read(core::ConstVecCoordId::position()));

        PointMapperType1 mapper1;
        PointMapperType2 mapper2;

        // empty variables (not used in init)
//        typename PointMapperType1::OutCoord o(defaulttype::NOINIT);

        p=Coord();

        typename PointMapperType1::MaterialToSpatial M1(defaulttype::NOINIT);
        Gradient1 dw1;
        Hessian1 ddw1;
        for(unsigned int j=0; j<ref1.size(); j++ )
        {
            unsigned int index=ref1[j];
//                mapper1.init( in10[index],o,p0,M0,w[j],dw[0],ddw[0]);
            mapper1.init(M1,w1[j],dw1,ddw1);
            mapper1.addapply(p,in1[index]);
        }

        typename PointMapperType2::MaterialToSpatial M2(defaulttype::NOINIT);
        Gradient2 dw2;
        Hessian2 ddw2;
        for(unsigned int j=0; j<ref2.size(); j++ )
        {
            unsigned int index=ref2[j];
            mapper2.init(M2,w2[j],dw2,ddw2);
            mapper2.addapply(p,in2[index]);
        }
    }

    virtual void mapDeformationGradient(MaterialToSpatial& F, const Coord &p0, const MaterialToSpatial& M, const VRef& ref1, const VRef& ref2, const VWeight1& w1, VWeight2& w2, const VGradient1& dw1, const VGradient2& dw2)
//    virtual void mapDeformationGradient(MaterialToSpatial& F, const Coord &p0, const MaterialToSpatial& M, const VRef& ref1, const VRef& ref2, const VWeight1& w1, const VWeight2& w2, const VGradient1& dw1, const VGradient2& dw2)
    {
        helper::ReadAccessor<Data<InVecCoord1> > in10 (*this->fromModel1->read(core::ConstVecCoordId::restPosition()));
        helper::ReadAccessor<Data<InVecCoord1> > in1 (*this->fromModel1->read(core::ConstVecCoordId::position()));
        helper::ReadAccessor<Data<InVecCoord2> > in20 (*this->fromModel2->read(core::ConstVecCoordId::restPosition()));
        helper::ReadAccessor<Data<InVecCoord2> > in2 (*this->fromModel2->read(core::ConstVecCoordId::position()));

        DeformationGradientMapperType1 mapper1;
        DeformationGradientMapperType2 mapper2;

        // empty variables (not used in init)
//        typename DeformationGradientMapperType1::OutCoord o;

//        typename DeformationGradientMapperType1::OutCoord Fc;


        typename DeformationGradientMapperType1::OutCoord Fc;

        typename PointMapperType1::MaterialToSpatial M1(defaulttype::NOINIT);
        Hessian1 ddw1;
        for(unsigned int j=0; j<ref1.size(); j++ )
        {
            unsigned int index=ref1[j];
            mapper1.init(M1,w1[j],dw1[j],ddw1);
            mapper1.addapply(Fc,in1[index]);
        }

        typename PointMapperType2::MaterialToSpatial M2(defaulttype::NOINIT);
        Hessian2 ddw2;
        for(unsigned int j=0; j<ref2.size(); j++ )
        {
            unsigned int index=ref2[j];
            mapper2.init(M2,w2[j],dw2[j],ddw2);
            mapper2.addapply(Fc,in2[index]);
        }


        F=Fc.getF();
    }

    virtual void initJacobianBlocks()
    {
        helper::ReadAccessor<Data<InVecCoord1> > in1 (*this->fromModel1->read(core::ConstVecCoordId::restPosition()));
        helper::ReadAccessor<Data<InVecCoord2> > in2 (*this->fromModel2->read(core::ConstVecCoordId::restPosition()));
        helper::ReadAccessor<Data<OutVecCoord> > out (*this->toModel->read(core::ConstVecCoordId::position()));

        size_t size=this->f_pos0.getValue().size();

        bool dw1  = !this->f_dw1.getValue().empty();
        bool ddw1 = !this->f_ddw1.getValue().empty();
        bool dw2  = !this->f_dw2.getValue().empty();
        bool ddw2 = !this->f_ddw2.getValue().empty();
        bool F0  = !this->f_F0.getValue().empty();
        static const MaterialToSpatial FI = identity<MaterialToSpatial>();

        this->jacobian1.resize(size);
        this->jacobian2.resize(size);
        for(size_t i=0; i<size; i++ )
        {
            this->jacobian1[i].resize(0);
            this->jacobian2[i].resize(0);

            size_t nbref1 = this->f_index1.getValue()[i].size();
            for(size_t j=0; j<nbref1; j++ )
            {
//                unsigned int index=this->f_index1.getValue()[i][j];
                    BlockType1 b;
//                    b.init( in1[index],
//                            out[i],
//                            this->f_pos0.getValue()[i],
//                            F0 ? this->f_F0.getValue()[i] : FI,
//                            this->f_w1.getValue()[i][j],
//                            dw1 ? this->f_dw1.getValue()[i][j] : Gradient1(),
//                            ddw1 ? this->f_ddw1.getValue()[i][j] : Hessian1() );
                    b.init( F0 ? this->f_F0.getValue()[i] : FI,
                            this->f_w1.getValue()[i][j],
                            dw1 ? this->f_dw1.getValue()[i][j] : Gradient1(),
                            ddw1 ? this->f_ddw1.getValue()[i][j] : Hessian1() );
                    this->jacobian1[i].push_back(b);
            }

            size_t nbref2 = this->f_index2.getValue()[i].size();
            for(size_t j=0; j<nbref2; j++ )
            {
//                unsigned int index=this->f_index2.getValue()[i][j];
                    BlockType2 b;
//                    b.init( in1[index],
//                            out[i],
//                            this->f_pos0.getValue()[i],
//                            F0 ? this->f_F0.getValue()[i] : FI,
//                            this->f_w2.getValue()[i][j],
//                            dw2 ? this->f_dw2.getValue()[i][j] : Gradient2(),
//                            ddw2 ? this->f_ddw2.getValue()[i][j] : Hessian2() );
                    b.init( F0 ? this->f_F0.getValue()[i] : FI,
                            this->f_w2.getValue()[i][j],
                            dw2 ? this->f_dw2.getValue()[i][j] : Gradient2(),
                            ddw2 ? this->f_ddw2.getValue()[i][j] : Hessian2() );
                    this->jacobian2[i].push_back(b);
            }
        }
    }

};




} // namespace mapping
} // namespace component
} // namespace sofa

#endif

