/******************************************************************************
*       SOFA, Simulation Open-Framework Architecture, version 1.0 RC 1        *
*                (c) 2006-2011 INRIA, USTL, UJF, CNRS, MGH                    *
*                                                                             *
* This program is free software; you can redistribute it and/or modify it     *
* under the terms of the GNU General Public License as published by the Free  *
* Software Foundation; either version 2 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 General Public License for    *
* more details.                                                               *
*                                                                             *
* You should have received a copy of the GNU General Public License along     *
* with this program; if not, write to the Free Software Foundation, Inc., 51  *
* Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.                   *
*******************************************************************************
*                            SOFA :: Applications                             *
*                                                                             *
* Authors: The SOFA Team and external contributors (see Authors.txt)          *
*                                                                             *
* Contact information: contact@sofa-framework.org                             *
******************************************************************************/

/* Thomas Lemaire, 2014 */
#include <SofaTest/MultiMapping_test.h>
#include <SofaSimulationGraph/DAGSimulation.h>
#include <sofa/defaulttype/VecTypes.h>
#include <SofaBaseTopology/MeshTopology.h>
#include "../DistanceToMeshMultiMapping.h"
#include "../ProjectionToMeshEngine.h"

namespace sofa {
namespace {

using std::cout;
using std::cerr;
using std::endl;
using namespace core;
using namespace component;
using defaulttype::Vec;
using defaulttype::Mat;
using sofa::helper::vector;


/**  Test suite for DistanceToMeshMultiMapping.
  */
template <typename _DistanceToMeshMultiMapping>
struct DistanceToMeshMultiMappingTest : public MultiMapping_test<_DistanceToMeshMultiMapping>
{

    typedef _DistanceToMeshMultiMapping DistanceToMeshMultiMapping;

    typedef typename DistanceToMeshMultiMapping::In InDataTypes;
    typedef typename InDataTypes::VecCoord InVecCoord;
    typedef typename InDataTypes::VecDeriv InVecDeriv;
    typedef typename InDataTypes::Coord InCoord;
    typedef typename InDataTypes::Deriv InDeriv;
    typedef container::MechanicalObject<InDataTypes> InMechanicalObject;
    typedef typename InMechanicalObject::ReadVecCoord  ReadInVecCoord;
    typedef typename InMechanicalObject::WriteVecCoord WriteInVecCoord;
    typedef typename InMechanicalObject::WriteVecDeriv WriteInVecDeriv;
    typedef typename InDataTypes::Real InReal;
    typedef Mat<InDataTypes::spatial_dimensions,InDataTypes::spatial_dimensions,InReal> RotationMatrix;


    typedef typename DistanceToMeshMultiMapping::Out OutDataTypes;
    typedef typename OutDataTypes::VecCoord OutVecCoord;
    typedef typename OutDataTypes::VecDeriv OutVecDeriv;
    typedef typename OutDataTypes::Coord OutCoord;
    typedef typename OutDataTypes::Deriv OutDeriv;
    typedef container::MechanicalObject<OutDataTypes> OutMechanicalObject;
    typedef typename OutMechanicalObject::WriteVecCoord WriteOutVecCoord;
    typedef typename OutMechanicalObject::WriteVecDeriv WriteOutVecDeriv;
    typedef typename OutMechanicalObject::ReadVecCoord ReadOutVecCoord;
    typedef typename OutMechanicalObject::ReadVecDeriv ReadOutVecDeriv;

    component::topology::MeshTopology /**inTopology1,*/ *inTopology2;

    void setupScene() {
        this->MultiMapping_test<DistanceToMeshMultiMapping>::setupScene(2); // 2 parents, 1 child

//        inTopology1 = modeling::addNew<component::topology::MeshTopology>(this->parents[0]).get();
        inTopology2 = modeling::addNew<component::topology::MeshTopology>(this->parents[1]).get();

    }


    /** @name Test_Cases
      For each of these cases, we can test if the mapping work
      */
    ///@{
    bool test_null_distance(bool useTangentPlaneProjection)
    {
        this->setupScene();

        DistanceToMeshMultiMapping* mapping = static_cast<DistanceToMeshMultiMapping*>( this->mapping );
        mapping->f_printLog = true;

        vector< InVecCoord > incoords(2);
        // mesh1 - a single point
        incoords[0].resize(1);
        InDataTypes::set( incoords[0][0], 0,0,0 );

        // mesh2 - a triangle
        incoords[1].resize(3);
        InDataTypes::set( incoords[1][0], -1,0,0 );
        InDataTypes::set( incoords[1][1], 1,-1,0 );
        InDataTypes::set( incoords[1][2], 1,1,0 );
        inTopology2->addTriangle(0,1,2);



        component::engine::BaseProjectionToMeshEngine<InDataTypes> *baseProjectionToMeshEngine;

        if (useTangentPlaneProjection) {
            component::engine::ProjectionToVertexPlaneEngine<InDataTypes>* projectionToVertexPlaneEngine = modeling::addNew< component::engine::ProjectionToVertexPlaneEngine<InDataTypes> >(this->root).get();
            InVecCoord normals;
            normals.push_back(InCoord(0,0,1));
            normals.push_back(InCoord(0,0,1));
            normals.push_back(InCoord(0,0,1));
            projectionToVertexPlaneEngine->d_normals2.setValue(normals);
            baseProjectionToMeshEngine = projectionToVertexPlaneEngine;
        }
        else {
            component::engine::ProjectionToMeshEngine<InDataTypes>* projectionToMeshEngine = modeling::addNew< component::engine::ProjectionToMeshEngine<InDataTypes> >(this->root).get();
            projectionToMeshEngine->d_triangles2.setValue(inTopology2->getTriangles());
            baseProjectionToMeshEngine = projectionToMeshEngine;
        }


        mapping->d_closests.setParent( &baseProjectionToMeshEngine->o_closests );

        baseProjectionToMeshEngine->d_source.setParent( "@"+this->inDofs[0]->getPathName()+".position" );
        baseProjectionToMeshEngine->d_target.setParent( "@"+this->inDofs[1]->getPathName()+".position" );


        helper::vector<defaulttype::Vec2i> indexPairs;
        indexPairs.push_back(defaulttype::Vec2i(0,0));
        baseProjectionToMeshEngine->f_indexPairs.setValue(indexPairs);
        mapping->f_indexPairs.setValue(indexPairs);

        // expected child positions
        OutVecCoord outcoords(1);
        OutDataTypes::set( outcoords[0], 0, /*unused*/0, /*unused*/0);

        return this->runTest(incoords,outcoords);
    }
    ///@}

//    bool test_positive_distance()
//    {
//        this->setupScene();

//        DistanceToMeshMultiMapping* mapping = static_cast<DistanceToMeshMultiMapping*>( this->mapping );
//        mapping->f_printLog = true;

//        vector< InVecCoord > incoords(2);
//        // mesh1 - a single point
//        incoords[0].resize(1);
//        InDataTypes::set( incoords[0][0], 0,0,1 );

//        // mesh2 - a triangle
//        incoords[1].resize(3);
//        InDataTypes::set( incoords[1][0], -1,0,0 );
//        InDataTypes::set( incoords[1][1], 1,-1,0 );
//        InDataTypes::set( incoords[1][2], 1,1,0 );
//        inTopology2->addTriangle(0,1,2);

//        projectionToMeshEngine->d_source.setValue( incoords[0] );
//        projectionToMeshEngine->d_target.setValue( incoords[1] );

//        InVecCoord normals;
//        normals.push_back(InCoord(0,0,1));
//        normals.push_back(InCoord(0,0,1));
//        normals.push_back(InCoord(0,0,1));
//        projectionToMeshEngine->d_normals2.setValue(normals);

//        helper::vector<defaulttype::Vec2i> indexPairs;
//        indexPairs.push_back(defaulttype::Vec2i(0,0));
//        mapping->f_indexPairs.setValue(indexPairs);
//        projectionToMeshEngine->f_indexPairs.setValue(indexPairs);

//        // expected child positions
//        OutVecCoord outcoords(1);
//        OutDataTypes::set( outcoords[0], 1, /*unused*/0, /*unused*/0);

//        projectionToMeshEngine->init();
//        return this->runTest(incoords,outcoords);
//    }

//    bool test_negative_distance()
//    {
//        this->setupScene();

//        DistanceToMeshMultiMapping* mapping = static_cast<DistanceToMeshMultiMapping*>( this->mapping );

//        vector< InVecCoord > incoords(2);
//        // mesh1 - a single point
//        incoords[0].resize(1);
//        InDataTypes::set( incoords[0][0], 0,0,-1 );

//        // mesh2 - a triangle
//        incoords[1].resize(3);
//        InDataTypes::set( incoords[1][0], -1,0,0 );
//        InDataTypes::set( incoords[1][1], 1,-1,0 );
//        InDataTypes::set( incoords[1][2], 1,1,0 );
//        inTopology2->addTriangle(0,1,2);

//        projectionToMeshEngine->d_source.setValue( incoords[0] );
//        projectionToMeshEngine->d_target.setValue( incoords[1] );

//        InVecCoord normals;
//        normals.push_back(InCoord(0,0,1));
//        normals.push_back(InCoord(0,0,1));
//        normals.push_back(InCoord(0,0,1));
//        projectionToMeshEngine->d_normals2.setValue(normals);

//        helper::vector<defaulttype::Vec2i> indexPairs;
//        indexPairs.push_back(defaulttype::Vec2i(0,0));
//        mapping->f_indexPairs.setValue(indexPairs);
//        projectionToMeshEngine->f_indexPairs.setValue(indexPairs);

//        // expected child positions
//        OutVecCoord outcoords(1);
//        OutDataTypes::set( outcoords[0], -1, /*unused*/0, /*unused*/0);

//        projectionToMeshEngine->init();
//        return this->runTest(incoords,outcoords);
//    }

//    bool test_two_points()
//    {
//        this->setupScene();

//        DistanceToMeshMultiMapping* mapping = static_cast<DistanceToMeshMultiMapping*>( this->mapping );

//        vector< InVecCoord > incoords(2);
//        // mesh1 - a single point
//        incoords[0].resize(2);
//        InDataTypes::set( incoords[0][0], 0,0,1 );
//        InDataTypes::set( incoords[0][1], 0.5,0.5,0.75 );


//        // mesh2 - a triangle
//        incoords[1].resize(3);
//        InDataTypes::set( incoords[1][0], -1,0,0 );
//        InDataTypes::set( incoords[1][1], 1,-1,0 );
//        InDataTypes::set( incoords[1][2], 1,1,0 );
//        inTopology2->addTriangle(0,1,2);

//        projectionToMeshEngine->d_source.setValue( incoords[0] );
//        projectionToMeshEngine->d_target.setValue( incoords[1] );

//        InVecCoord normals;
//        normals.push_back(InCoord(0,0,1));
//        normals.push_back(InCoord(0,0,1));
//        normals.push_back(InCoord(0,0,1));
//        projectionToMeshEngine->d_normals2.setValue(normals);

//        mapping->d_restLength.setValue(1.0);
//        // expected child positions
//        OutVecCoord outcoords(2);
//        OutDataTypes::set( outcoords[0], 1, /*unused*/0, /*unused*/0);
//        OutDataTypes::set( outcoords[1], 0.75, /*unused*/0., /*unused*/0.);

//        projectionToMeshEngine->init();
//        return this->runTest(incoords,outcoords);
//    }

};


// Define the list of types to instanciate. We do not necessarily need to test all combinations.
using testing::Types;
typedef Types<
mapping::DistanceToMeshMultiMapping<defaulttype::Vec3Types,defaulttype::Vec1Types>
> DataTypes; // the types to instanciate.

// Test suite for all the instanciations
TYPED_TEST_CASE(DistanceToMeshMultiMappingTest, DataTypes);
TYPED_TEST( DistanceToMeshMultiMappingTest , one_point )
{
    ASSERT_TRUE(this->test_null_distance(true));
//    ASSERT_TRUE(this->test_one_point(false));
}
//TYPED_TEST( DistanceToMeshMultiMappingTest , positive )
//{
//    ASSERT_TRUE(this->test_positive());
//}
//TYPED_TEST( DistanceToMeshMultiMappingTest , negative )
//{
//    ASSERT_TRUE(this->test_negative());
//}
//TYPED_TEST( DistanceToMeshMultiMappingTest , two_points )
//{
//    ASSERT_TRUE(this->test_two_points());
//}

} // namespace
} // namespace sofa
