 
/******************************************************************************
*       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                             *
******************************************************************************/

#include "ProgressiveTarget.h"

#include <sofa/defaulttype/RigidTypes.h>

namespace sofa
{

namespace component
{

namespace engine
{

template <class DataTypes>
ProgressiveTarget<DataTypes>::ProgressiveTarget()
    : d_current(initData(&d_current, "current", "current positions"))
    , d_target(initData(&d_target, "target", "target positions"))
    , d_step(initData(&d_step, helper::vector<Real>(1, 1e-3), "step", "step increment, if size is 1 the same value used for all particules, step[0] is for translation, step[1] is for rotation"))
    , d_output(initData(&d_output, "output", "intermediate target positions"))
{ }

template <class DataTypes>
ProgressiveTarget<DataTypes>::~ProgressiveTarget()
{ }

template <class DataTypes>
void ProgressiveTarget<DataTypes>::init()
{
    addInput(&d_current);
    addInput(&d_target);
    addInput(&d_step);

    addOutput(&d_output);

    reinit();
}

template <class DataTypes>
void ProgressiveTarget<DataTypes>::reinit()
{
    setDirtyValue();
    if (0 == d_target.getValue().size())
        d_target.setValue(d_current.getValue());
}

template <class DataTypes>
void ProgressiveTarget<DataTypes>::update()
{
    cleanDirty();

    helper::ReadAccessor<Data<VecCoord> > current(d_current);
    helper::ReadAccessor<Data<VecCoord> > target(d_target);
    helper::ReadAccessor<Data< helper::vector<Real> > > step(d_step);

    assert( current.size() == target.size() );
    assert( 1 == step.size() || current.size() == step.size());

    helper::WriteAccessor< Data<VecCoord> > output(d_output);

    output.resize(current.size());
    for (unsigned int i=0 ; i<current.size() ; ++i) {
        Coord v = target[i]-current[i];
        if ( v.norm() < step[1 == step.size() ? 0 : i] ) {
            // this target is nearly reached
            output[i] = target[i];
        }
        else {
            v.normalize();
            output[i] = current[i] + v*step[1 == step.size() ? 0 : i];
        }
    }
}



template <class DataTypes>
std::string ProgressiveTarget<DataTypes>::getTemplateName() const
{
    return templateName(this);
}

template <class DataTypes>
std::string ProgressiveTarget<DataTypes>::templateName(const ProgressiveTarget<DataTypes>*)
{
    return DataTypes::Name();
}

/*
 *  specific implementation for RigidTypes
 */

template <>
void ProgressiveTarget<sofa::defaulttype::Rigid3dTypes>::update()
{
    typedef sofa::defaulttype::Rigid3dTypes::Vec3 Vec3;
    typedef sofa::defaulttype::Rigid3dTypes::Quat Quat;

    cleanDirty();

    helper::ReadAccessor<Data<VecCoord> > current(d_current);
    helper::ReadAccessor<Data<VecCoord> > target(d_target);
    helper::ReadAccessor<Data< helper::vector<Real> > > step(d_step);

    assert( current.size() == target.size() );
    assert( 2 == step.size() );

    helper::WriteAccessor< Data<VecCoord> > output(d_output);

    output.resize(current.size());

    for (unsigned int i=0 ; i<current.size() ; ++i) {
        // translation part of the rigid target
        Vec3 v = target[i].getCenter()-current[i].getCenter();
        if ( v.norm() < step[0] ) {
            // this target is nearly reached
            output[i].getCenter() = target[i].getCenter();
        }
        else {
            v.normalize();
            output[i].getCenter() = current[i].getCenter() + v*step[0];
        }

        // orientation part of the rigid target
        Vec3 rot = (current[i].getOrientation().inverse() * target[i].getOrientation()).quatToRotationVector();
        if (  rot.norm() < step[1] ) {
            // this target is nearly reached
            output[i].getOrientation() = target[i].getOrientation();
        }
        else {
            rot.normalize();
            output[i].getOrientation() = current[i].getOrientation() * Quat::createFromRotationVector(rot*step[1]);
        }
    }

}

} // namespace engine

} // namespace component

} // namespace sofa
