/******************************************************************************
*       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_ENGINE_JOINTBIASIMAGE_H
#define SOFA_COMPONENT_ENGINE_JOINTBIASIMAGE_H

#include <sofa/core/DataEngine.h>
#include <sofa/defaulttype/Vec.h>
#include <sofa/helper/vector.h>
#include <sofa/core/topology/Topology.h>

#include "tinyspline/tinysplinecpp.h"

namespace sofa
{

namespace component
{

namespace engine
{

/**
 * This engine computes a polyline (positions + edges) which is the sampling of a B-spline.
 * When initSpline data is not empty, the spline is initialized as a sequence of Bezier curves of degree 3.
 *
 * This engine depends on tinyspline
 * https://github.com/retuxx/tinyspline
 *
 * @author Thomas Lemaire @date 2015
 */
template <class DataTypes>
class SplineEngine : public core::DataEngine
{
public:
    typedef core::DataEngine Inherited;
    SOFA_CLASS(SOFA_TEMPLATE(SplineEngine, DataTypes), Inherited);

    typedef typename DataTypes::Real Real;
    typedef helper::ReadAccessor<Data< Real > > raReal;
    typedef typename DataTypes::Coord Coord;
    enum { dim = Coord::spatial_dimensions };
    typedef typename DataTypes::VecCoord VecCoord;
    typedef helper::ReadAccessor<Data< VecCoord > > raVecCoord;
    typedef helper::WriteOnlyAccessor<Data< VecCoord > > waVecCoord;
    typedef sofa::core::topology::Topology::Edge Edge;
    typedef sofa::helper::vector< Edge > SeqEdges;
    typedef helper::WriteOnlyAccessor<Data< SeqEdges > > waSeqEdges;

    Data< VecCoord > d_initSpline;
    Data< VecCoord > d_controlPoints;
    Data< std::size_t > d_degree;
    Data< bool > d_useBezier;
    Data< Real > d_ds;
    Data< VecCoord > d_position;
    Data< SeqEdges > d_edges;

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

    SplineEngine() : Inherited()
      , d_initSpline( initData(&d_initSpline, VecCoord(), "initSpline", "at reinit() fit an degree 3 spline to these points"))
      , d_controlPoints( initData(&d_controlPoints, VecCoord(), "controlPoints", "spline control points"))
      , d_degree( initData(&d_degree, (std::size_t)3, "degree", "degree of the spline"))
      , d_useBezier( initData(&d_useBezier, false, "useBezier", "use Bezier curve instead of b-spline"))
      , d_ds( initData(&d_ds, (Real)0.1, "ds", "sampling step"))
      , d_position( initData(&d_position, VecCoord(), "position", "sampled position on the spline"))
      , d_edges( initData(&d_edges, SeqEdges(), "edges", "spline edges"))
      , m_spline()
    {
        d_ds.setReadOnly(true);
    }

    ~SplineEngine() {}

    virtual void init()
    {
        addInput(&d_controlPoints);
        addInput(&d_ds);
        addOutput(&d_position);
        addOutput(&d_edges);
        setDirtyValue();
        reinit();
    }

    virtual void reinit() {
        raVecCoord initSpline(d_initSpline);
        if (!initSpline.empty()) {
            // c++ to c...
            std::vector<float> points(dim*initSpline.size());
            for (std::size_t i=0; i<initSpline.size(); ++i) {
                for (std::size_t j=0; j<dim; ++j)
                    points[i*dim+j]=initSpline[i][j];
            }
            try {
                // interpolate points into a spline
                tsBSpline interpolatedSpline;
                tsError err = ts_bspline_interpolate(points.data(), initSpline.size(), dim, &interpolatedSpline);
                if (err!=TS_SUCCESS) {
                    serr << ts_enum_str(err) << sendl;
                    return;
                }
                ts_bspline_copy(&interpolatedSpline, m_spline.data());
            }
            catch(std::exception& e) {
                serr << e.what() << sendl;
                return;
            }
            // update spline parameters
            d_useBezier.setValue(true);
            d_degree.setValue(m_spline.deg());
            // c to c++...
            waVecCoord controlPoints(d_controlPoints);
            controlPoints.clear();
            Coord pt;
            std::vector<float> ctrlp = m_spline.ctrlp();
            std::size_t i=0;
            while (i<m_spline.nCtrlp()) {
                for (std::size_t j=0; j<dim; ++j)
                    pt[j]=ctrlp[i*dim+j];
                controlPoints.push_back(pt);
                ++i;
                // skip bezier curve segments end points which are duplicated
                if (i % m_spline.order() == 0)
                    ++i;
            }
            sout << "Spline initialized from " << initSpline.size() << " initial points with " << controlPoints.size() << " control points" <<sendl;
        }
        else {
            try {
                m_spline = ts::BSpline(d_degree.getValue(), dim, d_controlPoints.getValue().size(), d_useBezier.getValue() ? TS_BEZIERS : TS_CLAMPED);
            }
            catch(std::exception& e) {
                serr << e.what() << sendl;
                return;
            }
            sout << "Empty spline initialized with " << d_controlPoints.getValue().size() << " control points" << sendl;
        }
        update();
    }

    void update()
    {
        cleanDirty();

        raVecCoord controlPoints(d_controlPoints);
        if (!d_useBezier.getValue() && controlPoints.size() != m_spline.nCtrlp()) {
            serr << "Spline and controlPoints are not consistant " << controlPoints.size() << "!=" << m_spline.nCtrlp() << sendl;
            return;
        }
        raReal ds(d_ds);
        waVecCoord position(d_position);
        waSeqEdges edges(d_edges);
        position.clear();
        edges.clear();
        std::vector<float> ctrlp(m_spline.nCtrlp()*dim);
        if (!d_useBezier.getValue()) {
            for (std::size_t i=0; i<m_spline.nCtrlp(); ++i) {
                for (std::size_t j=0; j<dim; ++j)
                    ctrlp[i*dim+j] = controlPoints[i][j];
            }
        }
        else {
            // for bezier, bezier curve segments end points need to be duplicated
            std::size_t iTsCtrlp=0, iCtrlp=0;
            while (iTsCtrlp<m_spline.nCtrlp()) {
                for (std::size_t j=0; j<dim; ++j)
                    ctrlp[iTsCtrlp*dim+j] = controlPoints[iCtrlp][j];
                ++iTsCtrlp;
                if (iCtrlp>0 && iCtrlp % (m_spline.order()-1) == 0 && iTsCtrlp<m_spline.nCtrlp()) {
                    for (std::size_t j=0; j<dim; ++j)
                        ctrlp[iTsCtrlp*dim+j] = controlPoints[iCtrlp][j];
                    ++iTsCtrlp;
                }
                ++iCtrlp;
            }
        }
        try {
            m_spline.setCtrlp(ctrlp);
            std::vector<float> res;
            Coord point;
            Real s=0.;
            unsigned int pointIndex=0;
            while (s<=1.+ds) {
                res = m_spline.evaluate(s).result();
                for (std::size_t i=0; i<dim; ++i)
                    point[i] = res[i];
                position.push_back(point);
                if (pointIndex>0)
                    edges.push_back(Edge(pointIndex-1, pointIndex));
                ++pointIndex;
                s+=ds;
            }
        }
        catch(std::exception& e) {
            serr << e.what() << sendl;
        }

    }

private:
    // the spline object
    ts::BSpline m_spline;
    // index convertion from the engine control points to tiny spline control points
    // to remove duplicates in the case of joint bezier curve segments
    std::vector<std::size_t> m_ctrlpToTsCtrlpIndex;

};

} // namespace engine

} // namespace component

} // namespace sofa

#endif // SOFA_COMPONENT_ENGINE_JOINTBIASIMAGE_H
