/******************************************************************************
*       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_WeightMultiTranslationEngine_H
#define LINEARSUBSPACE_WeightMultiTranslationEngine_H


#include "LinearSubspace/config.h"

#include <sofa/helper/rmath.h>
#include <sofa/helper/OptionsGroup.h>
#include <sofa/helper/vector.h>
#include <sofa/helper/SVector.h>

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

#include <algorithm>
#include <iostream>
#include <map>
#include <string>

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


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 engine
{

/**
    This Engine translates weights from the weight matrix computed in JacobsonWeightComputationEngine to weights used by LSMultiMapping.
*/
template <typename DataTypes>
class SOFA_LinearSubspace_API WeightMultiTranslationEngine : public core::DataEngine
{

public:
    typedef core::DataEngine Inherited;
    SOFA_CLASS(SOFA_TEMPLATE(WeightMultiTranslationEngine, DataTypes) , Inherited);


    typedef uint Ref;
    typedef helper::vector<Ref> VRef;
    typedef helper::vector<helper::SVector<uint>> VecVRef;

    typedef typename DataTypes::Real Real;
    typedef Real Weight1;
    typedef helper::vector<Weight1> VWeight1;
    typedef helper::vector<helper::SVector<Weight1>> VecVWeight1;

    typedef defaulttype::Vec<4,Real> Vec4;
    typedef Vec4 Weight2;
    typedef helper::vector<Weight2> VWeight2;
    typedef helper::vector<helper::SVector<Weight2>> VecVWeight2;

    typedef Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic> DsMat;



    /// Input
    Data<DsMat> d_W;
    Data<uint> d_nbPosConstraints;

    /// Output
    Data<VecVRef> d_index1;
    Data<VecVRef> d_index2;
    Data<VecVWeight1> d_weight1;
    Data<VecVWeight2> d_weight2;


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

    virtual void init()
    {
        setDirtyValue();
    }

    virtual void reinit()
    {
        update();
    }

    void update()
    {
        helper::ReadAccessor< Data<DsMat>> W_accessor(d_W);
        const DsMat& W = W_accessor.ref();

        const uint nbPosConstraints = d_nbPosConstraints.getValue();

        helper::WriteOnlyAccessor<Data<VecVRef>> index1_accessor(d_index1);
        VecVRef& index1 = index1_accessor.wref();

        helper::WriteOnlyAccessor<Data<VecVRef>> index2_accessor(d_index2);
        VecVRef& index2 = index2_accessor.wref();

        helper::WriteOnlyAccessor<Data<VecVWeight1>> weight1_accessor(d_weight1);
        VecVWeight1& weight1 = weight1_accessor.wref();

        helper::WriteOnlyAccessor<Data<VecVWeight2>> weight2_accessor(d_weight2);
        VecVWeight2& weight2 = weight2_accessor.wref();



        uint nbVerts = W.rows();
        uint nbScalarParents = nbPosConstraints;
        uint nbVectorialParents = (W.cols() - nbScalarParents)/4;

        assert((W.cols() - nbScalarParents)%4 == 0);

//        std::cout << "[WeightMultiTranslationEngine]: START.\n nbVerts=" << nbVerts << ", nbParents" << nbParents << std::endl;

        index1.resize(nbVerts);
        index2.resize(nbVerts);
        weight1.resize(nbVerts);
        weight2.resize(nbVerts);

        for(uint i=0; i<nbVerts; i++)
        {
            index1[i].resize(nbScalarParents);
            weight1[i].resize(nbScalarParents);
            for(uint j=0; j<nbScalarParents; j++)
            {
                index1[i][j] = j;
                weight1[i][j] = W(i, j);
            }

            index2[i].resize(nbVectorialParents);
            weight2[i].resize(nbVectorialParents);
            for(uint j=0; j<nbVectorialParents; j++)
            {
                index2[i][j] = j;
                for(uint k=0; k<4; ++k)
                {
                    weight2[i][j][k] = W(i, nbScalarParents + j*4+k);
                }
            }
        }

//        std::cout << "[WeightMultiTranslationEngine]: DONE.\n indices.size()=" << indices.size() << ", weights.size()" << weights.size() << std::endl;
    }


protected:
    WeightMultiTranslationEngine() : Inherited()
      , d_W(initData(&d_W, DsMat(), "W", "Weight matrix"))
      , d_nbPosConstraints(initData(&d_nbPosConstraints, uint(), "nbPosConstraints", "Number of position constraint (as opposed to region constraints)."))
      , d_index1(initData(&d_index1, VecVRef(),"index1","for each scalar weight, index of the related parent."))
      , d_index2(initData(&d_index2, VecVRef(),"index2","for each vectorial weight, index of the related parent."))
      , d_weight1(initData(&d_weight1, VecVWeight1(),"weight1","scalar weights"))
      , d_weight2(initData(&d_weight2, VecVWeight2(),"weight2","vectorial weights"))
    {
        addInput(&d_W);
        addInput(&d_nbPosConstraints);

        addOutput(&d_index1);
        addOutput(&d_index2);
        addOutput(&d_weight1);
        addOutput(&d_weight2);
    }

    ~WeightMultiTranslationEngine()
    {

    }

};




}
}
}


#endif
