/**
 * @file
 * $Id$
 * $Revision$
 * $Author$
 * $Date$
 *
 * This file is part of The iWear Framework.
 *
 * The iWear Framework 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 as in version 2 of the License.

 * 
 * The iWear Framework 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
 * The iWear Framework; if not, write to the Free Software Foundation, Inc., 59
 * Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#ifndef __IWEAR_CSPLINE_H
#define __IWEAR_CSPLINE_H

#ifndef __IWEAR_MMATRIX_H
#include <iwear/mmatrix.h>
#endif

#ifndef __IWEAR_POINT_H
#include <iwear/point.h>
#endif

#ifndef __IWEAR_TUPLE_H
#include <iwear/tuple.h>
#endif

#include <vector>

using std::vector;

#include "iwear/cstdint.h"

namespace iwear
{
    namespace math
    {

template<class T>
class Spline2D
{
private:
protected:
    vector<Point2D<T> > data;
public:
    virtual ~Spline2D() { };
    virtual void put( const vector<Point2D<T> >& v );
    virtual void put( const Point2D<T>& );
    virtual void Reset( void );
    virtual void Calc( void ) = 0;
    virtual T get( T t ) const = 0;
    virtual Point2D<T> get_data( uint32_t dt ) const 
		{ if ( dt < data.size() ) return data[dt]; else return data[0]; }
};

template<class T>
inline void Spline2D<T>::put( const vector<Point2D<T> >& v )
{
    data = v;
}

template<class T>
inline void Spline2D<T>::put( const Point2D<T>& dat )
{
    data.push_back(dat);
}

template<class T>
inline void Spline2D<T>::Reset( void )
{
    data.clear();
}

template<class T>
class CSpline2D : public Spline2D<T>
{
private:
protected:
    vector<T> af;
    vector<T> bf;
    vector<T> cf;
    vector<T> df;

    mutable T left;
    mutable T right;
    map<T,int> invmap;
public:
    virtual void Calc( void );
    virtual T get( T t ) const;
    T get( T t, int j ) const;
};

template<class T>
inline T CSpline2D<T>::get( T t, int j ) const
{
    return ((af[j]*t +bf[j])*t +cf[j])*t +df[j];
}

template<class T>
inline T CSpline2D<T>::get( T t ) const
{
    // Search for the spline interval
    typename map<T,int>::const_iterator it = invmap.lower_bound(t);
    int acts = this->data.size() - 1;
    if ( it != invmap.end() )
    {
	acts = it->second;
	if ( acts ) acts--;
    }

    t -= this->data[acts].x;

    return get(t,acts);
}

template<class T>
inline void CSpline2D<T>::Calc( void )
{
    af.clear();
    bf.clear();
    cf.clear();
    df.clear();

    int32_t n = this->data.size() - 1;

    af.resize(n+1);
    bf.resize(n+1);
    cf.resize(n+1);
    df.resize(n+1);

    T h[n]; // Helper array with x point differences

#define X(ind) this->data[ind].x
#define Y(ind) this->data[ind].y

    int i;
    for ( i = 0; i < n; i++ )
    {
	h[i] = X(i+1) - X(i);
    }

    MathMatrix<T> m(n-1,n-1);
    MathVector<T> r(n-1);

//cout << "N = " << n << endl;    

//    m[0][i] = 1;
//    r[i] = 0;

    int j;
    for( j = 0; j < n-1; j++ )
    {
	// j is index in matrix, while i is index in helper arrays...
	// so i goes from 1 to (including) n-1
	i = j+1;
	if ( i == 1 ) // Upper left corner of the matrix
	{
	    m[j][j] = 2*(h[i-1] + h[i]);
	    m[j+1][j] = h[i];
	}
	else if (i < n-1)
	{
	    m[j-1][j] = h[i-1];
	    m[j][j] = 2*(h[i-1] + h[i]);
	    m[j+1][j] = h[i];
	}
	else
	{
	    m[j-1][j] = h[i-1];
	    m[j][j] = 2*(h[i-1] + h[i]);
	}
	r[j] = 3.0 * (
		  (Y(i+1) - Y(i+0)) / h[i] 
		- (Y(i+0) - Y(i+-1)) / h[i-1]);
    }

//    r[i] = 0;
//    m[i][i] = 1;
//	    m[i-1][i] = 1;
//    m[i][i] = 2 * h[i];
//    m[i-1][i] = h[i];

  //  cout << m << endl;
//    cout << r << endl;
    MathVector<T> s = m.solve(r);

//    cout << "Solved : " << endl;
  //  cout << s << endl;
  //  mout << r << endl;
  //  mout << m << endl;
  //  mout << s << endl;



    for ( i = 1; i < n; i++ )
    {
	bf[i] = s[i - 1]; // Transfer the solved vector into the b parameters of the equation
	df[i] = Y(i); // The translation is the y at the point
    }

    af[0] = bf[1] / (3 * h[0]);
    bf[0] = 0;
    cf[0] = (Y(1)-Y(0)) / h[0] - bf[1] * h[0] / 3.0;
    df[0] = Y(0);
    bf[n] = 0;
    
    for ( i = 1; i <= n-1; i++ )
    {
	af[i] = (bf[i+1] - bf[i])/(3.0*h[i]);
	cf[i] = (bf[i] + bf[i-1]) * h[i-1] + cf[i-1];
    }

    invmap.clear();
    for ( i = 0; i < n; i++ )
    {
	/*
	if ( i == 0 )
	{
	    T lb = - numeric_limits<T>::max();
	    invmap[lb] = i;
	    cout << "Setting bound " << lb << " on " << i << endl;
	}
	else if ( i == (n-1) )
	{
	    T lb = numeric_limits<T>::max();
	    invmap[lb] = i;
	}
	else */
	{
	    invmap[X(i)] = i;
	}
	/*
	cout << "a = " << af[i] << endl;
	cout << "b = " << bf[i] << endl;
	cout << "c = " << cf[i] << endl;
	cout << "d = " << df[i] << endl;
	*/
    }
    invmap[X(n)] = n;
    /*
*/
}
}
}
#endif