/******************************************************************************
*       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 <BranchingImage/BranchingImage.h>

namespace sofa
{

namespace component
{

namespace engine
{

/**
 * Bias the input bias image with the joints.
 * Each joint influences a sphere around its center, multiplying the input value with jointBias
 *
 * @author Thomas Lemaire @date 2015
 */
template <class _BranchingImageTypes>
class JointBiasImage : public core::DataEngine
{
public:
    typedef core::DataEngine Inherited;
    SOFA_CLASS(SOFA_TEMPLATE(JointBiasImage, _BranchingImageTypes), Inherited);

    typedef _BranchingImageTypes BranchingImageTypes;
    typedef typename BranchingImageTypes::T T;

    typedef defaulttype::ImageLPTransform<SReal> TransformType;
    typedef helper::ReadAccessor<Data< TransformType > > raTransform;
    typedef defaulttype::ImageLPTransform<SReal>::Coord Coord;

    typedef helper::ReadAccessor<Data< BranchingImageTypes > > raImage;
    typedef helper::WriteOnlyAccessor<Data< BranchingImageTypes > > waImage;

    typedef sofa::defaulttype::Vector3 Vector3;


    Data< BranchingImageTypes > d_inputBiasImage;
    Data< TransformType > d_imageTransform;
    typedef helper::vector<Vector3> VecPosition;
    typedef helper::ReadAccessor<Data< VecPosition > > raVecPosition;
    Data< VecPosition > d_jointCenters;
    Data< SReal > d_sphereRadius;
    Data< SReal > d_jointBias;
    Data< BranchingImageTypes > d_outputBiasImage;

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

    JointBiasImage() : Inherited()
      , d_inputBiasImage( initData(&d_inputBiasImage, BranchingImageTypes(), "inputBiasImage", ""))
      , d_imageTransform( initData(&d_imageTransform, "imageTransform", ""))
      , d_jointCenters(initData(&d_jointCenters, VecPosition(), "jointCenters", ""))
      , d_sphereRadius(initData(&d_sphereRadius, (SReal)0., "sphereRadius", ""))
      , d_jointBias(initData(&d_jointBias, (SReal)1., "biasInSphere", ""))
      , d_outputBiasImage(initData(&d_outputBiasImage, BranchingImageTypes(), "outputBiasImage", ""))
    {
        d_inputBiasImage.setReadOnly(true);
        d_imageTransform.setReadOnly(true);
        d_jointCenters.setReadOnly(true);
        d_sphereRadius.setReadOnly(true);
        d_jointBias.setReadOnly(true);
    }

    ~JointBiasImage() {}

    virtual void init()
    {
        addInput(&d_inputBiasImage);
        addInput(&d_imageTransform);
        addInput(&d_jointCenters);
        addInput(&d_sphereRadius);
        addInput(&d_jointBias);
        addOutput(&d_outputBiasImage);
        setDirtyValue();
    }

    virtual void reinit() {update();}

    void update()
    {
        raImage raInputBiasImage(d_inputBiasImage);
        waImage waOutputBiasImage(d_outputBiasImage);
        raTransform transform(d_imageTransform);
        raVecPosition jointCenters(d_jointCenters);

        cleanDirty();

        if (raInputBiasImage.ref().isEmpty()) return;

        waOutputBiasImage.wref() = raInputBiasImage.ref(); // init values

        if (d_jointCenters.getValue().empty()) return;

        SReal radius2 = d_sphereRadius.getValue()*d_sphereRadius.getValue();

        bimg_forT(waOutputBiasImage.wref(),t) {
            typename BranchingImageTypes::BranchingImage3D & outputBiasImage = waOutputBiasImage.wref().imgList[t];
            bimg_forXYZ(waOutputBiasImage.wref(), x,y,z) {
                Coord pt = transform->fromImage(Coord(x,y,z));
                bool isPtToBeBiased = false;
                for (unsigned int i=0; i<jointCenters.size(); ++i) {
                    if ((pt-jointCenters[i]).norm2() < radius2) {
                        isPtToBeBiased = true;
                        break;
                    }
                }
                if (isPtToBeBiased) {
                    typename BranchingImageTypes::VoxelIndex index(waOutputBiasImage->index3Dto1D(x,y,z),0);
                    // bias all suerimposed voxels
                    for(; index.offset<outputBiasImage[index.index1d].size() ; ++index.offset) {
                        outputBiasImage(index)[0]*=d_jointBias.getValue(); // only 1 channel images
                    }
                }
            }
        }
        std::cerr << std::endl;


    }


};

} // namespace engine

} // namespace component

} // namespace sofa

#endif // SOFA_COMPONENT_ENGINE_JOINTBIASIMAGE_H
