/******************************************************************************
*       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_LinearJacobianBlock_point_INL
#define LINEARSUBSPACE_LinearJacobianBlock_point_INL

#include "LSJacobianBlock.h"

#include <sofa/defaulttype/Vec.h>
#include <sofa/defaulttype/Mat.h>
#include <sofa/defaulttype/VecTypes.h>
#include <Flexible/types/AffineTypes.h>
#include <Flexible/deformationMapping/LinearJacobianBlock_point.inl>

namespace sofa
{

namespace defaulttype
{

//////////////////////////////////////////////////////////////////////////////////
////  Vec3 -> Vec3
//////////////////////////////////////////////////////////////////////////////////

template<class InReal,class OutReal>
class LSJacobianBlock< V3(InReal) , V3(OutReal) > :
    public  BaseJacobianBlock< V3(InReal) , V3(OutReal) >
{
    public:
    typedef V3(InReal) In;
    typedef V3(OutReal) Out;

    enum { dim = Out::spatial_dimensions };

    typedef BaseJacobianBlock<In,Out> Inherit;
    typedef typename Inherit::InCoord InCoord;
    typedef typename Inherit::InDeriv InDeriv;
    typedef typename Inherit::OutCoord OutCoord;
    typedef typename Inherit::OutDeriv OutDeriv;
    typedef typename Inherit::MatBlock MatBlock;
    typedef typename Inherit::KBlock KBlock;
    typedef typename Inherit::Real Real;

    typedef Real Weight;

    typedef Vec<dim,Real> Gradient;
    typedef Mat<dim,dim,Real> Hessian;
    typedef Vec<dim, Real> SpatialCoord;
    typedef Mat<dim,dim,Real> MaterialToSpatial;

    static const bool constant=true;

    Real w;

    void init( const MaterialToSpatial& /*M*/, const Weight& w, const Gradient& /*dw*/, const Hessian& /*ddw*/)
    {
        this->w = w;
    }

    void addapply( OutCoord& result, const InCoord& data )
    {
        result +=  data * w;
    }

    void addmult( OutDeriv& result,const InDeriv& data )
    {
        result += data * w ;
    }

    void addMultTranspose( InDeriv& result, const OutDeriv& data )
    {
        result += data * w ;
    }

    MatBlock getJ()
    {
        MatBlock J = MatBlock();
        for(unsigned int i=0; i<dim; i++)
            J(i,i) = w;
        return J;
    }

    // no geometric striffness (constant J)
    KBlock getK(const OutDeriv& /*childForce*/, bool=false) {return KBlock();}
    void addDForce( InDeriv& /*df*/, const InDeriv& /*dx*/,  const OutDeriv& /*childForce*/, const SReal& /*kfactor */) {}


};

//////////////////////////////////////////////////////////////////////////////////
////  Vec3 -> ExtVec3   same as Vec3 -> Factorize using partial instanciation ?
//////////////////////////////////////////////////////////////////////////////////

template<class InReal,class OutReal>
class LSJacobianBlock< V3(InReal) , EV3(OutReal) > :
    public  BaseJacobianBlock< V3(InReal) , EV3(OutReal) >
{
public:
    typedef V3(InReal) In;
    typedef EV3(OutReal) Out;

    typedef BaseJacobianBlock<In,Out> Inherit;
    typedef typename Inherit::InCoord InCoord;
    typedef typename Inherit::InDeriv InDeriv;
    typedef typename Inherit::OutCoord OutCoord;
    typedef typename Inherit::OutDeriv OutDeriv;
    typedef typename Inherit::MatBlock MatBlock;
    typedef typename Inherit::KBlock KBlock;
    typedef typename Inherit::Real Real;

    typedef Real Weight;

    enum { dim = Out::spatial_dimensions };

    typedef Vec<dim,Real> Gradient;
    typedef Mat<dim,dim,Real> Hessian;
    typedef Vec<dim, Real> SpatialCoord;
    typedef Mat<dim,dim,Real> MaterialToSpatial;

    /**
    Mapping:   \f$ p = w.t + w.(p0-t0)  \f$
    where :
        - t0 is t in the reference configuration,
        - p0 is the position of p in the reference configuration.

    Jacobian:    \f$ dp = w.dt \f$
      */

    static const bool constant=true;

    Real w;

    void init( const MaterialToSpatial& /*M*/, const Weight& w, const Gradient& /*dw*/, const Hessian& /*ddw*/)
    {
        this->w = w;
    }

    void addapply( OutCoord& result, const InCoord& data )
    {
        result +=  data * w;
    }

    void addmult( OutDeriv& result,const InDeriv& data )
    {
        result += data * w ;
    }

    void addMultTranspose( InDeriv& result, const OutDeriv& data )
    {
        result += data * w ;
    }

    MatBlock getJ()
    {
        MatBlock J = MatBlock();
        for(unsigned int i=0; i<dim; i++)
            J(i,i)=w;
        return J;
    }

    // no geometric striffness (constant J)
    KBlock getK(const OutDeriv& /*childForce*/, bool=false) {return KBlock();}
    void addDForce( InDeriv& /*df*/, const InDeriv& /*dx*/,  const OutDeriv& /*childForce*/, const SReal& /*kfactor */) {}
};




//////////////////////////////////////////////////////////////////////////////////
////  Vec3 -> F331
//////////////////////////////////////////////////////////////////////////////////

template<class InReal,class OutReal>
class LSJacobianBlock< V3(InReal) , F331(OutReal) > :
    public  BaseJacobianBlock< V3(InReal) , F331(OutReal) >
{
    public:
    typedef V3(InReal) In;
    typedef F331(OutReal) Out;

    enum { dim = Out::spatial_dimensions };
    enum { mdim = Out::material_dimensions };

    typedef BaseJacobianBlock<In,Out> Inherit;
    typedef typename Inherit::InCoord InCoord;
    typedef typename Inherit::InDeriv InDeriv;
    typedef typename Inherit::OutCoord OutCoord;
    typedef typename Inherit::OutDeriv OutDeriv;
    typedef typename Inherit::MatBlock MatBlock;
    typedef typename Inherit::KBlock KBlock;
    typedef typename Inherit::Real Real;

    typedef Real Weight;

    typedef Vec<dim,Real> Gradient;
    typedef Mat<dim,dim,Real> Hessian;
    typedef Vec<dim, Real> SpatialCoord;
    typedef Mat<dim,dim,Real> MaterialToSpatial;

    static const bool constant=true;

    Real w;

    OutCoord C;       ///< =  (p0-t0).grad w.M + w.M   =  constant term
    SpatialCoord Ft;  ///< =   grad w.M     =  d F/dt



    void init( const MaterialToSpatial& M, const Weight& w, const Gradient& dw, const Hessian& /*ddw*/)
    {
//        this->w = w;

        Ft=M.transposed()*dw;
//        Ft=SpatialCoord(1,1,1);
//        C.getF()=covMN(SPos-InPos,Ft);
        C.getF()=M*w;
//        C.getF()=covMN(Ft,Ft);
    }

    void addapply( OutCoord& result, const InCoord& data )
    {
//        result.getF() +=  covMN(data, InCoord(0,0,0));

        result.getF() +=  covMN(data,Ft) + C.getF();

    }

    void addmult( OutDeriv& result,const InDeriv& data )
    {
//        result +=  covMN(data,w);
        result.getF() +=  covMN(data,Ft);
    }

    void addMultTranspose( InDeriv& result, const OutDeriv& data )
    {
//        result +=  covMN(data,w);
        result +=  data.getF() *Ft;
    }

    MatBlock getJ()
    {
        MatBlock J = MatBlock();
        for(unsigned int i=0; i<dim; i++)
            J(i,i) = w;
        return J;
    }

    // no geometric striffness (constant J)
    KBlock getK(const OutDeriv& /*childForce*/, bool=false) {return KBlock();}
    void addDForce( InDeriv& /*df*/, const InDeriv& /*dx*/,  const OutDeriv& /*childForce*/, const SReal& /*kfactor */) {}


};


}
}
#endif
