/*=========================================================================

 Program:   wxMaracas
 Module:    $RCSfile: curve.cxx,v $
 Language:  C++
 Date:      $Date: 2008/10/31 16:32:54 $
 Version:   $Revision: 1.1 $
 
  Copyright: (c) 2002, 2003
  License:
  
   This software is distributed WITHOUT ANY WARRANTY; without even 
   the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR 
   PURPOSE.  See the above copyright notice for more information.
   
=========================================================================*/

// PS -> #include "gslobj.hxx" //PS
#include "marMatrix.h"
#include <math.h> //PS
#include "marMathConst.h" //PS

#include "curve.hxx"


int kCurve::MAX_DIMENSION = 0;
int kCurve::MAX_STATE_DIMENSION = 0;

#define KCURVE_CHANGE_DIMENSIONS( ) \
    kCurve::MAX_DIMENSION = ( kCurve::MAX_DIMENSION > _dimension )?\
	kCurve::MAX_DIMENSION: _dimension;\
    kCurve::MAX_STATE_DIMENSION = ( kCurve::MAX_STATE_DIMENSION > _stateDim )?\
kCurve::MAX_STATE_DIMENSION: _stateDim;

// Catmull-Rom basic functions and their derivatives.

// -------------------------------------------------------------------------
void f_catmull( double* N, double t )
{
    double t2 = t * t;
    double t3 = t2 * t;
	
    N[ 0 ] = -t3 + ( 2 * t2 ) - t;
    N[ 1 ] = ( 3 * t3 ) - ( 5 * t2 ) + 2;
    N[ 2 ] = -( 3 * t3 ) + ( 4 * t2 ) + t;
    N[ 3 ] = t3 - t2;
}

// -------------------------------------------------------------------------
void f_d1_catmull( double* N, double t )
{
    double t2 = t * t;
	
    N[ 0 ] = -( 3 * t2 ) + ( 4 * t ) - 1;
    N[ 1 ] = ( 9 * t2 ) - ( 10 * t );
    N[ 2 ] = -( 9 * t2 ) + ( 8 * t ) + 1;
    N[ 3 ] = ( 3 * t2 ) - ( 2 * t );
}

// -------------------------------------------------------------------------
void f_d2_catmull( double* N, double t )
{
    N[ 0 ] = -( 6 * t ) + 4;
    N[ 1 ] = ( 18 * t ) - 10;
    N[ 2 ] = -( 18 * t ) + 8;
    N[ 3 ] = ( 6 * t ) - 2;
}

// -------------------------------------------------------------------------
kCurve::kCurve( size_t dim, size_t sdim )
: _dimension( dim ), _stateDim( sdim )
{
    _mC = new double[ _dimension * 4 ];
    _mS = new double[ _stateDim * 4 ];
    KCURVE_CHANGE_DIMENSIONS( );
}

// -------------------------------------------------------------------------
kCurve::~kCurve( )
{
    reset( );
    delete[] _mC;
    delete[] _mS;
}

// -------------------------------------------------------------------------
kCurve& kCurve::operator=( const kCurve& r )
{
    reset( );
    delete[] _mC;
    delete[] _mS;
	
    _dimension = r._dimension;
    _stateDim = r._stateDim;
	
    _mC = new double[ _dimension * 4 ];
    _mS = new double[ _stateDim * 4 ];
	
    for( int i = 0; i < r._controlPoints.size( ); i++ )
		addControlPoint( r._controlPoints[ i ],
		r._controlState[ i ] );
    return( *this );
}

// -------------------------------------------------------------------------
uint kCurve::getClosestControlPoint( double* p )
{
    int res;
// PS ->     gslobj_vector vp( p, _dimension ), cp( _dimension ); //PS
	marVector vp (p,_dimension),cp(_dimension);
    double dist, min;
	
    cp = _controlPoints[ 0 ];
    min = ( cp - vp ).norm2( );
    res = 0;
	
    for( int i = 1; i < _controlPoints.size( ); i++ )
    {
		cp = _controlPoints[ i ];
		dist = ( cp - vp ).norm2( );
		if( min > dist )
		{
			min = dist;
			res = i;
		} // fi
    } // rof
	
    return( res );
}

// -------------------------------------------------------------------------
void kCurve::getPoint( double* p, double s )
{
    double t;
    int i;
	
    calculeSplineArguments( s, i, t );
    evaluate( p, i, t );
}

// -------------------------------------------------------------------------
void kCurve::getState( double* st, double s )
{
    double t;
    int i;
	
    calculeSplineArguments( s, i, t );
    evalState( st, i, t );
}

// -------------------------------------------------------------------------
void kCurve::getTangent( double* tg, double s )
{
    double t;
    int i;
	
    calculeSplineArguments( s, i, t );
    derivative1( tg, i, t );
	
// PS ->     gslobj_vector tmp( tg, _dimension ); //PS
	marVector tmp( tg, _dimension );
    memcpy( tg,
		( double* )( tmp.normalize( ) ),
		_dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::getNormal( double* n, double s )
{
    double t;
    int i;
	
    calculeSplineArguments( s, i, t );
    derivative2( n, i, t );
	
// PS ->     gslobj_vector tmp( n, _dimension ); //PS
	marVector tmp( n, _dimension );
    memcpy( n,
		( double* )( tmp.normalize( ) ),
		_dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::getBinormal( double* b, double s )
{
// PS ->     //gslobj_vector tg( _dimension ), n( _dimension ); //PS
	marVector tg( _dimension ), n( _dimension );
    double t;
    int i;
	
    calculeSplineArguments( s, i, t );
    derivative1( ( double* )tg, i, t );
    derivative2( ( double* )n, i, t );
	
    memcpy( b,
		( double* )( tg.cross( n ).normalize( ) ),
		_dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::addControlPoint( double* cp, double* sp )
{
    double* tmp = new double[ _dimension ];
    memcpy( tmp, cp, _dimension * sizeof( double ) );
    _controlPoints.push_back( tmp );
    if( sp != NULL && _stateDim > 0 ) {
		
		double *tmp1 = new double[ _stateDim ];
		memcpy( tmp1, sp, _stateDim * sizeof( double ) );
		_controlState.push_back( tmp1 );
		
    } else
		_controlState.push_back( NULL );
	
    if( _controlPoints.size( ) == 1 ) {
		
		_controlT.push_back( 0.0 );
		_controlL.push_back( 0.0 );
		
    } else {
		
		// Here we maintain some extra info about the control points.
		// These data will allow the high level programmer to think
		// (if so ;-) just in terms of an arclength parameter "s".
// PS -> 		//gslobj_vector v1( _dimension ), v0( _dimension ); //PS
		marVector v1( _dimension ), v0( _dimension );
		double len;
		int p;
		
		p = _controlPoints.size( ) - 1;
		v1 = _controlPoints[ p ];
		v0 = _controlPoints[ p - 1 ];
		len = ( v1 - v0 ).norm2( ) + _controlL[ p - 1 ];
		_controlL.push_back( len );
		_controlT.push_back( 0.0 );
		for( int i = 0; i < _controlT.size( ); i++ )
			_controlT[ i ] = _controlL[ i ] / len;
		
    }// fi
}

// -------------------------------------------------------------------------
void kCurve::getControlPoint( int i, double* cp, double* sp )
{
    memcpy( cp, _controlPoints[ i ], _dimension * sizeof( double ) );
    if( sp != NULL && _controlState[ i ] != NULL && _stateDim > 0 )
		memcpy( sp, _controlState[ i ], _stateDim * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::setControlPoint( int i, double* cp, double* sp )
{
    memcpy( _controlPoints[ i ], cp, _dimension * sizeof( double ) );
    if( sp != NULL && _stateDim > 0 )
		memcpy( _controlState[ i ], sp, _stateDim * sizeof( double ) );
	
    if( _controlPoints.size( ) > 1 ) {
// PS -> 		//gslobj_vector v1( _dimension ), v0( _dimension ); //PS
		marVector v1( _dimension ), v0( _dimension );
		double len;
		int it;
		
		for( it = i; it < _controlT.size( ); it++ ) {
			
			v1 = _controlPoints[ it ];
			v0 = _controlPoints[ it - 1 ];
			len = ( v1 - v0 ).norm2( ) + _controlL[ it - 1 ];
			_controlL[ i ] = len;
			
		} // rof
		
		for( it = 0; it < _controlT.size( ); it++ )
			_controlT[ it ] = _controlL[ it ] / len;
		
    }// fi
}

// -------------------------------------------------------------------------
double kCurve::length( double step )
{
// PS ->     //gslobj_vector nV( 4 ), q( _dimension ), p( _dimension ); //PS
	marVector nV( 4 ), q( _dimension ), p( _dimension );
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
	marMatrix mC( _mC, _dimension, 4 );
    double l = 0;
	
    loadCatmullControlMatrix( 0 );
    f_catmull( ( double* )nV, 0 );
    p = ( mC * nV ) * 0.5;
	
    for( int i = 0; i < _controlPoints.size( ); i++ ) {
		
		loadCatmullControlMatrix( i );
		for( double t = 0.0; t <= 1.0; t += step ) {
			
			f_catmull( ( double* )nV, t );
			q = ( mC * nV ) * 0.5;
			l += ( q - p ).norm2( );
			p = q;
			
		} // rof
		
    } // rof
    return( l );
}

// -------------------------------------------------------------------------
double kCurve::projectOverControlPoints( double* pr, double* pt )
{
// PS ->     //gslobj_vector xpc( 3 ), xpo( pt, 3 ); //PS
	marVector xpc( 3 ), xpo( pt, 3 );
// PS ->     //gslobj_vector xpa( 3 ), xpn( 3 ); //PS
	marVector xpa( 3 ), xpn( 3 ); 
// PS ->     //gslobj_vector xpr( pr, 3 ); //PS
	marVector xpr( pr, 3 ); 
    double sina, sinn, cosa, cosn, tha, thn;
    double d, e, t, l, tca, tcn, lca, lcn;
    uint icp = getClosestControlPoint( pt );
	
    getControlPoint( icp, ( double* )xpc, NULL );
	
    if( icp == 0 ) {
		
		getControlPoint( icp + 1, ( double* )xpn, NULL );
		
		sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
			( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
		cosn = ( xpo - xpc ).dot( xpn - xpc ) /
			( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );

// PS -> 		//thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 ); //PS
		thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
// PS -> 		//if( 0 <= thn && thn <= M_PI_2 ) { //PS
		if( 0 <= thn && thn <= (PI/2) ) {
			
			tca = _controlT[ icp ];
			lca = _controlL[ icp ];
			tcn = _controlT[ icp + 1 ];
			lcn = _controlL[ icp + 1 ];
			xpa = xpc;
			
			d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
				( xpn - xpa ).norm2( );
			e = ( xpo - xpa ).norm2( );
			e = ( e * e ) - ( d * d );
			e = sqrt( e );
			xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
			l = ( xpr - xpa ).norm2( ) + lca;
			t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
			
		} else {
			
			xpr = xpc;
			t = 0;
			
		} // fi
		
    } else if( icp == _controlPoints.size( ) - 1 ) {
		
		getControlPoint( icp - 1, ( double* )xpa, NULL );
		
		sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
			( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
		cosa = ( xpa - xpc ).dot( xpo - xpc ) /
			( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
		
// PS -> 		//tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 ); //PS
		tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
		
// PS -> 		//if( 0 <= tha && tha <= M_PI_2 ) { //PS
		if( 0 <= tha && tha <= (PI/2) ) {
			
			tca = _controlT[ icp - 1 ];
			lca = _controlL[ icp - 1 ];
			tcn = _controlT[ icp ];
			lcn = _controlL[ icp ];
			xpn = xpc;
			
			d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
				( xpn - xpa ).norm2( );
			e = ( xpo - xpa ).norm2( );
			e = ( e * e ) - ( d * d );
			e = sqrt( e );
			xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
			l = ( xpr - xpa ).norm2( ) + lca;
			t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
			
		} else {
			
			xpr = xpc;
			t = _controlT[ _controlPoints.size( ) - 1 ];
			
		} // fi
		
    } else {
		
		getControlPoint( icp - 1, ( double* )xpa, NULL );
		getControlPoint( icp + 1, ( double* )xpn, NULL );
		
		sina = ( ( xpa - xpc ).cross( xpo - xpc ) ).norm2( ) /
			( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
		sinn = ( ( xpo - xpc ).cross( xpn - xpc ) ).norm2( ) /
			( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
		cosa = ( xpa - xpc ).dot( xpo - xpc ) /
			( ( xpa - xpc ).norm2( ) * ( xpo - xpc ).norm2( ) );
		cosn = ( xpo - xpc ).dot( xpn - xpc ) /
			( ( xpo - xpc ).norm2( ) * ( xpn - xpc ).norm2( ) );
		
// PS -> 		//tha = acos( cosa ) + ( ( sina >= 0 )? M_PI: 0 );//PS
		tha = acos( cosa ) + ( ( sina >= 0 )? PI: 0 );
// PS -> 		//thn = acos( cosn ) + ( ( sinn >= 0 )? M_PI: 0 );//PS
		thn = acos( cosn ) + ( ( sinn >= 0 )? PI: 0 );
		
		if( tha < thn ) {
			
			tca = _controlT[ icp - 1 ];
			lca = _controlL[ icp - 1 ];
			tcn = _controlT[ icp ];
			lcn = _controlL[ icp ];
			xpn = xpc;
			
		} else {
			
			tca = _controlT[ icp ];
			lca = _controlL[ icp ];
			tcn = _controlT[ icp + 1 ];
			lcn = _controlL[ icp + 1 ];
			xpa = xpc;
			
		} // fi
		
		d = ( ( xpn - xpa ).cross( xpa - xpo ) ).norm2( ) /
			( xpn - xpa ).norm2( );
		e = ( xpo - xpa ).norm2( );
		e = ( e * e ) - ( d * d );
		e = sqrt( e );
		xpr = ( ( xpn - xpa ).normalize( ) * e ) + xpa;
		l = ( xpr - xpa ).norm2( ) + lca;
		t = ( ( ( tcn - tca ) / ( lcn - lca ) ) * ( l - lca ) ) + tca;
		
    } // fi
	
    return( t );
}

// -------------------------------------------------------------------------
double kCurve::projectOverCurve( double* pr, double* pt )
{ // TODO
    return( 0 );
}

// -------------------------------------------------------------------------
void kCurve::evaluate( double* p, int i, double t )
{
// PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
	marVector nV( 4 ), q( _dimension ); 
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
	marMatrix mC( _mC, _dimension, 4 ); 
	
    loadCatmullControlMatrix( i );
    f_catmull( ( double* )nV, t );
    q = ( mC * nV ) * 0.5;
    memcpy( p, ( double* )q, _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::evalState( double* s, int i, double t )
{
// PS ->     //gslobj_vector nV( 4 ), q( _stateDim ); //PS
	marVector nV( 4 ), q( _stateDim ); 
// PS ->     //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
	marMatrix mS( _mS, _stateDim, 4 ); 
	
    loadCatmullStateMatrix( i );
    f_catmull( ( double* )nV, t );
    q = ( mS * nV ) * 0.5;
    memcpy( s, ( double* )q, _stateDim * sizeof( double ) );
}

// ---------------------------------------------------------------------------
void kCurve::derivative1( double* d, int i, double t )
{
// PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
	marVector nV( 4 ), q( _dimension ); 
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
	marMatrix mC( _mC, _dimension, 4 );
	
    loadCatmullControlMatrix( i );
    f_d1_catmull( ( double* )nV, t );
    q = ( mC * nV ) * 0.5;
    memcpy( d, ( double* )q, _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::derivative2( double* d, int i, double t )
{
// PS ->     //gslobj_vector nV( 4 ), q( _dimension ); //PS
	marVector nV( 4 ), q( _dimension ); 
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
	marMatrix mC( _mC, _dimension, 4 ); 
	
    loadCatmullControlMatrix( i );
    f_d2_catmull( ( double* )nV, t );
    q = ( mC * nV ) * 0.5;
    memcpy( d, ( double* )q, _dimension * sizeof( double ) );
}

// -------------------------------------------------------------------------
void kCurve::reset( )
{
    int i;
	
    for( i = 0; i < _controlPoints.size( ); i++ )
		delete[] _controlPoints[ i ];
    _controlPoints.clear( );
	
    for( i = 0; i < _controlState.size( ); i++ )
		if( _controlState[ i ] != NULL ) delete[] _controlState[ i ];
		_controlState.clear( );
		
		_controlT.clear( );
		_controlL.clear( );
}

// -------------------------------------------------------------------------
void kCurve::loadCatmullControlMatrix( int i )
{
// PS ->     //gslobj_matrix mC( _mC, _dimension, 4 ); //PS
	marMatrix mC( _mC, _dimension, 4 ); 
    int l;
	
    for( int j = i - 1; j <= i + 2; j++ ) {
		
		for( int k = 0; k < _dimension; k++ ) {
			
			l = ( j >= 0 )? j: 0;
			l = ( l >= _controlPoints.size( ) )?
				_controlPoints.size( ) - 1: l;
			mC( k, j - i + 1 ) = _controlPoints[ l ][ k ];
			
		} // rof
		
    } // rof
}

// -------------------------------------------------------------------------
void kCurve::loadCatmullStateMatrix( int i )
{
// PS ->     //gslobj_matrix mS( _mS, _stateDim, 4 ); //PS
	marMatrix mS( _mS, _stateDim, 4 ); 
    int l;
	
    for( int j = i - 1; j <= i + 2; j++ ) {
		
		for( int k = 0; k < _stateDim; k++ ) {
			
			l = ( j >= 0 )? j: 0;
			l = ( l >= _controlState.size( ) )?
				_controlState.size( ) - 1: l;
			mS( k, j - i + 1 ) = _controlState[ l ][ k ];
			
		} // rof
		
    } // rof
}

// -------------------------------------------------------------------------
void kCurve::calculeSplineArguments( double s, int& i, double& t )
{
    for( i = 0; i < _controlT.size( ) && _controlT[ i ] <= s; i++ );
	
    if( s < 1.0 ) i--;
	
    t = ( ( _controlL[ _controlL.size( ) - 1 ] * s ) - _controlL[ i ] ) /
		( _controlL[ i + 1 ] - _controlL[ i ] );
    t = ( s < 1.0 )? t: 1.0;
    i = ( s < 1.0 )? i: _controlT.size( ) - 1;
}

// eof - curve.cxx