#ifndef __INTERPOLATION_H__
#define __INTERPOLATION_H__

#include <boost/numeric/ublas/matrix.hpp>
#include <vector>

using boost::numeric::ublas::vector;
using boost::numeric::ublas::matrix;
using boost::numeric::ublas::zero_matrix;

namespace {
enum polynome_type
{
    linear=0,
    monotonic=1,
    smooth=2
};

/**
 * Create interpolation polynomes between nodes
 **/
template<polynome_type> int interpolate ( boost::numeric::ublas::matrix<double> nodes,
   boost::numeric::ublas::vector<boost::numeric::ublas::matrix<double> >& polynomes );
}

// POLYNOME_TYPE
// Type of interpolation polynome
// 0 = linear (lines with positional continuity)
// 1 = monotonic (monotonic spline with tangential continuity)
// 2 = smooth (spline with curvature continuity)
#ifndef POLYNOME_TYPE
#define POLYNOME_TYPE 1
#endif

#if POLYNOME_TYPE == 0
namespace {
// integrating linear polynomes with one step
template<class Route, class State>
void integrate ( Route r, State& x , double t0 , double t1, const double dxdt, void (*write_out) ( const State& , const double ))
{
        write_out(x, t0);
        r(x, x, t0);
}

template<> inline
int interpolate<linear> ( matrix<double> nodes, vector<matrix<double> >& polynomes )
{
    const size_t size=nodes.size1();
    assert ( size>1 && "At least two nodes needed for interpolation" );

    // linear interpolation
    // calculate coefficients of linear polynomes
    polynomes.resize ( size-1 );
    for ( size_t i=0; i<size-1; ++i )
    {
        polynomes[i]=zero_matrix<double> ( 2, 2 );
        for ( size_t j = 0; j < polynomes[i].size1(); ++j )
        {
            polynomes[i] ( j, 0 ) =nodes ( i, j );
            polynomes[i] ( j, 1 ) =nodes ( i+1, j ) - nodes ( i, j );
        }
    }
}

}

#elif (POLYNOME_TYPE == 1) || (POLYNOME_TYPE == 2)

#include <boost/numeric/ublas/lu.hpp>
#include <boost/numeric/ublas/banded.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/odeint.hpp>

using boost::numeric::odeint::integrate;

namespace {

void hermite_matrix ( matrix<double>& );
void toeplitz_matrix ( matrix<double>&);

/**
 * Calculate the inverse matrix
 * @param inverse [in, out]
 **/
int inverse ( matrix<double>& inverse );

template<> inline
int interpolate<monotonic> ( matrix<double> nodes, vector<matrix<double> >& polynomes )
{
    using boost::numeric::ublas::matrix;
    using boost::numeric::odeint::integrate;
    typedef double T;

    // interpolation polynomes are monotone cubic splines
    // with continous first derivative
    // http://de.wikipedia.org/wiki/Kubisch_Hermitescher_Spline
    const size_t size=nodes.size1();
    assert ( size>1 && "At least two nodes needed for interpolation" );
    polynomes.resize ( size-1 );
    matrix<T> A;
    hermite_matrix(A);
    inverse ( A );
    // solve linear equation system AX=B for each node
    for ( size_t i=0; i<polynomes.size(); ++i )
    {
        // set right hand side of the linear equation system
        // one column for x and one for y coordinate
        matrix<T> B=zero_matrix<T> ( 4, 2 );
        for ( size_t j=0; j<2; ++j )
        {
            if ( i!=0 && i!=polynomes.size()-1 )
            {
                B ( 0, j ) =nodes ( i, j );
                B ( 1, j ) =nodes ( i+1, j );
                B ( 2, j ) =.5 * ( nodes ( i+1, j ) - nodes ( i-1, j ) ); //mk
                B ( 3, j ) =.5 * ( nodes ( i+2, j ) - nodes ( i, j ) ); //mk+1
            }
            else
            {
                B ( 0, j ) =nodes ( i, j );
                B ( 1, j ) =nodes ( i+1, j );
                if ( i )
                {
                    B ( 2, j ) =0.5* ( nodes ( i+1, j ) - nodes ( i-1, j ) );
                    B ( 3, j ) =1.0* ( nodes ( i+1, j ) - nodes ( i, j ) );
                }
                else
                {
                    B ( 2, j ) =1.0* ( nodes ( i+1, j ) - nodes ( i, j ) );
                    B ( 3, j ) =.5 * ( nodes ( i+2, j ) - nodes ( i, j ) );
                }
            }

            // preserve monotonicity
            // http://en.wikipedia.org/wiki/Monotone_cubic_interpolation
            if ( B ( 0, j ) ==B ( 1, j ) )
            {
                assert ( 0 );
                B ( 2, j ) =0;
                B ( 3, j ) =0;
            }
            else
            {
                T dk=nodes ( i+1, j ) - nodes ( i, j );
                T ak=B ( 2, j ) /dk;
                T bk=B ( 3, j ) /dk;
                T r=pow ( ak,2 ) +pow ( bk,2 );
                if ( r>=9 )
                {
                    B ( 2, j ) =3*ak*dk/sqrt ( r );
                    B ( 3, j ) =3*bk*dk/sqrt ( r );
                }
            }
        }
        polynomes[i]=trans ( prod ( A,B ) );
    }
}

template<> inline
int interpolate<smooth> ( matrix<double> nodes, vector<matrix<double> >& polynomes )
{
    const size_t size=nodes.size1();
    assert ( size>1 && "At least two nodes needed for interpolation" );
    /**
      interpolation polynomes are cubic splines
      with continous second derivative
      the coefficients are calculated by solving
      the linear equation system A*X=B
      @see http://mathworld.wolfram.com/CubicSpline.html
     */

    // create a Tridiagonal-Toeplitz-Matrix
    matrix<double> A(size,size);
    toeplitz_matrix( A );

    // set right hand side of the linear equation system
    // one column for x and one for y coordinate
    matrix<double> B ( zero_matrix<double> ( size, 2 ) );
    {
        B ( 0, 0 ) =nodes ( 1, 0 ) - nodes ( 0, 0 );
        B ( 0, 1 ) =nodes ( 1, 1 ) - nodes ( 0, 1 );
        for ( size_t i=1; i < size-1; ++i )
        {
            B ( i, 0 ) =nodes ( i+1, 0 ) - nodes ( i-1, 0 );
            B ( i, 1 ) =nodes ( i+1, 1 ) - nodes ( i-1, 1 );
        }
        B ( size-1, 0 ) =nodes ( size-1, 0 ) - nodes ( size-2, 0 );
        B ( size-1, 1 ) =nodes ( size-1, 1 ) - nodes ( size-2, 1 );
        B*=3;
    }

    // invert the matrix
    inverse ( A );

    // multiply right hand side with inverse matrix
    const matrix<double> X ( prod ( A, B ) );

    // calculate coefficients of cubic polynomes
    polynomes.resize ( size-1 );
    for ( size_t i=0; i<polynomes.size(); ++i )
    {
        polynomes[i]=zero_matrix<double> ( B.size2(), 4 );
        for ( size_t j = 0; j < polynomes[i].size1(); ++j )
        {
            polynomes[i] ( j, 0 ) =nodes ( i, j );
            polynomes[i] ( j, 1 ) =X ( i, j );
            polynomes[i] ( j, 2 ) =3 * ( nodes ( i+1, j ) - nodes ( i, j ) ) - 2*X ( i, j ) - X ( i+1, j );
            polynomes[i] ( j, 3 ) =2 * ( nodes ( i, j ) - nodes ( i+1, j ) ) +   X ( i, j ) + X ( i+1, j );
        }
    }
}
}

#else
#error POLYNOME_TYPE
#endif // POLYNOME_TYPE == 0
#endif //__INTERPOLATION_H__

#include "Interpolation.h"
#include <boost/numeric/ublas/lu.hpp>

using namespace boost::numeric::ublas;

namespace {

/**
 * Calculate the inverse matrix
 * @param inverse [in, out]
 **/
int inverse ( matrix<double>& inverse )
{
    typedef double T;

    typedef permutation_matrix<std::size_t> pmatrix;
    matrix<T> A ( inverse );

    // create a permutation matrix for the LU-factorization
    pmatrix pm ( A.size1() );

    // perform LU-factorization
    int res = lu_factorize ( A, pm );
    assert ( res == 0 );

    // create identity matrix of "inverse"
    inverse.assign ( identity_matrix<T> ( A.size1() ) );

    // backsubstitute to get the inverse
    lu_substitute ( A, pm, inverse );

    //cout << inverse << endl;
    return 0;
}

void hermite_matrix ( matrix<double>& M )
{
    M=zero_matrix<double> ( 4, 4 );
    M ( 0,0 ) =1;
    M ( 1,0 ) =1;
    M ( 1,1 ) =1;
    M ( 1,2 ) =1;
    M ( 1,3 ) =1;
    M ( 2,1 ) =1;
    M ( 3,1 ) =1;
    M ( 3,2 ) =2;
    M ( 3,3 ) =3;
}

void toeplitz_matrix ( matrix<double>& M )
{
    for ( signed i = 0; i < M.size1(); ++ i )
        for ( signed j = std::max ( i - 1, 0 ); j < std::min ( i + 2, signed ( M.size2 () ) ); ++ j )
            M ( i, j ) = ( i==j ) ? ( ( i==0 || i==M.size1()-1 ) ? 2 : 4 ) : 1;
}
}