#ifndef MAKE_STRAIGHT_ROD_HH
#define MAKE_STRAIGHT_ROD_HH

#include <vector>
#include <dune/common/fvector.hh>
#include <dune/fufem/crossproduct.hh>

#include "rigidbodymotion.hh"
#include <dune/gfe/localgeodesicfefunction.hh>

/** \brief A factory class that implements various ways to create rod configurations
 */

template <class GridView>
class RodFactory
{
    dune_static_assert(GridView::dimensionworld==1, "RodFactory is only implemented for grids in a 1d world");
    
public:

    RodFactory(const GridView& gridView)
    : gridView_(gridView)
    {}
    
/** \brief Make a straight, unsheared rod from two given endpoints

\param[out] rod The new rod
\param[in] n The number of vertices
*/
template <int dim>
    void create(std::vector<RigidBodyMotion<dim> >& rod,
                     const Dune::FieldVector<double,3>& beginning, const Dune::FieldVector<double,3>& end)
{
    // Compute the correct orientation
    Rotation<3,double> orientation = Rotation<3,double>::identity();

    Dune::FieldVector<double,3> zAxis(0);
    zAxis[2] = 1;
    Dune::FieldVector<double,3> axis = crossProduct(Dune::FieldVector<double,3>(end-beginning), zAxis);
    if (axis.two_norm() != 0)
        axis /= -axis.two_norm();

    Dune::FieldVector<double,3> d3 = end-beginning;
    d3 /= d3.two_norm();

    double angle = std::acos(zAxis * d3);

    if (angle != 0)
        orientation = Rotation<3,double>(axis, angle);

        // Set the values
        create(rod, RigidBodyMotion<dim>(beginning,orientation), RigidBodyMotion<dim>(end,orientation));
}


/** \brief Make a rod by interpolating between two end configurations

\param[out] rod The new rod
*/
    template <int spaceDim>
    void create(std::vector<RigidBodyMotion<spaceDim> >& rod,
                     const RigidBodyMotion<3,double>& beginning,
                     const RigidBodyMotion<3,double>& end)
{
    
    static const int dim = GridView::dimension;  // de facto: 1
    
    //////////////////////////////////////////////////////////////////////////////////////////////
    //  Get smallest and largest coordinate, in order to create an arc-length parametrization
    //////////////////////////////////////////////////////////////////////////////////////////////
    
    typename GridView::template Codim<dim>::Iterator vIt    = gridView_.template begin<dim>();
    typename GridView::template Codim<dim>::Iterator vEndIt = gridView_.template end<dim>();
    
    double min =  std::numeric_limits<double>::max();
    double max = -std::numeric_limits<double>::max();
    
    for (; vIt != vEndIt; ++vIt) {
        min = std::min(min, vIt->geometry().corner(0)[0]);
        max = std::max(max, vIt->geometry().corner(0)[0]);
    }
    
    ////////////////////////////////////////////////////////////////////////////////////
    //  Interpolate according to arc-length
    ////////////////////////////////////////////////////////////////////////////////////

    rod.resize(gridView_.size(dim));
    
    for (vIt = gridView_.template begin<dim>(); vIt != vEndIt; ++vIt) {
        int idx = gridView_.indexSet().index(*vIt);
        Dune::FieldVector<double,1> local = (vIt->geometry().corner(0)[0] - min) / (max - min);

        for (int i=0; i<3; i++)
            rod[idx].r[i] = (1-local)*beginning.r[i] + local*end.r[i];
        rod[idx].q = Rotation<3,double>::interpolate(beginning.q, end.q, local);
    }
}

    /** \brief Make a rod by setting each entry to the same value

    \param[out] rod The new rod
    */
    template <int spaceDim>
    void create(std::vector<RigidBodyMotion<spaceDim> >& rod,
                const RigidBodyMotion<spaceDim,double>& value)
    {
        rod.resize(gridView_.size(1));
        std::fill(rod.begin(), rod.end(), value);
    }

    /** \brief Make a rod by linearly interpolating between the end values

        \note The end values are expected to be in the input container!
        \param[in,out] rod The new rod
    */
    template <int spaceDim>
    void create(std::vector<RigidBodyMotion<spaceDim> >& rod)
    {
        static const int dim = GridView::dimension;  // de facto: 1
        assert(gridView_.size(dim)==rod.size());

        //////////////////////////////////////////////////////////////////////////////////////////////
        //  Get smallest and largest coordinate, in order to create an arc-length parametrization
        //////////////////////////////////////////////////////////////////////////////////////////////
    
        typename GridView::template Codim<dim>::Iterator vIt    = gridView_.template begin<dim>();
        typename GridView::template Codim<dim>::Iterator vEndIt = gridView_.template end<dim>();
    
        double min =  std::numeric_limits<double>::max();
        double max = -std::numeric_limits<double>::max();
        RigidBodyMotion<spaceDim> beginning, end;
    
        for (; vIt != vEndIt; ++vIt) {
            if (vIt->geometry().corner(0)[0] < min) {
                min = vIt->geometry().corner(0)[0];
                beginning = rod[gridView_.indexSet().index(*vIt)];
            }
            if (vIt->geometry().corner(0)[0] > max) {
                max = vIt->geometry().corner(0)[0];
                end = rod[gridView_.indexSet().index(*vIt)];
            }
        }
    
        ////////////////////////////////////////////////////////////////////////////////////
        //  Interpolate according to arc-length
        ////////////////////////////////////////////////////////////////////////////////////

        rod.resize(gridView_.size(dim));
    
        for (vIt = gridView_.template begin<dim>(); vIt != vEndIt; ++vIt) {
            int idx = gridView_.indexSet().index(*vIt);
            Dune::FieldVector<double,1> local = (vIt->geometry().corner(0)[0] - min) / (max - min);

            for (int i=0; i<3; i++)
                rod[idx].r[i] = (1-local)*beginning.r[i] + local*end.r[i];
            rod[idx].q = Rotation<3,double>::interpolate(beginning.q, end.q, local);
        }
    }

private:
    
    const GridView& gridView_;
};

#endif