#include "stdafx.h"
#include "AkimaSpline.h"

#include <cmath>

AkimaSpline::AkimaSpline()
           : m_c( NULL )
{
}

AkimaSpline::~AkimaSpline()
{
  delete m_c;
}

void AkimaSpline::fit( const std::vector< Point >& curve )
{
  if ( m_c )
  {
    delete[] m_c;
    m_c = NULL;
  }

  int n = (int)curve.size();

  if ( n >= 5 )
  {
	  int i;
	  double* x = new double[ n ];
	  double* y = new double[ n ];
	  double* w = new double[ n ];
	  double* diff = new double[ n ];
	  double* d = new double[ n ];
  
	  if ( !x || !y || !w || !diff || !d )
	  {  
		if ( x )
			delete[] x;
		if ( y )
			delete[] y;
		if ( w )
			delete[] w;
		if ( diff )
			delete[] diff;
		if ( d )
			delete[] d;
      
		return;
	 }
  

	for ( i = 0; i < n; i++ )
	{
		x[ i ] = (double)curve[ i ].x;
		y[ i ] = (double)curve[ i ].y;
	}
  
	for ( i = 0; i < n - 1; i++ )
	{
      double delta = x[ i + 1 ] - x[ i ];
      
      if ( std::fabs( delta ) < 1e-5 )
      {
      
        delta = 0.1;
      
      }

      diff[ i ] = ( y[ i + 1 ] - y[ i ] ) / delta;
	}
  
	for ( i = 0; i < n - 1; i++ )
	{
  
		w[ i ] = std::fabs( diff[ i ] - diff[ i - 1 ] );
  
	}
  
	for ( i = 2; i < n - 2; i++ )
	{
  
		if ( ( std::fabs( w[ i - 1 ] ) + std::fabs( w[ i + 1 ] ) ) > 1e-5 )
		{
    
			d[ i ] = ( w[ i + 1 ] * diff[ i - 1 ] + w[ i - 1 ] * diff[ i ] ) /
				     ( w[ i + 1 ] + w[ i - 1 ] );
    
		}
		else
		{
    
			d[ i ] = ( ( x[ i + 1 ] - x[ i ] ) * diff[ i - 1 ] + 
					   ( x[ i ] - x[ i - 1 ] ) * diff[ i ] ) / 
					   ( x[ i + 1 ] - x[ i - 1 ] );

		}
  
	}
  
	d[ 0 ] = diff3points( x[ 0 ], x[ 0 ], y[ 0 ], 
						  x[ 1 ], y[ 1 ], 
                          x[ 2 ], y[ 2 ] ); 
	d[ 1 ] = diff3points( x[ 1 ], x[ 0 ], y[ 0 ], 
						  x[ 1 ], y[ 1 ], 
                          x[ 2 ], y[ 2 ] ); 
	d[ n - 2 ] = diff3points( x[ n - 2 ], x[ n - 3 ], y[ n - 3 ], 
                              x[ n - 2 ], y[ n - 2 ], 
							  x[ n - 1 ], y[ n - 1 ] ); 
	d[ n - 1 ] = diff3points( x[ n - 1 ], x[ n - 3 ], y[ n - 3 ], 
                              x[ n - 2 ], y[ n - 2 ], 
							  x[ n - 1 ], y[ n - 1 ] ); 

	hermiteSpline( x, y, d, n );
  
	delete[] d;
	delete[] diff;
	delete[] w;
	delete[] y;
	delete[] x;
  }
}


std::vector< Point > AkimaSpline::getValues( int steps )
{
  std::vector< Point > interp;

  if ( m_c )
  {
    int n = (int)m_c[ 2 ];
    double x = m_c[ 3 ];
    double r = m_c[ 3 + n - 1 ];
    std::vector< Point > pList;
    Point oldP;
    
    n *= steps;
    
    double s = ( r - x ) / (double)n;
    
    oldP.x = (int)( x + 0.5 );
    oldP.y = (int)( interpolate( x ) + 0.5 );
    interp.push_back( oldP );
    
    while ( n-- )
    {
      Point p;
      p.x = (int)( x + 0.5 );
      p.y = (int)( interpolate( x ) + 0.5 );
      x += s;
      
      if ( ( p.x != oldP.x ) || ( p.y != oldP.y ) )
      {
        interp.push_back( p );
		oldP = p;
      }
    }
  }
  
  return interp;
}

double AkimaSpline::diff3points( double t, double x0, double y0, 
                                 double x1, double y1, double x2, double y2 )
{
  double a = 0;
  double b = 0;
  
  t -= x0;
  x1 -= x0;
  x2 -= x0;
  a = ( y2 - y0 - x2 / x1 * ( y1 - y0 ) ) / ( x2 * x2 - x1 * x2 );
  b = ( y1 - y0 - a * x1 * x1 ) / x1;
  
  return 2.0 * a * t + b;
}


void AkimaSpline::hermiteSpline( double* x, double* y, double* d, int n )
{
  if ( !x || !y || !d )
  {
  
    return;
  
  }

  int i = 0;
  int size = 3 + n + 4 * ( n - 1 );
  double delta = 0.0;
  double delta2 = 0.0;
  double delta3 = 0.0;
  
  m_c = new double[ size ];
  
  if ( !m_c )
  {
  
    return;
  
  }
  
  m_c[ 0 ] = size;
  m_c[ 1 ] = 3;
  m_c[ 2 ] = n;
  
  for ( i = 0; i < n; i++ )
  {
  
    m_c[ 3 + i ] = x[ i ];
  
  }
  
  for ( i = 0; i < n - 1; i++ )
  {
  
    delta = x[ i + 1 ] - x[ i ]; 
    delta2 = delta * delta; 
    delta3 = delta * delta2; 
    m_c[ 3 + n + 4 * i + 0] = y[ i ]; 
    m_c[ 3 + n + 4 * i + 1] = d[ i ]; 
    m_c[ 3 + n + 4 * i + 2] = ( 3 * ( y[ i + 1 ] - y[ i ] ) - 
                                2 * d[ i ] * delta -
                                d[ i + 1 ] * delta ) / delta2; 
    m_c[ 3 + n + 4 * i + 3] = ( 2 * ( y[ i ] - y[ i + 1 ] ) + 
                                d[ i ] * delta +
                                d[ i + 1 ] * delta ) / delta3; 
  
  
  }

}


double AkimaSpline::interpolate( double t )
{

  int n = (int)m_c[ 2 ];
  int l = 3;
  int r = 3 + n - 1;
  int m = 0;
  
  while ( l != ( r - 1 ) )
  {
  
    m = ( l + r ) / 2;
    
    if ( m_c[ m ] >= t )
    {
    
      r = m;
    
    }
    else
    {
    
      l = m;
      
    }
  
  }
  
  t -= m_c[ l ];
  m = 3 + n + 4 * ( l - 3 );
  
  return m_c[ m ] + t * ( m_c[ m + 1 ] + t * ( m_c[ m + 2 ] + 
                                               t * m_c[ m + 3 ] ) );

}